public Quaternion fromRotationMatrix(
float m00,
float m01,
float m02,
float m10,
float m11,
float m12,
float m20,
float m21,
float m22) {
// Use the Graphics Gems code, from
// ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z
// *NOT* the "Matrix and Quaternions FAQ", which has errors!
// the trace is the sum of the diagonal elements; see
// http://mathworld.wolfram.com/MatrixTrace.html
float t = m00 + m11 + m22;
// we protect the division by s by ensuring that s>=1
if (t >= 0) { // |w| >= .5
float s = FastMath.sqrt(t + 1); // |s|>=1 ...
w = 0.5f * s;
s = 0.5f / s; // so this division isn't bad
x = (m21 - m12) * s;
y = (m02 - m20) * s;
z = (m10 - m01) * s;
} else if ((m00 > m11) && (m00 > m22)) {
float s = FastMath.sqrt(1.0f + m00 - m11 - m22); // |s|>=1
x = s * 0.5f; // |x| >= .5
s = 0.5f / s;
y = (m10 + m01) * s;
z = (m02 + m20) * s;
w = (m21 - m12) * s;
} else if (m11 > m22) {
float s = FastMath.sqrt(1.0f + m11 - m00 - m22); // |s|>=1
y = s * 0.5f; // |y| >= .5
s = 0.5f / s;
x = (m10 + m01) * s;
z = (m21 + m12) * s;
w = (m02 - m20) * s;
} else {
float s = FastMath.sqrt(1.0f + m22 - m00 - m11); // |s|>=1
z = s * 0.5f; // |z| >= .5
s = 0.5f / s;
x = (m02 + m20) * s;
y = (m21 + m12) * s;
w = (m10 - m01) * s;
}
return this;
}
/*
* (non-Javadoc)
* @see com.jme.bounding.BoundingVolume#intersectsWhere(com.jme.math.Ray)
*/
public int collideWithRay(Ray ray, CollisionResults results) {
Vector3f vect1 = Vector3f.newInstance();
Vector3f diff = vect1.set(ray.getOrigin()).subtractLocal(center);
float a = diff.dot(diff) - (getRadius() * getRadius());
float a1, discr, root;
if (a <= 0.0) {
// inside sphere
a1 = ray.direction.dot(diff);
discr = (a1 * a1) - a;
root = FastMath.sqrt(discr);
float distance = root - a1;
Vector3f point = new Vector3f(ray.direction).multLocal(distance).addLocal(ray.origin);
CollisionResult result = new CollisionResult(point, distance);
results.addCollision(result);
Vector3f.recycle(vect1);
return 1;
}
a1 = ray.direction.dot(diff);
if (a1 >= 0.0) {
Vector3f.recycle(vect1);
return 0;
}
discr = a1 * a1 - a;
if (discr < 0.0) {
Vector3f.recycle(vect1);
return 0;
} else if (discr >= FastMath.ZERO_TOLERANCE) {
root = FastMath.sqrt(discr);
float dist = -a1 - root;
Vector3f point = new Vector3f(ray.direction).multLocal(dist).addLocal(ray.origin);
results.addCollision(new CollisionResult(point, dist));
dist = -a1 + root;
point = new Vector3f(ray.direction).multLocal(dist).addLocal(ray.origin);
results.addCollision(new CollisionResult(point, dist));
Vector3f.recycle(vect1);
return 2;
} else {
float dist = -a1;
Vector3f point = new Vector3f(ray.direction).multLocal(dist).addLocal(ray.origin);
results.addCollision(new CollisionResult(point, dist));
Vector3f.recycle(vect1);
return 1;
}
}
/**
* Calculates the minimum bounding sphere of 2 points. Used in welzl's algorithm.
*
* @param O The 1st point inside the sphere.
* @param A The 2nd point inside the sphere.
* @see #calcWelzl(java.nio.FloatBuffer)
*/
private void setSphere(Vector3f O, Vector3f A) {
radius =
FastMath.sqrt(
((A.x - O.x) * (A.x - O.x) + (A.y - O.y) * (A.y - O.y) + (A.z - O.z) * (A.z - O.z))
/ 4f)
+ RADIUS_EPSILON
- 1f;
center.interpolate(O, A, .5f);
}
/**
* <code>toAngleAxis</code> sets a given angle and axis to that represented by the current
* quaternion. The values are stored as following: The axis is provided as a parameter and built
* by the method, the angle is returned as a float.
*
* @param axisStore the object we'll store the computed axis in.
* @return the angle of rotation in radians.
*/
public float toAngleAxis(Vector3f axisStore) {
float sqrLength = x * x + y * y + z * z;
float angle;
if (sqrLength == 0.0f) {
angle = 0.0f;
if (axisStore != null) {
axisStore.x = 1.0f;
axisStore.y = 0.0f;
axisStore.z = 0.0f;
}
} else {
angle = (2.0f * FastMath.acos(w));
if (axisStore != null) {
float invLength = (1.0f / FastMath.sqrt(sqrLength));
axisStore.x = x * invLength;
axisStore.y = y * invLength;
axisStore.z = z * invLength;
}
}
return angle;
}
public float distance(Vector3f point) {
return FastMath.sqrt(distanceSquared(point));
}