protected void close() {
if (points.isEmpty()) {
return;
}
if (points.size() > 1
&& ((Point2d) points.lastElement()).equals((Point2d) points.firstElement())) {
points.setSize(points.size() - 1);
normals.setSize(normals.size() - 2);
}
if (points.size() >= 3) {
double dx = ((Point2d) points.firstElement()).x - ((Point2d) points.lastElement()).x;
double dy = ((Point2d) points.firstElement()).y - ((Point2d) points.lastElement()).y;
double len = Math.sqrt(dx * dx + dy * dy);
Vector3d a = new Vector3d(dx / len, 0, dy / len);
Vector3d b = new Vector3d(0, 1, 0);
Vector3d c = MathUtilities.crossProduct(a, b);
normals.setElementAt(new Vector3f(-(float) c.x, -(float) c.z, 0), 1);
normals.setElementAt(new Vector3f(-(float) c.x, -(float) c.z, 0), (points.size() - 1) * 2);
} else {
points.clear();
normals.clear();
}
}
public Matrix33 calculateStandUp(ReferenceFrame asSeenBy) {
Matrix33 axes = getAxes(asSeenBy);
javax.vecmath.Vector3d yAxis = MathUtilities.getYAxis();
javax.vecmath.Vector3d zAxis =
MathUtilities.normalizeV(MathUtilities.crossProduct(axes.getRow(0), yAxis));
javax.vecmath.Vector3d xAxis = MathUtilities.crossProduct(yAxis, zAxis);
return new Matrix33(xAxis, yAxis, zAxis);
}
public static Matrix33 calculateOrientation(
javax.vecmath.Vector3d forward, javax.vecmath.Vector3d upGuide) {
if (upGuide == null) {
upGuide = MathUtilities.getYAxis();
}
javax.vecmath.Vector3d zAxis = MathUtilities.normalizeV(forward);
javax.vecmath.Vector3d xAxis =
MathUtilities.normalizeV(MathUtilities.crossProduct(upGuide, zAxis));
if (Double.isNaN(xAxis.lengthSquared())) {
throw new RuntimeException(
"cannot calculate orientation: forward=" + forward + " upGuide=" + upGuide);
}
javax.vecmath.Vector3d yAxis = MathUtilities.crossProduct(zAxis, xAxis);
return new Matrix33(xAxis, yAxis, zAxis);
}
protected void addPoint(Point2d point) {
points.add(point);
normals.setSize(normals.size() + 2);
if (points.size() > 1) {
double dx =
((Point2d) points.lastElement()).x - ((Point2d) points.elementAt(points.size() - 2)).x;
double dy =
((Point2d) points.lastElement()).y - ((Point2d) points.elementAt(points.size() - 2)).y;
double len = Math.sqrt(dx * dx + dy * dy);
Vector3d a = new Vector3d(dx / len, 0, dy / len);
Vector3d b = new Vector3d(0, 1, 0);
Vector3d c = MathUtilities.crossProduct(a, b);
normals.setElementAt(new Vector3f(-(float) c.x, -(float) c.z, 0), (points.size() - 2) * 2);
normals.setElementAt(
new Vector3f(-(float) c.x, -(float) c.z, 0), (points.size() - 1) * 2 + 1);
}
}
@Override
public javax.vecmath.Matrix4d getAbsoluteTransformation() {
synchronized (m_absoluteTransformationLock) {
if (m_absoluteTransformation == null) {
Container parent = getParent();
if (parent != null) {
m_absoluteTransformation =
MathUtilities.multiply(m_localTransformation, parent.getAbsoluteTransformation());
} else {
m_absoluteTransformation = new javax.vecmath.Matrix4d(m_localTransformation);
}
if (Math.abs(m_absoluteTransformation.m33 - 1.0) > 0.01) {
System.err.println(
"JAVA SCENEGRAH LOCAL: holy corrupt matrix batman " + m_absoluteTransformation);
}
}
return new javax.vecmath.Matrix4d(m_absoluteTransformation);
}
}
public void dragged(int dx, int dy, boolean isControlDown, boolean isShiftDown) {
if (pickedTransformable != null) {
boolean controlDown = isControlDown;
boolean shiftDown = isShiftDown;
double deltaFactor;
if (camera instanceof edu.cmu.cs.stage3.alice.core.camera.OrthographicCamera) {
edu.cmu.cs.stage3.alice.core.camera.OrthographicCamera orthoCamera =
(edu.cmu.cs.stage3.alice.core.camera.OrthographicCamera) camera;
double nearClipHeightInScreen =
renderTarget
.getAWTComponent()
.getHeight(); // TODO: should be viewport, but not working right now
double nearClipHeightInWorld =
orthoCamera.getSceneGraphOrthographicCamera().getPlane()[3]
- orthoCamera.getSceneGraphOrthographicCamera().getPlane()[1];
deltaFactor = nearClipHeightInWorld / nearClipHeightInScreen;
if (controlDown) {
if (shiftDown) {
helper.setTransformationRightNow(
edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), camera);
helper.setPositionRightNow(zeroVec, pickedTransformable);
pickedTransformable.rotateRightNow(
edu.cmu.cs.stage3.math.MathUtilities.getXAxis(), -dy * .01, helper);
pickedTransformable.rotateRightNow(
edu.cmu.cs.stage3.math.MathUtilities.getYAxis(), -dx * .01, pickedTransformable);
} else {
helper.setTransformationRightNow(
edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), camera);
helper.setPositionRightNow(zeroVec, pickedTransformable);
pickedTransformable.rotateRightNow(
edu.cmu.cs.stage3.math.MathUtilities.getZAxis(), -dx * .01, helper);
}
} else if (shiftDown) {
helper.setTransformationRightNow(
edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), camera);
helper.setPositionRightNow(zeroVec, pickedTransformable);
tempVec.x = 0.0;
tempVec.y = -dy * deltaFactor;
tempVec.z = 0.0;
pickedTransformable.moveRightNow(tempVec, helper);
} else {
helper.setTransformationRightNow(
edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), camera);
helper.setPositionRightNow(zeroVec, pickedTransformable);
tempVec.x = dx * deltaFactor;
tempVec.y = -dy * deltaFactor;
tempVec.z = 0.0;
pickedTransformable.moveRightNow(tempVec, helper);
}
} else {
double projectionMatrix11 =
renderTarget.getProjectionMatrix(camera.getSceneGraphCamera()).getElement(1, 1);
double nearClipDist = camera.nearClippingPlaneDistance.doubleValue();
double nearClipHeightInWorld = 2 * (nearClipDist / projectionMatrix11);
double nearClipHeightInScreen =
renderTarget
.getAWTComponent()
.getHeight(); // TODO: should be viewport, but not working right now
double pixelHeight = nearClipHeightInWorld / nearClipHeightInScreen;
// double pixelHeight = nearClipHeightInWorld/300;
double objectDist = pickedTransformable.getPosition(camera).getLength();
deltaFactor = (objectDist * pixelHeight) / nearClipDist;
if (controlDown) {
if (shiftDown) {
helper.setTransformationRightNow(
edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), camera);
helper.setPositionRightNow(zeroVec, pickedTransformable);
pickedTransformable.rotateRightNow(
edu.cmu.cs.stage3.math.MathUtilities.getXAxis(), -dy * .01, helper);
pickedTransformable.rotateRightNow(
edu.cmu.cs.stage3.math.MathUtilities.getYAxis(), -dx * .01, pickedTransformable);
} else {
helper.setTransformationRightNow(
edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), world);
helper.setPositionRightNow(zeroVec, pickedTransformable);
pickedTransformable.rotateRightNow(
edu.cmu.cs.stage3.math.MathUtilities.getYAxis(), -dx * .01, helper);
}
} else if (shiftDown) {
helper.setTransformationRightNow(
edu.cmu.cs.stage3.math.MathUtilities.createIdentityMatrix4d(), world);
helper.setPositionRightNow(zeroVec, pickedTransformable);
tempVec.x = 0.0;
tempVec.y = -dy * deltaFactor;
tempVec.z = 0.0;
pickedTransformable.moveRightNow(tempVec, helper);
} else {
javax.vecmath.Matrix4d cameraTransformation =
camera.getSceneGraphTransformable().getAbsoluteTransformation();
cameraUp.x = cameraTransformation.m10;
cameraUp.y = cameraTransformation.m11;
cameraUp.z = cameraTransformation.m12;
cameraForward.x = cameraTransformation.m20;
cameraForward.y = cameraTransformation.m21;
cameraForward.z = cameraTransformation.m22;
helper.setPositionRightNow(zeroVec, pickedTransformable);
if (Math.abs(cameraForward.y) < Math.abs(cameraUp.y)) { // if we're looking mostly level
cameraForward.y = 0.0;
helper.setOrientationRightNow(cameraForward, cameraUp, world);
} else { // if we're looking mostly up or down
cameraUp.y = 0.0;
cameraForward.negate();
helper.setOrientationRightNow(cameraUp, cameraForward, world);
}
tempVec.x = dx * deltaFactor;
tempVec.y = 0.0;
tempVec.z = -dy * deltaFactor;
pickedTransformable.moveRightNow(tempVec, helper);
}
}
}
}
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;
}
}
public void setAbsoluteTransformation(javax.vecmath.Matrix4d m) {
ReferenceFrame vehicle = (ReferenceFrame) getParent();
setLocalTransformation(MathUtilities.multiply(m, vehicle.getInverseAbsoluteTransformation()));
}