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