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);
}
@Override
public void update(double t) {
if (getTimeRemaining(t) > 0) {
edu.cmu.cs.stage3.math.Matrix44 asSeenByTrans =
m_asSeenBy.getTransformation(subject.getWorld());
((edu.cmu.cs.stage3.alice.core.Transformable) m_asSeenBy)
.standUpRightNow(subject.getWorld());
double portion = getTimeElapsed(t) / (getTimeElapsed(t) + getTimeRemaining(t));
if (portion <= 1.0) {
double x;
double y;
double z;
double dx;
double dy;
double dz;
// get the appropriate position
x = m_xHermite.evaluate(portion);
y = m_yHermite.evaluate(portion);
z = m_zHermite.evaluate(portion);
subject.setPositionRightNow(x, y, z, m_asSeenBy);
// face the direction you are moving
dx = m_xHermite.evaluateDerivative(portion);
// dy = m_yHermite.evaluateDerivative(portion);
dy = 0.0;
dz = m_zHermite.evaluateDerivative(portion);
if (!(dx == 0 && dy == 0 && dz == 0)) {
Matrix33 orient = new Matrix33();
orient.setForwardUpGuide(
new javax.vecmath.Vector3d(dx, dy, dz), new javax.vecmath.Vector3d(0, 1, 0));
// System.out.println(m_asSeenBy);
subject.setOrientationRightNow(orient, m_asSeenBy);
// subject.s
} else {
// System.out.println("deriv 0");
}
if (timePerStep == -1) {
if (!Double.isNaN(duration.doubleValue())) {
timePerStep = duration.doubleValue() / numberOfSteps;
} else {
timePerStep = 1.0 / stepSpeed.doubleValue();
}
}
// int stepNumber = (int)java.lang.Math.ceil(
// getTimeElapsed(t) * stepSpeed.doubleValue()) - 1;
int stepNumber = (int) java.lang.Math.ceil(getTimeElapsed(t) * (1.0 / timePerStep)) - 1;
if (stepNumber == -1) {
stepNumber = 0;
}
if (stepNumber == numberOfSteps) {
stepNumber -= 1;
}
double portionOfStep = (getTimeElapsed(t) - stepNumber * timePerStep) / timePerStep;
if (portionOfStep > 1.0) {
portionOfStep = 1.0;
}
boolean lastStep = false;
if (stepNumber == numberOfSteps - 1) {
lastStep = true;
}
if (stepNumber % 2 == 0) {
stepRight(portionOfStep, lastStep);
} else {
stepLeft(portionOfStep, lastStep);
}
super.update(t);
}
((edu.cmu.cs.stage3.alice.core.Transformable) m_asSeenBy)
.setTransformationRightNow(asSeenByTrans, subject.getWorld());
}
}
public Vector3[] getOrientationAsForwardAndUpGuide(ReferenceFrame asSeenBy) {
Matrix33 axes = getOrientationAsAxes(asSeenBy);
Vector3[] orientation = {axes.getRow(2), axes.getRow(1)};
return orientation;
}
