/**
* <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;
}
protected void doGroundMovement() {
if (!isOnGround) return;
// ground movement is as follows, the arrow key dictate the target velocity, and accelleration
// is
// added to achieve that. If opposing keys are held the target velocity is set to zero.
// This is the velocity in the normal plane in the player space
Vector3f targetVelocity = new Vector3f();
if (forward) targetVelocity.z += 1;
if (backward) targetVelocity.z -= 1;
if (left) targetVelocity.x -= 1;
if (right) targetVelocity.x += 1;
if (forward != backward || left != right) {
targetVelocity.normalize();
targetVelocity.scale(MAX_SPEED);
}
forward = false;
backward = false;
left = false;
right = false;
Vector3f forward = new Vector3f(0, 0, 1);
QuaternionUtil.quatRotate(owner.getOrientation(), forward, forward);
// right = normal x forward
// forward = right x normal
Vector3f right = new Vector3f();
right.cross(surfaceNormal, forward);
forward.cross(right, surfaceNormal);
right.normalize();
forward.normalize();
Matrix3f transform = new Matrix3f();
transform.setColumn(0, right);
transform.setColumn(1, surfaceNormal);
transform.setColumn(2, forward);
transform.transform(targetVelocity);
Vector3f delta = new Vector3f();
owner.getRigidBody().getLinearVelocity(delta);
delta.sub(targetVelocity, delta);
// feet can't pull and don't need to push for now
// Ds = D - N(N.D)
Vector3f parComp = new Vector3f(surfaceNormal);
parComp.scale(surfaceNormal.dot(delta));
delta.sub(delta, parComp);
owner.getRigidBody().applyCentralForce(scaleForce(delta));
}
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);
}
/**
* <code>toAxes</code> takes in an array of three vectors. Each vector corresponds to an axis of
* the coordinate system defined by the quaternion rotation.
*
* @param axis the array of vectors to be filled.
*/
public void toAxes(Vector3f axis[]) {
Matrix3f tempMat = toRotationMatrix();
axis[0] = tempMat.getColumn(0, axis[0]);
axis[1] = tempMat.getColumn(1, axis[1]);
axis[2] = tempMat.getColumn(2, axis[2]);
}
