public static Matrix3d QuatToMatrix3d(Vector4d q)
{
double n = q.Dot(q);
double s = (n > 0.0) ? (2.0 / n) : 0.0f;
double xs, ys, zs;
double wx, wy, wz;
double xx, xy, xz;
double yy, yz, zz;
xs = q.x * s; ys = q.y * s; zs = q.z * s;
wx = q.w * xs; wy = q.w * ys; wz = q.w * zs;
xx = q.x * xs; xy = q.x * ys; xz = q.x * zs;
yy = q.y * ys; yz = q.y * zs; zz = q.z * zs;
Matrix3d m = new Matrix3d();
m[0,0] = 1.0 - (yy + zz); m[1,0] = xy - wz; m[2,0] = xz + wy;
m[0,1] = xy + wz; m[1,1] = 1.0 - (xx + zz); m[2,1] = yz - wx;
m[0,2] = xz - wy; m[1,2] = yz + wx; m[2,2] = 1.0 - (xx + yy);
return m;
}
public Quaternion(Matrix3d m)
{
double T = 1.0 + m.Trace();
double S, W, X, Y, Z;
if (T > eps)
{
S = Math.Sqrt(T) * 2.0;
X = (m[5] - m[7]) / S;
Y = (m[6] - m[2]) / S;
Z = (m[1] - m[3]) / S;
W = 0.25 * S;
}
else if (m[0] > m[4] && m[0] > m[8])
{
S = Math.Sqrt(1.0 + m[0] - m[4] - m[8]) * 2.0;
X = 0.25 * S;
Y = (m[1] + m[3]) / S;
Z = (m[6] + m[2]) / S;
W = (m[5] - m[7]) / S;
}
else if (m[4] > m[8])
{
S = Math.Sqrt(1.0 + m[4] - m[0] - m[8]) * 2;
X = (m[1] + m[3]) / S;
Y = 0.25 * S;
Z = (m[5] + m[7]) / S;
W = (m[6] - m[2]) / S;
}
else
{
S = Math.Sqrt(1.0 + m[8] - m[0] - m[4]) * 2;
X = (m[6] + m[2]) / S;
Y = (m[5] + m[7]) / S;
Z = 0.25 * S;
W = (m[1] - m[3]) / S;
}
this.s = W;
this.v = new Vector3d(X, Y, Z);
}
public Matrix3d ToMatrix3d()
{
double xx = v.x * v.x;
double xy = v.x * v.y;
double xz = v.x * v.z;
double xw = v.x * s;
double yy = v.y * v.y;
double yz = v.y * v.z;
double yw = v.y * s;
double zz = v.z * v.z;
double zw = v.z * s;
Matrix3d m = new Matrix3d();
m[0] = 1 - 2 * (yy + zz);
m[1] = 2 * (xy + zw);
m[2] = 2 * (xz - yw);
m[3] = 2 * (xy - zw);
m[4] = 1 - 2 * (xx + zz);
m[5] = 2 * (yz + xw);
m[6] = 2 * (xz + yw);
m[7] = 2 * (yz - xw);
m[8] = 1 - 2 * (xx + yy);
return m;
}
public Matrix3d ToMatrix3D()
{
Matrix3d ret = new Matrix3d();
ret[0] = this[0, 0];
ret[1] = this[0, 1];
ret[2] = this[0, 2];
ret[3] = this[1, 0];
ret[4] = this[1, 1];
ret[5] = this[1, 2];
ret[6] = this[2, 0];
ret[7] = this[2, 1];
ret[8] = this[2, 2];
return ret;
}
public Matrix4d(Matrix3d m)
{
for (int i=0; i<3; i++)
for (int j=0; j<3; j++)
this[i,j] = m[i,j];
}
public Matrix3d ToMatrix3d()
{
Matrix3d ret = new Matrix3d();
ret[0] = e[0];
ret[1] = e[1];
ret[2] = e[2];
ret[3] = e[4];
ret[4] = e[5];
ret[5] = e[6];
ret[6] = e[8];
ret[7] = e[9];
ret[8] = e[10];
return ret;
}
public Matrix3d OrthogonalFactor(double eps)
{
Matrix3d Q = new Matrix3d(this);
Matrix3d Q2 = new Matrix3d();
double err = 0;
do
{
Q2 = (Q + Q.InverseTranspose()) / 2.0;
err = (Q2 - Q).SqNorm();
Q = Q2;
} while (err > eps);
return Q2;
}
public static Matrix3d operator *(Matrix3d m1, Matrix3d m2)
{
Matrix3d ret = new Matrix3d();
for (int i = 0; i < row_size; i++)
for (int j = 0; j < row_size; j++)
{
ret[i, j] = 0.0;
for (int k = 0; k < row_size; k++)
ret[i, j] += m1[i, k] * m2[k, j];
}
return ret;
}
public Matrix3d InverseSVD()
{
SVD svd = new SVD(e, 3, 3);
Matrix3d inv = new Matrix3d(svd.Inverse);
lastSVDIsFullRank = svd.FullRank;
return inv;
}
public Matrix3d InverseTranspose()
{
double a = e[0];
double b = e[1];
double c = e[2];
double d = e[3];
double E = e[4];
double f = e[5];
double g = e[6];
double h = e[7];
double i = e[8];
double det = a * (E * i - f * h) - b * (d * i - f * g) + c * (d * h - E * g);
if (det == 0) throw new ArithmeticException();
Matrix3d inv = new Matrix3d();
inv[0] = (E * i - f * h) / det;
inv[3] = (c * h - b * i) / det;
inv[6] = (b * f - c * E) / det;
inv[1] = (f * g - d * i) / det;
inv[4] = (a * i - c * g) / det;
inv[7] = (c * d - a * f) / det;
inv[2] = (d * h - E * g) / det;
inv[5] = (b * g - a * h) / det;
inv[8] = (a * E - b * d) / det;
return inv;
}
public static Matrix3d IdentityMatrix()
{
Matrix3d m = new Matrix3d();
m[0] = m[4] = m[8] = 1.0;
return m;
}
public static Matrix3d operator /(Matrix3d m, double d)
{
Matrix3d ret = new Matrix3d();
for (int i=0; i<len; i++) ret[i] = m[i] / d;
return ret;
}
public static Matrix3d operator -(Matrix3d m1, Matrix3d m2)
{
Matrix3d ret = new Matrix3d();
for (int i = 0; i < len; i++) ret[i] = m1[i] - m2[i];
return ret;
}
public Matrix3d(Matrix3d m) : this(m.e)
{
}
public Matrix3d Transpose()
{
Matrix3d m = new Matrix3d();
for (int i = 0; i < row_size; i++)
for (int j = 0; j < row_size; j++)
m[j, i] = this[i, j];
return m;
}
public Matrix3d OuterCross(Vector3d v)
{
Matrix3d m = new Matrix3d();
m[0, 0] = x * v.x;
m[0, 1] = x * v.y;
m[0, 2] = x * v.z;
m[1, 0] = y * v.x;
m[1, 1] = y * v.y;
m[1, 2] = y * v.z;
m[2, 0] = z * v.x;
m[2, 1] = z * v.y;
m[2, 2] = z * v.z;
return m;
}
public Matrix3d(Matrix3d m)
: this(m.e)
{
}
