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; }