public static MyMatrix3d IdentityMatrix()
{
MyMatrix3d m = new MyMatrix3d();
m[0] = m[4] = m[8] = 1.0;
return(m);
}
public MyMatrix3d 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();
}
MyMatrix3d inv = new MyMatrix3d();
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);
}
// compute the releft point of p according to the Canvas3 (Canvas3_normal, Canvas3_center)
public static MyVector3 Reflect(MyVector3 p, MyVector3 Canvas3_normal, MyVector3 Canvas3_center)
{
MyVector3 u = p - Canvas3_center;
// create a coord system (x,y,z), project to that sys, reflect and project back
MyVector3 x = Canvas3_normal;
MyVector3 y;
if (x.x == 0 && x.y == 0)
{
y = new MyVector3(0, -x.z, x.y);
}
else
{
y = new MyVector3(x.y, -x.x, 0);
}
MyVector3 z = x.Cross(y);
MyMatrix3d R = new MyMatrix3d(x, y, z);
MyMatrix3d InvR = R.InverseSVD();
MyMatrix4d U = new MyMatrix4d(R);
MyMatrix4d V = new MyMatrix4d(InvR);
MyMatrix4d I = MyMatrix4d.IdentityMatrix();
I[0, 0] = -1; // reflect matrix along yz Canvas3
MyMatrix4d T = V * I * U; // the reflection matrix
// reflect
MyVector4 r = new MyVector4(u, 0);
MyVector3 q = Canvas3_center + (T * r).XYZ();
return(q);
}
public MyMatrix3d InverseSVD()
{
SVD svd = new SVD(e, 3, 3);
MyMatrix3d inv = new MyMatrix3d(svd.Inverse);
lastSVDIsFullRank = svd.FullRank;
return(inv);
}
public MyMatrix4d(MyMatrix3d m)
{
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
this[i, j] = m[i, j];
}
}
}
public static MyMatrix3d operator /(MyMatrix3d m, double d)
{
MyMatrix3d ret = new MyMatrix3d();
for (int i = 0; i < len; i++)
{
ret[i] = m[i] / d;
}
return(ret);
}
public static MyMatrix3d operator -(MyMatrix3d m1, MyMatrix3d m2)
{
MyMatrix3d ret = new MyMatrix3d();
for (int i = 0; i < len; i++)
{
ret[i] = m1[i] - m2[i];
}
return(ret);
}
public MyMatrix3d Transpose()
{
MyMatrix3d m = new MyMatrix3d();
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 MyMatrix3d ToMyMatrix3d()
{
MyMatrix3d ret = new MyMatrix3d();
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 MyMatrix3d OrthogonalFactor(double eps)
{
MyMatrix3d Q = new MyMatrix3d(this);
MyMatrix3d Q2 = new MyMatrix3d();
double err = 0;
do
{
Q2 = (Q + Q.InverseTranspose()) / 2.0;
err = (Q2 - Q).SqNorm();
Q = Q2;
} while (err > eps);
return(Q2);
}
public MyMatrix3d OuterCross(MyVector3 v)
{
MyMatrix3d m = new MyMatrix3d();
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 MyMatrix3d ToMyMatrix3d()
{
MyMatrix3d ret = new MyMatrix3d();
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 static MyMatrix3d operator *(MyMatrix3d m1, MyMatrix3d m2)
{
MyMatrix3d ret = new MyMatrix3d();
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 static MyVector3 RotateAroundAxis(double angle, MyVector3 v, MyVector3 a)
{
double s = Math.Sin(angle);
double c = Math.Cos(angle);
MyMatrix3d m = new MyMatrix3d();
m[0, 0] = a.x * a.x * (1 - c) + c;
m[0, 1] = a.x * a.y * (1 - c) + a.z * s;
m[0, 2] = a.x * a.z * (1 - c) - a.y * s;
m[1, 0] = a.x * a.y * (1 - c) - a.z * s;
m[1, 1] = a.y * a.y * (1 - c) + c;
m[1, 2] = a.y * a.z * (1 - c) + a.x * s;
m[2, 0] = a.x * a.z * (1 - c) + a.y * s;
m[2, 1] = a.y * a.z * (1 - c) - a.x * s;
m[2, 2] = a.z * a.z * (1 - c) + c;
return(m * v);
}
public Quaternion(MyMatrix3d 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 MyVector3(X, Y, Z);
}
public static MyMatrix3d QuatToMyMatrix3d(MyVector4 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;
MyMatrix3d m = new MyMatrix3d();
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 MyMatrix3d ToMyMatrix3d()
{
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;
MyMatrix3d m = new MyMatrix3d();
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 MyMatrix3d(MyMatrix3d m) : this(m.e)
{
}