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);
}
