final P3DRootNode root = getRoot();
Vector3d up = root != null ? new Vector3d(root.getUpVector()) : new Vector3d(0.0, 1.0, 0.0);
- double a = up.angle(getPathLegEndpointVector());
+ final Vector3d legDir = getPathLegEndpointVector();
+ double a = up.angle(legDir);
if (a < 0.1 || (Math.PI - a) < 0.1) {
// Rotate components
up.set(up.getY(), up.getZ(), up.getX());
}
+
+ // Project up vector into a normal of the leg direction before applying to 'dir'
+ MathTools.mad(up, legDir, -legDir.dot(up)/legDir.lengthSquared());
+ up.normalize();
return getControlPointOrientationQuat(dir, up, angle);
}