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