public Frame3f(Vector3F origin, Vector3F x, Vector3F y, Vector3F z) { this.origin = origin; Matrix3f m = new Matrix3f(x, y, z, false); this.rotation = m.ToQuaternion(); }
public Matrix3f ToRotationMatrix() { float twoX = 2 * x; float twoY = 2 * y; float twoZ = 2 * z; float twoWX = twoX * w; float twoWY = twoY * w; float twoWZ = twoZ * w; float twoXX = twoX * x; float twoXY = twoY * x; float twoXZ = twoZ * x; float twoYY = twoY * y; float twoYZ = twoZ * y; float twoZZ = twoZ * z; Matrix3f m = Matrix3f.Zero; m[0, 0] = 1 - (twoYY + twoZZ); m[0, 1] = twoXY - twoWZ; m[0, 2] = twoXZ + twoWY; m[1, 0] = twoXY + twoWZ; m[1, 1] = 1 - (twoXX + twoZZ); m[1, 2] = twoYZ - twoWX; m[2, 0] = twoXZ - twoWY; m[2, 1] = twoYZ + twoWX; m[2, 2] = 1 - (twoXX + twoYY); return(m); }
public void SetFromRotationMatrix(Matrix3f rot) { // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes // article "Quaternion Calculus and Fast Animation". Index3i next = new Index3i(1, 2, 0); float trace = rot[0, 0] + rot[1, 1] + rot[2, 2]; float root; if (trace > 0) { // |w| > 1/2, may as well choose w > 1/2 root = (float)Math.Sqrt(trace + (float)1); // 2w w = ((float)0.5) * root; root = ((float)0.5) / root; // 1/(4w) x = (rot[2, 1] - rot[1, 2]) * root; y = (rot[0, 2] - rot[2, 0]) * root; z = (rot[1, 0] - rot[0, 1]) * root; } else { // |w| <= 1/2 int i = 0; if (rot[1, 1] > rot[0, 0]) { i = 1; } if (rot[2, 2] > rot[i, i]) { i = 2; } int j = next[i]; int k = next[j]; root = (float)Math.Sqrt(rot[i, i] - rot[j, j] - rot[k, k] + (float)1); Vector3F quat = new Vector3F(x, y, z); quat[i] = ((float)0.5) * root; root = ((float)0.5) / root; w = (rot[k, j] - rot[j, k]) * root; quat[j] = (rot[j, i] + rot[i, j]) * root; quat[k] = (rot[k, i] + rot[i, k]) * root; x = quat.x; y = quat.y; z = quat.z; } Normalize(); // we prefer normalized quaternions... }
public Quaternionf(Matrix3f mat) { x = y = z = 0; w = 1; SetFromRotationMatrix(mat); }