public Quaternion(Matrix3 matrix)
{
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternion Calculus and Fast Animation".
tuple = new double[4];
int[] next = {1, 2, 0};
var trace = matrix.entry[0]
+ matrix.entry[4]
+ matrix.entry[8];
double root;
if (trace > 0)
{
// |w| > 1/2, may as well choose w > 1/2
root = System.Math.Sqrt(trace + 1); // 2w
tuple[0] = 0.5 * root;
root = 0.5 / root; // 1/(4w)
tuple[1] = (matrix.entry[5] - matrix.entry[7]) * root;
tuple[2] = (matrix.entry[6] - matrix.entry[2]) * root;
tuple[3] = (matrix.entry[1] - matrix.entry[3]) * root;
}
else
{
// |w| <= 1/2
var i = 0;
if (matrix.entry[4] > matrix.entry[0])
i = 1;
if (matrix.entry[8] > matrix.entry[i + 3 * i])
i = 2;
var j = next[i];
var k = next[j];
root = System.Math.Sqrt(matrix.entry[i + 3 * i]
- matrix.entry[j + 3 * j]
- matrix.entry[k + 3 * k] + 1);
tuple[i + 1] = 0.5 * root;
root = 0.5 / root;
tuple[0] = (matrix.entry[j + 3 * k] - matrix.entry[k + 3 * j]) * root;
tuple[j + 1] = (matrix.entry[i + 3 * j] + matrix.entry[j + 3 * i]) * root;
tuple[k + 1] = (matrix.entry[i + 3 * k] + matrix.entry[k + 3 * i]) * root;
}
}
public Vector3 Mult(Matrix3 matrix)
{
return new Vector3(
tuple[0] * matrix.entry[0] + tuple[1] * matrix.entry[1] + tuple[2] * matrix.entry[2],
tuple[0] * matrix.entry[3] + tuple[1] * matrix.entry[4] + tuple[2] * matrix.entry[5],
tuple[0] * matrix.entry[6] + tuple[1] * matrix.entry[7] + tuple[2] * matrix.entry[8]);
}
public Matrix3 TransposeTimes(Matrix3 matrix)
{
// P = A^T*B, P[r][c] = sum_m A[m][r]*B[m][c]
Matrix3 result = new Matrix3();
for (int row = 0; row < 3; row++)
{
for (int col = 0; col < 3; col++)
{
int i = I(row, col);
result.entry[i] = 0;
for (int mid = 0; mid < 3; mid++)
result.entry[i] += entry[I(mid, row)] * matrix.entry[I(mid, col)];
}
}
return result;
}
public Matrix3 Transpose()
{
Matrix3 result = new Matrix3();
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
result.entry[I(i, j)] = entry[I(j, i)];
return result;
}
public Matrix3 TimesTranspose(Matrix3 matrix)
{
// P = A*B^T, P[r][c] = sum_m A[r][m]*B[c][m]
Matrix3 result = new Matrix3();
for (int row = 0; row < 3; row++)
{
for (int col = 0; col < 3; col++)
{
int i = I(row, col);
result.entry[i] = 0;
for (int mid = 0; mid < 3; mid++)
result.entry[i] += entry[I(row, mid)] * matrix.entry[I(col, mid)];
}
}
return result;
}
public Matrix3 Sub(Matrix3 matrix)
{
Matrix3 result = new Matrix3();
for (int i = 0; i < 9; i++)
result.entry[i] = entry[i] - matrix.entry[i];
return result;
}
public Matrix3 Negative()
{
Matrix3 result = new Matrix3();
for (int i = 0; i < 9; i++)
result.entry[i] = -entry[i];
return result;
}
public Matrix3(Matrix3 matrix)
{
entry = new double[9];
for (int i = 0; i < 9; i++)
entry[i] = matrix.entry[i];
}
public Matrix3 Div(double scalar)
{
Matrix3 result = new Matrix3();
if (scalar != 0)
{
double invScalar = 1 / scalar;
for (int i = 0; i < 9; i++)
result.entry[i] = entry[i] * invScalar;;
}
else
for (int i = 0; i < 9; i++)
result.entry[i] = double.MaxValue;
return result;
}
public Matrix3 Adjoint()
{
Matrix3 result = new Matrix3();
result.entry[0] = entry[4] * entry[8] - entry[5] * entry[7];
result.entry[3] = entry[2] * entry[7] - entry[1] * entry[8];
result.entry[6] = entry[1] * entry[5] - entry[2] * entry[4];
result.entry[1] = entry[5] * entry[6] - entry[3] * entry[8];
result.entry[4] = entry[0] * entry[8] - entry[2] * entry[6];
result.entry[7] = entry[2] * entry[3] - entry[0] * entry[5];
result.entry[2] = entry[3] * entry[7] - entry[4] * entry[6];
result.entry[5] = entry[1] * entry[6] - entry[0] * entry[7];
result.entry[8] = entry[0] * entry[4] - entry[1] * entry[3];
return result;
}
public Matrix3 Add(double scalar)
{
Matrix3 result = new Matrix3();
for (int i = 0; i < 9; i++)
result.entry[i] = entry[i] * scalar;
return result;
}
public static Matrix3 RotationMatrix(double alpha, double beta, double gamma)
{
Matrix3 result = new Matrix3();
result.entry[0] = + System.Math.Cos(alpha) * System.Math.Cos(gamma)
- System.Math.Sin(alpha) * System.Math.Cos(beta) * System.Math.Sin(gamma);
result.entry[1] = - System.Math.Sin(alpha) * System.Math.Cos(gamma)
- System.Math.Cos(alpha) * System.Math.Cos(beta) * System.Math.Sin(gamma);
result.entry[2] = + System.Math.Sin(beta) * System.Math.Sin(gamma);
result.entry[3] = + System.Math.Cos(alpha) * System.Math.Sin(gamma)
+ System.Math.Sin(alpha) * System.Math.Cos(beta) * System.Math.Cos(gamma);
result.entry[4] = - System.Math.Sin(alpha) * System.Math.Sin(gamma)
- System.Math.Cos(alpha) * System.Math.Cos(beta) * System.Math.Cos(gamma);
result.entry[5] = - System.Math.Sin(beta) * System.Math.Cos(gamma);
result.entry[6] = + System.Math.Sin(alpha) * System.Math.Sin(beta);
result.entry[7] = + System.Math.Cos(alpha) * System.Math.Sin(beta);
result.entry[8] = + System.Math.Cos(beta);
return result;
}
public Matrix3 ToRotationMatrix()
{
var result = new Matrix3();
var x = 2 * tuple[1];
var y = 2 * tuple[2];
var z = 2 * tuple[3];
var wx = x * tuple[0];
var wy = y * tuple[0];
var wz = z * tuple[0];
var xx = x * tuple[1];
var xy = y * tuple[1];
var xz = z * tuple[1];
var yy = y * tuple[2];
var yz = z * tuple[2];
var zz = z * tuple[3];
result.entry[0] = 1 - (yy + zz);
result.entry[3] = xy - wz;
result.entry[6] = xz + wy;
result.entry[1] = xy + wz;
result.entry[4] = 1 - (xx + zz);
result.entry[7] = yz - wx;
result.entry[2] = xz - wy;
result.entry[5] = yz + wx;
result.entry[8] = 1 - (xx + yy);
return result;
}
