/**
* <code>toRotationMatrix</code> converts this quaternion to a rotational matrix. The result is
* stored in result.
*
* @param result The Matrix3f to store the result in.
* @return the rotation matrix representation of this quaternion.
*/
public Matrix3f toRotationMatrix(Matrix3f result) {
float norm = norm();
// we explicitly test norm against one here, saving a division
// at the cost of a test and branch. Is it worth it?
float s = (norm == 1f) ? 2f : (norm > 0f) ? 2f / norm : 0;
// compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
// will be used 2-4 times each.
float xs = x * s;
float ys = y * s;
float zs = z * s;
float xx = x * xs;
float xy = x * ys;
float xz = x * zs;
float xw = w * xs;
float yy = y * ys;
float yz = y * zs;
float yw = w * ys;
float zz = z * zs;
float zw = w * zs;
// using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
result.m00 = 1 - (yy + zz);
result.m01 = (xy - zw);
result.m02 = (xz + yw);
result.m10 = (xy + zw);
result.m11 = 1 - (xx + zz);
result.m12 = (yz - xw);
result.m20 = (xz - yw);
result.m21 = (yz + xw);
result.m22 = 1 - (xx + yy);
return result;
}
public void orthogonalLineFit(FloatBuffer points) {
if (points == null) {
return;
}
points.rewind();
// compute average of points
int length = points.remaining() / 3;
BufferUtils.populateFromBuffer(origin, points, 0);
for (int i = 1; i < length; i++) {
BufferUtils.populateFromBuffer(compVec1, points, i);
origin.addLocal(compVec1);
}
origin.multLocal(1f / (float) length);
// compute sums of products
float sumXX = 0.0f, sumXY = 0.0f, sumXZ = 0.0f;
float sumYY = 0.0f, sumYZ = 0.0f, sumZZ = 0.0f;
points.rewind();
for (int i = 0; i < length; i++) {
BufferUtils.populateFromBuffer(compVec1, points, i);
compVec1.subtract(origin, compVec2);
sumXX += compVec2.x * compVec2.x;
sumXY += compVec2.x * compVec2.y;
sumXZ += compVec2.x * compVec2.z;
sumYY += compVec2.y * compVec2.y;
sumYZ += compVec2.y * compVec2.z;
sumZZ += compVec2.z * compVec2.z;
}
// find the smallest eigen vector for the direction vector
compMat1.m00 = sumYY + sumZZ;
compMat1.m01 = -sumXY;
compMat1.m02 = -sumXZ;
compMat1.m10 = -sumXY;
compMat1.m11 = sumXX + sumZZ;
compMat1.m12 = -sumYZ;
compMat1.m20 = -sumXZ;
compMat1.m21 = -sumYZ;
compMat1.m22 = sumXX + sumYY;
compEigen1.calculateEigen(compMat1);
direction = compEigen1.getEigenVector(0);
}