public void setPivot(ReferenceFrame pivot) {
Matrix44 m = getTransformation(pivot);
Matrix44 mInverse = Matrix44.invert(m);
transform(mInverse, this);
for (int i = 0; i < getChildCount(); i++) {
Component child = getChildAt(i);
if (child instanceof Transformable) {
((Transformable) child).transform(m, this);
} else if (child instanceof Visual) {
((Visual) child).transform(m);
}
}
}
static {
s_calculatePointAtHelperOffset.setName("s_calculatePointAtHelperOffset");
s_calculatePointAtHelperA.setName("s_calculatePointAtHelperA");
s_calculatePointAtHelperB.setName("s_calculatePointAtHelperB");
s_calculatePointAtHelperOffset.setIsHelper(true);
s_calculatePointAtHelperA.setIsHelper(true);
s_calculatePointAtHelperB.setIsHelper(true);
}
public Matrix33 calculatePointAt(
ReferenceFrame target,
javax.vecmath.Vector3d offset,
javax.vecmath.Vector3d upGuide,
ReferenceFrame asSeenBy,
boolean onlyAffectYaw) {
synchronized (s_calculatePointAtHelperOffset) {
if (upGuide == null) {
upGuide = MathUtilities.getYAxis();
}
if (asSeenBy == null) {
asSeenBy = (ReferenceFrame) getParent();
}
Matrix44 transform = getTransformation(asSeenBy);
Vector3 position = transform.getPosition();
// Vector3 position = new Vector3( transform.m30, transform.m31,
// transform.m32 );
ReferenceFrame actualTarget;
if (offset == null) {
actualTarget = target;
} else {
s_calculatePointAtHelperOffset.setParent(target);
Matrix44 m = new Matrix44();
m.m30 = offset.x;
m.m31 = offset.y;
m.m32 = offset.z;
s_calculatePointAtHelperOffset.setLocalTransformation(m);
actualTarget = s_calculatePointAtHelperOffset;
}
Matrix33 result;
if (onlyAffectYaw) {
// setup "helperA" with the orientation of "asSeenBy" and the
// position of "this"
s_calculatePointAtHelperA.setParent(asSeenBy);
s_calculatePointAtHelperA.setLocalTransformation(new Matrix44());
s_calculatePointAtHelperA.setPosition(Vector3.ZERO, this);
// calculate the angle of rotation around y of "actualTarget" as
// seen by "helperA"
Vector3 targetPosition = actualTarget.getPosition(s_calculatePointAtHelperA);
double targetTheta = Math.atan2(targetPosition.x, targetPosition.z);
// place "helperB" out in front of "this"
s_calculatePointAtHelperB.setParent(this);
s_calculatePointAtHelperB.setPosition(MathUtilities.getZAxis(), this);
// calculate the angle of rotation around Y of "helperB" as seen
// by "helperA"
Vector3 forwardPosition = s_calculatePointAtHelperB.getPosition(s_calculatePointAtHelperA);
double forwardTheta = Math.atan2(forwardPosition.x, forwardPosition.z);
// setup "helperB" to have position and orientation of "this"
s_calculatePointAtHelperB.setLocalTransformation(new Matrix44());
// calculate how much to rotate
double deltaTheta = targetTheta - forwardTheta;
// rotate "helperB" around Y as seen by "helperA"
s_calculatePointAtHelperB.rotate(
MathUtilities.getYAxis(), deltaTheta, s_calculatePointAtHelperA);
// extract result
result = s_calculatePointAtHelperB.getAxes(asSeenBy);
// clean up
s_calculatePointAtHelperA.setParent(null);
s_calculatePointAtHelperB.setParent(null);
} else {
javax.vecmath.Vector3d targetPosition = actualTarget.getPosition(asSeenBy);
javax.vecmath.Vector3d zAxis =
MathUtilities.normalizeV(MathUtilities.subtract(targetPosition, position));
javax.vecmath.Vector3d xAxis =
MathUtilities.normalizeV(MathUtilities.crossProduct(upGuide, zAxis));
if (Double.isNaN(xAxis.lengthSquared())) {
xAxis.set(0, 0, 0);
zAxis.set(0, 0, 0);
// throw new RuntimeException(
// "cannot calculate point at: zAxis=" + zAxis + " upGuide="
// + upGuide );
}
javax.vecmath.Vector3d yAxis = MathUtilities.crossProduct(zAxis, xAxis);
result = new Matrix33(xAxis, yAxis, zAxis);
}
if (offset == null) {
s_calculatePointAtHelperOffset.setParent(null);
}
return result;
}
}