@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());
}
}
@Override
public void prologue(double t) {
beginEqualsEnd = false;
done = false;
subject = WalkToAnimation.this.subject.getTransformableValue();
m_asSeenBy = asSeenBy.getReferenceFrameValue();
edu.cmu.cs.stage3.math.Matrix44 asSeenByTrans =
m_asSeenBy.getTransformation(subject.getWorld());
((edu.cmu.cs.stage3.alice.core.Transformable) m_asSeenBy).standUpRightNow(subject.getWorld());
m_transformationBegin = subject.getTransformation(m_asSeenBy);
if (m_asSeenBy == null) {
throw new edu.cmu.cs.stage3.alice.core.SimulationPropertyException(
subject.name.getStringValue() + " needs something or someone to walk to.",
null,
asSeenBy);
}
if (subject == m_asSeenBy) {
throw new edu.cmu.cs.stage3.alice.core.SimulationPropertyException(
subject.name.getStringValue() + " can't walk to " + subject.name.getStringValue() + ".",
getCurrentStack(),
asSeenBy);
}
if (subject.isAncestorOf(m_asSeenBy)) {
throw new edu.cmu.cs.stage3.alice.core.SimulationPropertyException(
subject.name.getStringValue() + " can't walk to a part of itself",
getCurrentStack(),
asSeenBy);
}
// find end transformation
javax.vecmath.Vector3d posAbs = getPositionEnd();
javax.vecmath.Vector3d curPos = subject.getPosition();
subject.setPositionRightNow(posAbs, m_asSeenBy);
// javax.vecmath.Matrix3d paMatrix =
// subject.calculatePointAt(m_asSeenBy, null, new
// javax.vecmath.Vector3d(0,1,0), null, true);
javax.vecmath.Matrix3d paMatrix =
subject.calculatePointAt(
m_asSeenBy, null, new javax.vecmath.Vector3d(0, 1, 0), m_asSeenBy, true);
subject.setPositionRightNow(curPos);
javax.vecmath.Matrix4d pov = asSeenBy.getReferenceFrameValue().getPointOfView();
pov.set(paMatrix);
pov.setRow(3, posAbs.x, posAbs.y, posAbs.z, 1.0);
m_transformationEnd = new edu.cmu.cs.stage3.math.Matrix44(pov);
double dx = m_transformationBegin.m30 - m_transformationEnd.m30;
double dy = m_transformationBegin.m31 - m_transformationEnd.m31;
double dz = m_transformationBegin.m32 - m_transformationEnd.m32;
double distance = Math.sqrt(dx * dx + dy * dy + dz * dz);
double s = distance;
m_xHermite =
new edu.cmu.cs.stage3.math.HermiteCubic(
m_transformationBegin.m30,
m_transformationEnd.m30,
m_transformationBegin.m20 * s,
m_transformationEnd.m20 * s);
m_yHermite =
new edu.cmu.cs.stage3.math.HermiteCubic(
m_transformationBegin.m31,
m_transformationEnd.m31,
m_transformationBegin.m21 * s,
m_transformationEnd.m21 * s);
m_zHermite =
new edu.cmu.cs.stage3.math.HermiteCubic(
m_transformationBegin.m32,
m_transformationEnd.m32,
m_transformationBegin.m22 * s,
m_transformationEnd.m22 * s);
super.prologue(t);
getActualStepLength();
((edu.cmu.cs.stage3.alice.core.Transformable) m_asSeenBy)
.setTransformationRightNow(asSeenByTrans, subject.getWorld());
}
