/**
* Notification that this sensor has just been clicked on to start a drag action.
*
* @param hitPoint Where the input device intersected the object sensor
* @param position Where the sensor origin is in local coordinates
*/
public void notifySensorDragStart(float[] hitPoint, float[] location) {
super.notifySensorDragStart(hitPoint, location);
dragRadius =
(float)
Math.sqrt(
hitPoint[0] * hitPoint[0] + hitPoint[1] * hitPoint[1] + hitPoint[2] * hitPoint[2]);
initialNormal[0] = hitPoint[0] / dragRadius;
initialNormal[1] = hitPoint[1] / dragRadius;
initialNormal[2] = hitPoint[2] / dragRadius;
}
/** Calculate transforms needed to handle VRML semantics formula: T x C x R x SR x S x -SR x -C */
private void updateTransform() {
// System.out.println(this);
tempVec.x = -vfCenter[0];
tempVec.y = -vfCenter[1];
tempVec.z = -vfCenter[2];
matrix.setIdentity();
matrix.setTranslation(tempVec);
float scaleVal = 1.0f;
if (floatEq(vfScale[0], vfScale[1]) && floatEq(vfScale[0], vfScale[2])) {
scaleVal = vfScale[0];
tempMtx1.set(scaleVal);
// System.out.println("S" + tempMtx1);
} else {
// non-uniform scale
// System.out.println("Non Uniform Scale");
tempAxis.x = vfScaleOrientation[0];
tempAxis.y = vfScaleOrientation[1];
tempAxis.z = vfScaleOrientation[2];
tempAxis.angle = -vfScaleOrientation[3];
double tempAxisNormalizer =
1
/ Math.sqrt(
tempAxis.x * tempAxis.x + tempAxis.y * tempAxis.y + tempAxis.z * tempAxis.z);
tempAxis.x *= tempAxisNormalizer;
tempAxis.y *= tempAxisNormalizer;
tempAxis.z *= tempAxisNormalizer;
tempMtx1.set(tempAxis);
tempMtx2.mul(tempMtx1, matrix);
// Set the scale by individually setting each element
tempMtx1.setIdentity();
tempMtx1.m00 = vfScale[0];
tempMtx1.m11 = vfScale[1];
tempMtx1.m22 = vfScale[2];
matrix.mul(tempMtx1, tempMtx2);
tempAxis.x = vfScaleOrientation[0];
tempAxis.y = vfScaleOrientation[1];
tempAxis.z = vfScaleOrientation[2];
tempAxis.angle = vfScaleOrientation[3];
tempMtx1.set(tempAxis);
}
tempMtx2.mul(tempMtx1, matrix);
// System.out.println("Sx-C" + tempMtx2);
float magSq =
vfRotation[0] * vfRotation[0]
+ vfRotation[1] * vfRotation[1]
+ vfRotation[2] * vfRotation[2];
if (magSq < ZEROEPS) {
tempAxis.x = 0;
tempAxis.y = 0;
tempAxis.z = 1;
tempAxis.angle = 0;
} else {
if ((magSq > 1.01) || (magSq < 0.99)) {
float mag = (float) (1 / Math.sqrt(magSq));
tempAxis.x = vfRotation[0] * mag;
tempAxis.y = vfRotation[1] * mag;
tempAxis.z = vfRotation[2] * mag;
} else {
tempAxis.x = vfRotation[0];
tempAxis.y = vfRotation[1];
tempAxis.z = vfRotation[2];
}
tempAxis.angle = vfRotation[3];
}
tempMtx1.set(tempAxis);
// System.out.println("R" + tempMtx1);
matrix.mul(tempMtx1, tempMtx2);
// System.out.println("RxSx-C" + matrix);
tempVec.x = vfCenter[0];
tempVec.y = vfCenter[1];
tempVec.z = vfCenter[2];
tempMtx1.setIdentity();
tempMtx1.setTranslation(tempVec);
// System.out.println("C" + tempMtx1);
tempMtx2.mul(tempMtx1, matrix);
// System.out.println("CxRxSx-C" + tempMtx2);
tempVec.x = vfTranslation[0];
tempVec.y = vfTranslation[1];
tempVec.z = vfTranslation[2];
tempMtx1.setIdentity();
tempMtx1.setTranslation(tempVec);
matrix.mul(tempMtx1, tempMtx2);
transform.set(matrix);
implTG.setTransform(transform);
}
/**
* Convenience method to generate the tracking output based on the input hit position.
*
* @param location The position of the sensor locally
* @param direction Vector showing the direction the sensor is pointing
*/
protected void processDrag(float[] location, float[] direction) {
// Intersect the ray with the geometry to work out where the object
// virtual geometry has been hit. If the geometry has not been hit
// then don't generate an event. Just leave it as the last one.
if (!intersectionUtils.raySphere(location, direction, ORIGIN, dragRadius, wkPoint)) return;
// Calculate the normalised form of the position
float dist =
(float)
Math.sqrt(wkPoint[0] * wkPoint[0] + wkPoint[1] * wkPoint[1] + wkPoint[2] * wkPoint[2]);
float x = 0;
float y = 0;
float z = 0;
if (dist != 0) {
x = wkPoint[0] / dist;
y = wkPoint[1] / dist;
z = wkPoint[2] / dist;
}
// To calculate the trackPoint, normalise the vector to turn it into a
// unit sphere, then multiply by the initial radius calculated to get the
// position on the sphere.
vfTrackPointChanged[0] = x;
vfTrackPointChanged[1] = y;
vfTrackPointChanged[2] = z;
hasChanged[FIELD_TRACKPOINT_CHANGED] = true;
fireFieldChanged(FIELD_TRACKPOINT_CHANGED);
// The rotation direction is a cross product of the initial position
// and the current position. The angle is then derived from the angle
// between the two vectors.
float x1 = initialPosition[0];
float y1 = initialPosition[1];
float z1 = initialPosition[2];
float x2 = wkPoint[0];
float y2 = wkPoint[1];
float z2 = wkPoint[2];
float cross_x = y1 * z2 - z1 * y2;
float cross_y = z1 * x2 - x1 * z2;
float cross_z = x1 * y2 - y1 * x2;
// Angle is cos(theta) = (A / |A|) . (B / |B|)
// A is treated as the inital normal. B is x,y,z calculated above.
double dot = initialNormal[0] * x + initialNormal[1] * y + initialNormal[2] * z;
float angle = (float) Math.acos(dot);
vfRotationChanged[0] = cross_x;
vfRotationChanged[1] = cross_y;
vfRotationChanged[2] = cross_z;
vfRotationChanged[3] = angle;
hasChanged[FIELD_ROTATION_CHANGED] = true;
fireFieldChanged(FIELD_ROTATION_CHANGED);
}