return (planeNormal.dot(point) + d);
}
+ public static Vector3d projectToPlane(Vector3d v, Vector3d planeNormal) {
+ //v.normalize();
+ //planeNormal.normalize();
+ Vector3d t = new Vector3d();
+ t.cross(v,planeNormal);
+ t.cross(planeNormal, t);
+ return t;
+
+ }
+
public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
intersectPoint.set(planePoint);
intersectPoint.sub(linePoint);
public static boolean createRotation(Vector3d original, Vector3d rotated, AxisAngle4d result) {
- if (rotated.lengthSquared() > 0.01)
- rotated.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 = 0.0;
- result.y = 1.0;
- result.z = 0.0;
- } else if (d < -0.9999) {
- // original and rotated are parallel, pointing at the opposite direction
- Vector3d a = Z_AXIS;
- if (Math.abs(a.dot(original)) > 0.8 )
- a = Y_AXIS;
- result.set(a, Math.PI);
- } else {
- double angle = original.angle(rotated);
- Vector3d axis = new Vector3d();
- axis.cross(original, rotated);
- result.set(axis,angle);
- }
- return true;
+ if (rotated.lengthSquared() > 0.01)
+ rotated.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 = 0.0;
+ result.y = 1.0;
+ result.z = 0.0;
+ } else if (d < -0.9999) {
+ // original and rotated are parallel, pointing at the opposite direction
+ Vector3d a = Z_AXIS;
+ if (Math.abs(a.dot(original)) > 0.8 )
+ a = Y_AXIS;
+ result.set(a, Math.PI);
+ } else {
+ double angle = original.angle(rotated);
+ Vector3d axis = new Vector3d();
+ axis.cross(original, rotated);
+ result.set(axis,angle);
}
+ return true;
+ }
public static boolean createRotation(Vector3d original, Vector3d rotated, Quat4d result) {
- if (rotated.lengthSquared() > 0.01)
- rotated.normalize();
- else
- return false;
- double d = original.dot(rotated);
- if (d > 0.9999) {
- // original and rotated are parallel, pointing at the same direction
- result.w = 1.0;
- result.x = 0.0;
- result.y = 0.0;
- result.z = 0.0;
- } else if (d < -0.9999) {
- // original and rotated are parallel, pointing at the opposite direction
- Vector3d a = Z_AXIS;
- if (Math.abs(a.dot(original)) > 0.8 )
- a = Y_AXIS;
- getQuat(a, Math.PI, result);
-
- } else {
- double angle = original.angle(rotated);
- Vector3d axis = new Vector3d();
- axis.cross(original, rotated);
- getQuat(axis, angle, result);
- }
- return true;
+ if (rotated.lengthSquared() > 0.01)
+ rotated.normalize();
+ else
+ return false;
+ double d = original.dot(rotated);
+ if (d > 0.9999) {
+ // original and rotated are parallel, pointing at the same direction
+ result.w = 1.0;
+ result.x = 0.0;
+ result.y = 0.0;
+ result.z = 0.0;
+ } else if (d < -0.9999) {
+ // original and rotated are parallel, pointing at the opposite direction
+ Vector3d a = Z_AXIS;
+ if (Math.abs(a.dot(original)) > 0.8 )
+ a = Y_AXIS;
+ getQuat(a, Math.PI, result);
+
+ } else {
+ double angle = original.angle(rotated);
+ Vector3d axis = new Vector3d();
+ axis.cross(original, rotated);
+ getQuat(axis, angle, result);
}
+ 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 {
+ Vector3d p1 = projectToPlane(original, axis);
+ Vector3d p2 = projectToPlane(rotated, axis);
+ double angle = p1.angle(p2);
+ result.set(axis,angle);
+
+ }
+ return true;
+ }
public static void getQuat(Vector3d axis, double angle, Quat4d q)
{