public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {
return (planeNormal.dot(point) + d);
}
-
+
+ public static Vector3d projectToPlane(Vector3d v, Vector3d planeNormal) {
+ //v.normalize();
+ //planeNormal.normalize();
+ Vector3d t = new Vector3d();
+ if (planeNormal == X_AXIS) {
+ t.set(0, v.y, v.z);
+ }
+ else if (planeNormal == Y_AXIS) {
+ t.set(v.x, 0, v.z);
+ }
+ else if (planeNormal == Z_AXIS) {
+ t.set(v.x, v.y, 0);
+ }
+ else {
+ 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 = projectToPlane(getMinAxis(original), original);
+ a.normalize();
+ 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 = projectToPlane(getMinAxis(original), original);
+ a.normalize();
+ result.set(a.getX(), a.getY(), a.getZ(), 0.0);
+ } else {
+ // Cosine and sine of half angle
+ double cosHalf = Math.sqrt((1 + d)/2);
+ double sinHalf = Math.sqrt((1 - d)/2);
+ Vector3d axis = new Vector3d();
+ axis.cross(original, rotated);
+ axis.normalize();
+ result.set(sinHalf * axis.getX(), sinHalf * axis.getY(), sinHalf * axis.getZ(), cosHalf);
}
+ return true;
+ }
+
+ public static Vector3d getMinAxis(Vector3d original) {
+ double absX = Math.abs(original.getX());
+ double absY = Math.abs(original.getY());
+ double absZ = Math.abs(original.getZ());
+
+ if (absX <= absY && absX <= absZ)
+ return X_AXIS;
+ else if (absY <= absZ)
+ return Y_AXIS;
+ else
+ return Z_AXIS;
+ }
+
+ 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;
+ }
public static void getQuat(Vector3d axis, double angle, Quat4d q)
{
mat.m23 = -(f+n)/(f-n);
return mat;
}
+
+ /**
+ * Round the number to a given number of decimals, if the rounded result contains at least three
+ * zero trailing decimal places within the rounded result.
+ */
+ public static double round(double value, int nsig) {
+ if (Math.abs(value) < NEAR_ZERO)
+ return 0.0;
+
+ int decimals = (int) Math.round(Math.log10(value));
+ int shift = nsig - decimals;
+ double multiplier = Math.pow(10.0, shift);
+ long roundedValue = Math.round(value * multiplier);
+ if (roundedValue % 1000 == 0) {
+ // Rounding results in at least three zeroes at the end
+ return roundedValue / multiplier;
+ } else {
+ // Number was not close to a shorter decimal representation, return it as is
+ return value;
+ }
+ }
}
Code Review / simantics / 3d.git / blobdiff