+ return true;
+ }
+
+ public static boolean createRotation(Vector3d original, Vector3d rotated, Vector3d axis, AxisAngle4d result) {
+
+ if (rotated.lengthSquared() > 0.01)
+ rotated.normalize();
+ else
+ return false;
+ if (original.lengthSquared() > 0.01)
+ original.normalize();
+ else
+ return false;
+ if (axis.lengthSquared() > 0.01)
+ axis.normalize();
+ else
+ return false;
+ double d = original.dot(rotated);
+ if (d > 0.9999) {
+ // original and rotated are parallel, pointing at the same direction
+ result.angle = 0.0;
+ result.x = axis.x;
+ result.y = axis.y;
+ result.z = axis.z;
+ } else if (d < -0.9999) {
+ // original and rotated are parallel, pointing at the opposite direction
+ result.angle = Math.PI;
+ result.x = axis.x;
+ result.y = axis.y;
+ result.z = axis.z;
+ } else {
+ // Project vectors to Axis plane
+ Vector3d p1 = projectToPlane(original, axis);
+ Vector3d p2 = projectToPlane(rotated, axis);
+ // Create vectors where z-axis is plane normal
+ Quat4d q = getQuat(createRotation(axis, Z_AXIS));
+ Vector3d t1 = new Vector3d();
+ Vector3d t2 = new Vector3d();
+ rotate(q, p1, t1);
+ rotate(q, p2, t2);
+ // Calculate angles on z-axis plane.
+ double a1 = Math.atan2(t1.y, t1.x);
+ double a2 = Math.atan2(t2.y, t2.x);
+ result.set(axis,a2-a1);
+
+ }
+ return true;
+ }