static public Plane3 <T> MultiplyByMatrix(Plane3 <T> plane, Matrix4d4 <T> m, T unit) { Vector3d <T> dir1 = new Vector3d <T>(unit, Numeric <T> .Zero(), Numeric <T> .Zero()) % plane.normal; Numeric <T> dir1Len = dir1 ^ dir1; Vector3d <T> tmp = new Vector3d <T>(Numeric <T> .Zero(), unit, Numeric <T> .Zero()) % plane.normal; Numeric <T> tmpLen = tmp ^ tmp; if (tmpLen > dir1Len) { dir1 = tmp; dir1Len = tmpLen; } tmp = new Vector3d <T>(Numeric <T> .Zero(), Numeric <T> .Zero(), unit) % plane.normal; tmpLen = tmp ^ tmp; if (tmpLen > dir1Len) { dir1 = tmp; } Vector3d <T> dir2 = dir1 % plane.normal; Vector3d <T> point = plane.distance * plane.normal; return(new Plane3 <T>(point * m, (point + dir2) * m, (point + dir1) * m, unit)); }
public Matrix4d4(Matrix4d4 <T> v) { x = new Numeric <T> [4][]; for (int i = 0; i < 4; i++) { x[i] = new Numeric <T> [4]; } x[0][0] = v.x[0][0]; x[0][1] = v.x[0][1]; x[0][2] = v.x[0][2]; x[0][3] = v.x[0][3]; x[1][0] = v.x[1][0]; x[1][1] = v.x[1][1]; x[1][2] = v.x[1][2]; x[1][3] = v.x[1][3]; x[2][0] = v.x[2][0]; x[2][1] = v.x[2][1]; x[2][2] = v.x[2][2]; x[2][3] = v.x[2][3]; x[3][0] = v.x[3][0]; x[3][1] = v.x[3][1]; x[3][2] = v.x[3][2]; x[3][3] = v.x[3][3]; }
//public IntersectionResult<Vector3d<T>> Intersect(Line3<T> line) //{ // Numeric<T> d = normal ^ line.Dir; // if (d.Equals(Numeric<T>.Zero())) // return new IntersectionResult<Vector3d<T>>(false); // T t = -((normal ^ line.From) - distance) / d; // Vector3d<T> point = line.GetPoint(t); // return new IntersectionResult<Vector3d<T>>(point, true); //} //public IntersectionResult<T> IntersectT(Line3<T> line) //{ // Numeric<T> d = normal ^ line.Dir; // if (d.Equals(Numeric<T>.Zero())) // return new IntersectionResult<T>(false); // T t = -((normal ^ line.From) - distance) / d; // return new IntersectionResult<T>(t, true); //} public void MultiplyByMatrix(Matrix4d4 <T> m, T unit) { Vector3d <T> dir1 = new Vector3d <T>(unit, Numeric <T> .Zero(), Numeric <T> .Zero()) % normal; Numeric <T> dir1Len = dir1 ^ dir1; Vector3d <T> tmp = new Vector3d <T>(Numeric <T> .Zero(), unit, Numeric <T> .Zero()) % normal; Numeric <T> tmpLen = tmp ^ tmp; if (tmpLen > dir1Len) { dir1 = tmp; dir1Len = tmpLen; } tmp = new Vector3d <T>(Numeric <T> .Zero(), Numeric <T> .Zero(), unit) % normal; tmpLen = tmp ^ tmp; if (tmpLen > dir1Len) { dir1 = tmp; } Vector3d <T> dir2 = dir1 % normal; Vector3d <T> point = distance * normal; this = new Plane3 <T>(point * m, (point + dir2) * m, (point + dir1) * m, unit); }
/// <summary> /// Matrix multiplication /// </summary> /// <param name="v"></param> /// <returns></returns> public static Matrix4d4 <T> operator *(Matrix4d4 <T> v1, Matrix4d4 <T> v2) { Matrix4d4 <T> tmp = new Matrix4d4 <T>(Numeric <T> .Zero()); Matrix4d4 <T> .Multiply(v1, v2, ref tmp); return(tmp); }
public static Matrix4d4 <T> Identity(T unit) { Matrix4d4 <T> m = new Matrix4d4 <T>(Numeric <T> .Zero()); m.MakeIdentity(unit); return(m); }
public override bool Equals(object obj) { if (obj != null && obj is Matrix4d4 <T> ) { Matrix4d4 <T> other = (Matrix4d4 <T>)obj; return(Equals(ref this, ref other)); } return(base.Equals(obj)); }
public Matrix4d4 <T> Shear(Shear6 <T> h) { Matrix4d4 <T> P = new Matrix4d4 <T>(this); for (int i = 0; i < 4; i++) { x[0][i] = P[0][i] + h.YX * P[1][i] + h.ZX * P[2][i]; x[1][i] = h.XY * P[0][i] + P[1][i] + h.ZY * P[2][i]; x[2][i] = h.XZ * P[0][i] + h.YZ * P[1][i] + P[2][i]; } return(this); }
public Matrix4d4 <V> Select <V>(Func <T, V> transf) where V : IEquatable <V> { Matrix4d4 <V> transfMatrix = new Matrix4d4 <V>(Numeric <V> .Zero()); for (int i = 0; i < 4; ++i) { for (int j = 0; j < 4; ++j) { transfMatrix[i][j] = transf(x[i][j]); } } return(transfMatrix); }
/// <summary> /// Set matrix to rotation by angle /// </summary> /// <param name="angle">angle of rotation in radians</param> /// <returns></returns> public Matrix4d4 <T> Rotate(Vector3d <T> angle, T unit) { Numeric <T> cosRZ, sinRZ, cosRY, sinRY, cosRX, sinRX; Numeric <T> m00, m01, m02; Numeric <T> m10, m11, m12; Numeric <T> m20, m21, m22; T rx = angle.X; T ry = angle.Y; T rz = angle.Z; cosRZ = (GenericMath.Cos(rz, unit)); cosRY = (GenericMath.Cos(ry, unit)); cosRX = (GenericMath.Cos(rx, unit)); sinRZ = (GenericMath.Sin(rz, unit)); sinRY = (GenericMath.Sin(ry, unit)); sinRX = (GenericMath.Sin(rx, unit)); m00 = cosRZ * cosRY; m01 = sinRZ * cosRY; m02 = -sinRY; m10 = -sinRZ * cosRX + cosRZ * sinRY * sinRX; m11 = cosRZ * cosRX + sinRZ * sinRY * sinRX; m12 = cosRY * sinRX; m20 = -sinRZ * -sinRX + cosRZ * sinRY * cosRX; m21 = cosRZ * -sinRX + sinRZ * sinRY * cosRX; m22 = cosRY * cosRX; Matrix4d4 <T> P = new Matrix4d4 <T>(this); x[0][0] = P[0][0] * m00 + P[1][0] * m01 + P[2][0] * m02; x[0][1] = P[0][1] * m00 + P[1][1] * m01 + P[2][1] * m02; x[0][2] = P[0][2] * m00 + P[1][2] * m01 + P[2][2] * m02; x[0][3] = P[0][3] * m00 + P[1][3] * m01 + P[2][3] * m02; x[1][0] = P[0][0] * m10 + P[1][0] * m11 + P[2][0] * m12; x[1][1] = P[0][1] * m10 + P[1][1] * m11 + P[2][1] * m12; x[1][2] = P[0][2] * m10 + P[1][2] * m11 + P[2][2] * m12; x[1][3] = P[0][3] * m10 + P[1][3] * m11 + P[2][3] * m12; x[2][0] = P[0][0] * m20 + P[1][0] * m21 + P[2][0] * m22; x[2][1] = P[0][1] * m20 + P[1][1] * m21 + P[2][1] * m22; x[2][2] = P[0][2] * m20 + P[1][2] * m21 + P[2][2] * m22; x[2][3] = P[0][3] * m20 + P[1][3] * m21 + P[2][3] * m22; return(this); }
/// <summary> /// Matrix multiplication /// </summary> /// <param name="v"></param> /// <returns></returns> static public void Multiply(Matrix4d4 <T> a, Matrix4d4 <T> b, ref Matrix4d4 <T> c) { T a0, a1, a2, a3; a0 = a[0][0]; a1 = a[0][1]; a2 = a[0][2]; a3 = a[0][3]; c[0][0] = a0 * b[0][0] + a1 * b[1][0] + a2 * b[2][0] + a3 * b[3][0]; c[0][1] = a0 * b[0][1] + a1 * b[1][1] + a2 * b[2][1] + a3 * b[3][1]; c[0][2] = a0 * b[0][2] + a1 * b[1][2] + a2 * b[2][2] + a3 * b[3][2]; c[0][3] = a0 * b[0][3] + a1 * b[1][3] + a2 * b[2][3] + a3 * b[3][3]; a0 = a[1][0]; a1 = a[1][1]; a2 = a[1][2]; a3 = a[1][3]; c[1][0] = a0 * b[0][0] + a1 * b[1][0] + a2 * b[2][0] + a3 * b[3][0]; c[1][1] = a0 * b[0][1] + a1 * b[1][1] + a2 * b[2][1] + a3 * b[3][1]; c[1][2] = a0 * b[0][2] + a1 * b[1][2] + a2 * b[2][2] + a3 * b[3][2]; c[1][3] = a0 * b[0][3] + a1 * b[1][3] + a2 * b[2][3] + a3 * b[3][3]; a0 = a[2][0]; a1 = a[2][1]; a2 = a[2][2]; a3 = a[2][3]; c[2][0] = a0 * b[0][0] + a1 * b[1][0] + a2 * b[2][0] + a3 * b[3][0]; c[2][1] = a0 * b[0][1] + a1 * b[1][1] + a2 * b[2][1] + a3 * b[3][1]; c[2][2] = a0 * b[0][2] + a1 * b[1][2] + a2 * b[2][2] + a3 * b[3][2]; c[2][3] = a0 * b[0][3] + a1 * b[1][3] + a2 * b[2][3] + a3 * b[3][3]; a0 = a[3][0]; a1 = a[3][1]; a2 = a[3][2]; a3 = a[3][3]; c[3][0] = a0 * b[0][0] + a1 * b[1][0] + a2 * b[2][0] + a3 * b[3][0]; c[3][1] = a0 * b[0][1] + a1 * b[1][1] + a2 * b[2][1] + a3 * b[3][1]; c[3][2] = a0 * b[0][2] + a1 * b[1][2] + a2 * b[2][2] + a3 * b[3][2]; c[3][3] = a0 * b[0][3] + a1 * b[1][3] + a2 * b[2][3] + a3 * b[3][3]; }
private static bool Equals(ref Matrix4d4 <T> v1, ref Matrix4d4 <T> v2) { return(EqualityComparer <T> .Default.Equals(v1.x[0][0], v2.x[0][0]) && EqualityComparer <T> .Default.Equals(v1.x[0][1], v2.x[0][1]) && EqualityComparer <T> .Default.Equals(v1.x[0][2], v2.x[0][2]) && EqualityComparer <T> .Default.Equals(v1.x[0][3], v2.x[0][3]) && EqualityComparer <T> .Default.Equals(v1.x[1][0], v2.x[1][0]) && EqualityComparer <T> .Default.Equals(v1.x[1][1], v2.x[1][1]) && EqualityComparer <T> .Default.Equals(v1.x[1][2], v2.x[1][2]) && EqualityComparer <T> .Default.Equals(v1.x[1][3], v2.x[1][3]) && EqualityComparer <T> .Default.Equals(v1.x[2][0], v2.x[2][0]) && EqualityComparer <T> .Default.Equals(v1.x[2][1], v2.x[2][1]) && EqualityComparer <T> .Default.Equals(v1.x[2][2], v2.x[2][2]) && EqualityComparer <T> .Default.Equals(v1.x[2][3], v2.x[2][3]) && EqualityComparer <T> .Default.Equals(v1.x[3][0], v2.x[3][0]) && EqualityComparer <T> .Default.Equals(v1.x[3][1], v2.x[3][1]) && EqualityComparer <T> .Default.Equals(v1.x[3][2], v2.x[3][2]) && EqualityComparer <T> .Default.Equals(v1.x[3][3], v2.x[3][3])); }
Matrix4d4 <T> SetTheMatrix(Matrix4d4 <T> v) { x[0][0] = v.x[0][0]; x[0][1] = v.x[0][1]; x[0][2] = v.x[0][2]; x[0][3] = v.x[0][3]; x[1][0] = v.x[1][0]; x[1][1] = v.x[1][1]; x[1][2] = v.x[1][2]; x[1][3] = v.x[1][3]; x[2][0] = v.x[2][0]; x[2][1] = v.x[2][1]; x[2][2] = v.x[2][2]; x[2][3] = v.x[2][3]; x[3][0] = v.x[3][0]; x[3][1] = v.x[3][1]; x[3][2] = v.x[3][2]; x[3][3] = v.x[3][3]; return(this); }
/// <summary> /// Transpose the matrix /// </summary> /// <returns></returns> public Matrix4d4 <T> Transpose() { Matrix4d4 <T> tmp = new Matrix4d4 <T>(x[0][0], x[1][0], x[2][0], x[3][0], x[0][1], x[1][1], x[2][1], x[3][1], x[0][2], x[1][2], x[2][2], x[3][2], x[0][3], x[1][3], x[2][3], x[3][3]); this = tmp; return(this); }
/// <summary> /// Gauss–Jordan elimination /// </summary> /// <exception cref="MatrixNotInvertibleException">MatrixNotInvertibleException</exception> /// <returns></returns> public Matrix4d4 <T> Inverse(T unit) { int i, j, k; Matrix4d4 <T> s = Matrix4d4 <T> .Identity(unit); Matrix4d4 <T> t = new Matrix4d4 <T>(this); // Forward elimination for (i = 0; i < 3; i++) { int pivot = i; Numeric <T> pivotsize = (t[i][i]); if (pivotsize < Numeric <T> .Zero()) { pivotsize = -pivotsize; } for (j = i + 1; j < 4; j++) { Numeric <T> tmp = (t[j][i]); if (tmp < Numeric <T> .Zero()) { tmp = -tmp; } if (tmp > pivotsize) { pivot = j; pivotsize = tmp; } } if (pivotsize.Equals(Numeric <T> .Zero())) { throw new Exception("Cannot invert singular matrix."); } if (pivot != i) { for (j = 0; j < 4; j++) { T tmp; tmp = t[i][j]; t[i][j] = t[pivot][j]; t[pivot][j] = tmp; tmp = s[i][j]; s[i][j] = s[pivot][j]; s[pivot][j] = tmp; } } for (j = i + 1; j < 4; j++) { T f = t[j][i] / t[i][i]; for (k = 0; k < 4; k++) { t[j][k] -= f * t[i][k]; s[j][k] -= f * s[i][k]; } } } // Backward substitution for (i = 3; i >= 0; --i) { Numeric <T> f; if ((f = t[i][i]).Equals(Numeric <T> .Zero())) { throw new Exception("Cannot invert singular matrix."); } for (j = 0; j < 4; j++) { t[i][j] /= f; s[i][j] /= f; } for (j = 0; j < i; j++) { f = t[j][i]; for (k = 0; k < 4; k++) { t[j][k] -= f * t[i][k]; s[j][k] -= f * s[i][k]; } } } return(s); }
public bool Equals(Matrix4d4 <T> other) { return(Equals(this, other)); }
//public Vector3d<T> ClosestPointTo(Line3<T> line, Func<double, T> fromFloat = null, Func<T, double> magnitude = null) //{ // if (fromFloat == null) // fromFloat = Numeric<T>.FromFloat; // if (magnitude == null) // magnitude = Numeric<T>.ToFloat; // // Assumes the lines are normalized // Vector3d<T> posLpos = fromFloat - line.from; // Numeric<T> c = dir ^ posLpos; // Numeric<T> a = line.dir ^ dir; // Numeric<T> f = line.dir ^ posLpos; // Numeric<T> num = c - a * f; // Numeric<T> denom = a * a - fromFloat(1); // Numeric<T> absDenom = ((denom >= Numeric<T>.Zero()) ? denom : -denom); // if (absDenom < fromdouble(1)) // { // Numeric<T> absNum = ((num >= Numeric<T>.Zero()) ? num : -num); // if (magnitude(absNum) >= magnitude(absDenom) * double.MaxValue) // return fromFloat; // } // return fromFloat + dir * (num / denom); //} public void Multiply(Matrix4d4 <T> m, T unit) { this = new Line3 <T>(from * m, (from + dir) * m, unit); }