+ public Quat4d getControlPointOrientationQuat(Vector3d dir, double angle, boolean reversed) {
+ if (turnAxis == null) {
+ if (dir.lengthSquared() > MathTools.NEAR_ZERO)
+ dir.normalize();
+ Quat4d q = getControlPointOrientationQuat(dir, angle);
+ if (reversed) {
+ Quat4d q2 = new Quat4d();
+ q2.set(new AxisAngle4d(MathTools.Y_AXIS, Math.PI));
+ q.mulInverse(q2);
+ }
+ return q;
+ } else {
+ if (dir.lengthSquared() > MathTools.NEAR_ZERO)
+ dir.normalize();
+ return getControlPointOrientationQuat(dir, turnAxis, angle);
+ }
+ }
+
+ public Quat4d getControlPointOrientationQuat(double angle, boolean reversed) {
+ Vector3d dir = getPathLegDirection();
+ return getControlPointOrientationQuat(dir, angle, reversed);
+ }
+
+ public Quat4d getControlPointOrientationQuat(Vector3d dir, double angle) {
+ if (dir == null || dir.lengthSquared() < MathTools.NEAR_ZERO)
+ return MathTools.getIdentityQuat();
+
+ final P3DRootNode root = getRoot();
+ Vector3d up = root != null ? new Vector3d(root.getUpVector()) : new Vector3d(0.0, 1.0, 0.0);
+ 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);
+ }
+
+ public P3DRootNode getRoot() {
+ INode n = getParent();
+ while (n != null && !(n instanceof P3DRootNode))
+ n = n.getParent();
+ return (P3DRootNode) n;
+ }
+
+ public static Quat4d getControlPointOrientationQuat(Vector3d dir, Vector3d up, double angle) {
+ if (dir == null || dir.lengthSquared() < MathTools.NEAR_ZERO)
+ return MathTools.getIdentityQuat();
+
+ final Vector3d front = new Vector3d(1.0,0.0,0.0);
+
+ Quat4d q1 = new Quat4d();
+
+
+ Vector3d right = new Vector3d();
+
+ up = new Vector3d(up);
+ right.cross(dir, up);
+ up.cross(right, dir);
+ right.normalize();
+ up.normalize();
+
+ Matrix3d m = new Matrix3d();
+ m.m00 = dir.x;
+ m.m10 = dir.y;
+ m.m20 = dir.z;
+ m.m01 = up.x;
+ m.m11 = up.y;
+ m.m21 = up.z;
+ m.m02 = right.x;
+ m.m12 = right.y;
+ m.m22 = right.z;
+
+ //q1.set(m); MathTools contains more stable conversion
+ MathTools.getQuat(m, q1);
+
+ // if (DEBUG) System.out.println("PipingTools.getPipeComponentOrientationQuat() " + dir+ " " + up + " " + right);
+
+ Quat4d q2 = new Quat4d();
+ q2.set(new AxisAngle4d(front, angle));
+ q1.mul(q2);
+ return q1;
+ }