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 Vector3[] getOrientationAsForwardAndUpGuide(ReferenceFrame asSeenBy) {
Matrix33 axes = getOrientationAsAxes(asSeenBy);
Vector3[] orientation = {axes.getRow(2), axes.getRow(1)};
return orientation;
}