public static Matrix4d operator *(Matrix4d m1, Matrix4d m2)
{
Matrix4d ret = new Matrix4d();
for (int i = 0; i < row_size; i++)
{
for (int j = 0; j < row_size; j++)
{
ret[i, j] = 0.0f;
for (int k = 0; k < row_size; k++)
{
ret[i, j] += m1[i, k] * m2[k, j];
}
}
}
return(ret);
}
public static void FindRotAxisAngle(Matrix4d rotMat, out Vector3 rotAxis, out float angle)
{
float trace = rotMat[0, 0] + rotMat[1, 1] + rotMat[2, 2];
angle = Mathf.Acos((trace - 1) / 2);
if (rotMat[0, 1] > 0)
{
angle = -angle; /// this may be violated...
}
float sin2 = 2 * Mathf.Sin(angle);
float x = (rotMat[2, 1] - rotMat[1, 2]) / sin2;
float y = (rotMat[0, 2] - rotMat[2, 0]) / sin2;
float z = (rotMat[1, 0] - rotMat[0, 1]) / sin2;
rotAxis = new Vector3(x, y, z).normalized;
}
public Matrix4d OuterCross(Vector4d v)
{
Matrix4d m = new Matrix4d();
m[0, 0] = x * v.x;
m[0, 1] = x * v.y;
m[0, 2] = x * v.z;
m[0, 3] = x * v.w;
m[1, 0] = y * v.x;
m[1, 1] = y * v.y;
m[1, 2] = y * v.z;
m[1, 3] = y * v.w;
m[2, 0] = z * v.x;
m[2, 1] = z * v.y;
m[2, 2] = z * v.z;
m[2, 3] = z * v.w;
m[3, 0] = w * v.x;
m[3, 1] = w * v.y;
m[3, 2] = w * v.z;
m[3, 3] = w * v.w;
return(m);
}
public static Matrix4d OuterCross(Vector4 v1, Vector4 v2)
{
Matrix4d m = new Matrix4d();
m[0, 0] = v1.x * v2.x;
m[0, 1] = v1.x * v2.y;
m[0, 2] = v1.x * v2.z;
m[0, 3] = v1.x * v2.w;
m[1, 0] = v1.y * v2.x;
m[1, 1] = v1.y * v2.y;
m[1, 2] = v1.y * v2.z;
m[1, 3] = v1.y * v2.w;
m[2, 0] = v1.z * v2.x;
m[2, 1] = v1.z * v2.y;
m[2, 2] = v1.z * v2.z;
m[2, 3] = v1.z * v2.w;
m[3, 0] = v1.w * v2.x;
m[3, 1] = v1.w * v2.y;
m[3, 2] = v1.w * v2.z;
m[3, 3] = v1.w * v2.w;
return(m);
}
private Matrix4d QuatToMatrix4d(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;
Matrix4d m = new Matrix4d();
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);
m[3, 3] = 1.0;
return(m);
}
public Matrix4d GetMatrix()
{
if (type == MotionType.Rotation)
{
return(QuatToMatrix4d(quat));
}
if (type == MotionType.Scale)
{
Matrix4d m = Matrix4d.IdentityMatrix();
m[0, 0] = m[1, 1] = m[2, 2] = 1.0 + (edPt.x - stPt.x) * adjustWidth;
return(m);
}
if (type == MotionType.Pan)
{
Matrix4d m = Matrix4d.IdentityMatrix();
m[0, 3] = edPt.x - stPt.x;
m[1, 3] = edPt.y - stPt.y;
return(m);
}
return(Matrix4d.IdentityMatrix());
}
public static Vector3 GetTranslation(Matrix4d T)
{
return(new Vector3(T[0, 3], T[1, 3], T[2, 3]));
}
public Matrix4d(Matrix4d m) : this(m.e)
{
}