/// <summary>
/// Calculates the inverse matrix using Householder transformations
/// </summary>
public static M44d QrInverse(this M44d mat)
{
var qr = new Matrix <double>((double[])mat, 4, 4);
var diag = qr.QrFactorize();
return((M44d)(qr.QrInverse(diag).Data));
}
/// <summary>
/// Transforms a <see cref="M34d"/> to a <see cref="M33d"/> by deleting the
/// specified row and column. Internally the <see cref="M34d"/> is tarnsformed
/// to a <see cref="M44d"/> to delete the row and column.
/// </summary>
/// <param name="deleted_row">Row to delete.</param>
/// <param name="deleted_column">Column to delete.</param>
/// <returns>A <see cref="M33d"/>.</returns>
public M33d Minor(int deleted_row, int deleted_column)
{
M44d temp = (M44d)this;
M33d result = new M33d();
int checked_row = 0;
for (int actual_row = 0; actual_row < 4; actual_row++)
{
int checked_column = 0;
if (actual_row != deleted_row)
{
for (int actual_column = 0; actual_column < 4; actual_column++)
{
if (actual_column != deleted_column)
{
result[checked_row, checked_column] = temp[actual_row, actual_column];
checked_column++;
}
}
checked_row++;
}
}
return(result);
}
public static Trafo3d FromNormalFrame(V3d origin, V3d normal)
{
M44d forward, backward;
M44d.NormalFrame(origin, normal, out forward, out backward);
return(new Trafo3d(forward, backward));
}
/// <summary>
/// 2D plane space to 3D world space.
/// Plane space is defined by a normal-frame from Point and Normal of the plane.
/// </summary>
public static M44d GetPlaneToWorld(this Plane3d self)
{
M44d local2global, _;
M44d.NormalFrame(self.Point, self.Normal, out local2global, out _);
return(local2global);
}
/// <summary>
/// 3D world space to 2D plane space.
/// Plane space is defined by a normal-frame from Point and Normal of the plane.
/// </summary>
public static M44d GetWorldToPlane(this Plane3d self)
{
M44d _, global2local;
M44d.NormalFrame(self.Point, self.Normal, out _, out global2local);
return(global2local);
}
public static void TransformPosArray(this M44d mat, V3d[] points)
{
for (int i = 0; i < points.Length; i++)
{
points[i] = mat.TransformPos(points[i]);
}
}
public Similarity3d(M44d m, double epsilon = (double)0.00001)
{
if (!(m.M30.IsTiny(epsilon) && m.M31.IsTiny(epsilon) && m.M32.IsTiny(epsilon)))
{
throw new ArgumentException("Matrix contains perspective components.");
}
if (m.M33.IsTiny(epsilon))
{
throw new ArgumentException("Matrix is not homogeneous.");
}
m /= m.M33; //normalize it
var m33 = (M33d)m;
var s0 = m33.C0.Norm2;
var s1 = m33.C1.Norm2;
var s2 = m33.C2.Norm2;
var s = (s0 * s1 * s2).Pow((double)1.0 / 3); //geometric mean of scale
if (!((s0 / s - 1).IsTiny(epsilon) && (s1 / s - 1).IsTiny(epsilon) && (s2 / s - 1).IsTiny(epsilon)))
{
throw new ArgumentException("Matrix features non-uniform scaling");
}
m33 /= s;
Scale = s;
EuclideanTransformation = new Euclidean3d(m33, m.C3.XYZ);
}
/// <summary>
/// Returns the handedness of the given transformation matrix that is assumed to be row-major.
/// A right-handed coodinate system is given when
/// (X cross Y) dot Z is positive,
/// otherwise left-handed.
/// </summary>
public static CoordinateSystem.Handedness Handedness(this M44d mat)
{
var x = mat.R0.XYZ;
var y = mat.R1.XYZ;
var z = mat.R2.XYZ;
return(x.Cross(y).Dot(z) > 0 ? CoordinateSystem.Handedness.Right : CoordinateSystem.Handedness.Left);
}
public static Trafo3d Parse(string s)
{
var x = s.NestedBracketSplitLevelOne().ToArray();
return(new Trafo3d(
M44d.Parse(x[0].ToString()),
M44d.Parse(x[1].ToString())
));
}
public static M44d Add(M34d a, M44d b)
{
return(new M44d(
a.M00 + b.M00, a.M01 + b.M01, a.M02 + b.M02, a.M03 + b.M03,
a.M10 + b.M10, a.M11 + b.M11, a.M12 + b.M12, a.M13 + b.M13,
a.M20 + b.M20, a.M21 + b.M21, a.M22 + b.M22, a.M23 + b.M23,
b.M30, b.M31, b.M32, 1 + b.M33
));
}
public static M44d Subtract(M44d a, M34d b)
{
return(new M44d(
a.M00 - b.M00, a.M01 - b.M01, a.M02 - b.M02, a.M03 - b.M03,
a.M10 - b.M10, a.M11 - b.M11, a.M12 - b.M12, a.M13 - b.M13,
a.M20 - b.M20, a.M21 - b.M21, a.M22 - b.M22, a.M23 - b.M23,
a.M30, a.M31, a.M32, a.M33 - 1
));
}
//public static Quadric operator -(Quadric lhs, Quadric rhs)
//{
// Quadric result = new Quadric();
// result.ErrorQuadric = lhs.ErrorQuadric - rhs.ErrorQuadric;
// result.ErrorHeuristic = Quadric.ToHeuristic(result.ErrorQuadric);
// return result;
//}
#endregion
#region Static Methods
static M44d ToHeuristic(M44d quadric)
{
var result = new M44d();
result = quadric;
result.R3 = new V4d(0, 0, 0, 1);
return(result);
}
public void CreateHeuristic()
{
if (m_errorQuadric == M44d.Zero)
{
throw new InvalidOperationException("Must call CreateQuadric(...) first");
}
ErrorHeuristic = ToHeuristic(ErrorQuadric);
}
/// <summary>
/// Computes from a <see cref="V3d"/> point (origin) and
/// a <see cref="V3d"/> normal the transformation matrix
/// and its inverse.
/// </summary>
/// <param name="origin">The point which will become the new origin.</param>
/// <param name="normal">The normal vector of the new ground plane.</param>
/// <param name="local2global">A <see cref="M44d"/>The trafo from local to global system.</param>
/// <param name="global2local">A <see cref="M44d"/>The trafofrom global to local system.</param>
public static void NormalFrame(V3d origin, V3d normal,
out M44d local2global, out M44d global2local
)
{
V3d min;
double x = Fun.Abs(normal.X);
double y = Fun.Abs(normal.Y);
double z = Fun.Abs(normal.Z);
if (x < y)
{
if (x < z)
{
min = V3d.XAxis;
}
else
{
min = V3d.ZAxis;
}
}
else
{
if (y < z)
{
min = V3d.YAxis;
}
else
{
min = V3d.ZAxis;
}
}
V3d xVec = Vec.Cross(normal, min);
xVec.Normalize(); // this is now guaranteed to be normal to the input normal
V3d yVec = Vec.Cross(normal, xVec);
yVec.Normalize();
V3d zVec = normal;
zVec.Normalize();
local2global = new M44d(xVec.X, yVec.X, zVec.X, origin.X,
xVec.Y, yVec.Y, zVec.Y, origin.Y,
xVec.Z, yVec.Z, zVec.Z, origin.Z,
0, 0, 0, 1);
M44d mat = new M44d(xVec.X, xVec.Y, xVec.Z, 0,
yVec.X, yVec.Y, yVec.Z, 0,
zVec.X, zVec.Y, zVec.Z, 0,
0, 0, 0, 1);
var shift = M44d.Translation(-origin);
global2local = mat * shift;
}
public static M44d Enlarge(M33d m)
{
M44d enlarged = new M44d(
m.M00, m.M01, m.M02, 0,
m.M10, m.M11, m.M12, 0,
m.M20, m.M21, m.M22, 0,
0, 0, 0, 1.0f);
return(enlarged);
}
/// <summary>
/// Returns the trafo that transforms from the coordinate system
/// specified by the basis into the world coordinate system.
/// </summary>
public static Trafo3d FromBasis(V3d xAxis, V3d yAxis, V3d zAxis, V3d orign)
{
var mat = new M44d(
xAxis.X, yAxis.X, zAxis.X, orign.X,
xAxis.Y, yAxis.Y, zAxis.Y, orign.Y,
xAxis.Z, yAxis.Z, zAxis.Z, orign.Z,
0, 0, 0, 1);
return(new Trafo3d(mat, mat.Inverse));
}
public static V3d[] TransformedDirArray(this M44d mat, V3d[] directions)
{
var result = new V3d[directions.Length];
for (int i = 0; i < directions.Length; i++)
{
result[i] = mat.TransformDir(directions[i]);
}
return(result);
}
public static V3d[] TransformedPosArray(this M44d mat, ICollection <V3d> points)
{
var result = new V3d[points.Count];
int i = 0;
foreach (var p in points)
{
result[i++] = mat.TransformPos(p);
}
return(result);
}
/// <summary>
/// Creates a rigid transformation from a matrix <paramref name="m"/>.
/// </summary>
public Euclidean3d(M44d m, double epsilon = 1e-12)
: this(((M33d)m) / m.M33, m.C3.XYZ / m.M33, epsilon)
{
if (!(m.M30.IsTiny(epsilon) && m.M31.IsTiny(epsilon) && m.M32.IsTiny(epsilon)))
{
throw new ArgumentException("Matrix contains perspective components.");
}
if (m.M33.IsTiny(epsilon))
{
throw new ArgumentException("Matrix is not homogeneous.");
}
}
/// <summary>
/// Copies from the position array indexed by a backward map into
/// a target array, starting at the supplied offset, thereby
/// transforming all positions using the supplied matrix.
/// </summary>
/// <returns>target array</returns>
public static V3f[] BackwardIndexedTransformPosAndCopyTo(
this V3f[] source,
V3f[] target,
int[] backwardMap,
int offset,
M44d m44d)
{
var count = backwardMap.Length;
for (int i = 0; i < count; i++)
{
target[i + offset] = (V3f)m44d.TransformPos((V3d)source[backwardMap[i]]);
}
return(target);
}
public static List <int[]> ComputeNonConcaveSubPolygons(
this Polygon3d polygon, double absoluteEpsilon)
{
V3d normal = polygon.ComputeDoubleAreaNormal();
double len2 = normal.LengthSquared;
if (len2 < absoluteEpsilon * absoluteEpsilon)
{
return(new int[polygon.PointCount].SetByIndex(i => i).IntoList());
}
M44d.NormalFrame(V3d.Zero, normal * (1.0 / Fun.Sqrt(len2)), out M44d local2global, out M44d global2local);
var polygon2d = polygon.ToPolygon2d(p => global2local.TransformPos(p).XY);
return(polygon2d.ComputeNonConcaveSubPolygons(absoluteEpsilon));
}
/// <summary>
/// Builds a hull from the given view-projection transformation (left, right, bottom, top, near, far).
/// The view volume is assumed to be [-1, -1, -1] [1, 1, 1].
/// The normals of the hull planes point to the outside and are normalized.
/// A point inside the visual hull will has negative height to all planes.
/// </summary>
public static Hull3d GetVisualHull(this M44d viewProj)
{
var r0 = viewProj.R0;
var r1 = viewProj.R1;
var r2 = viewProj.R2;
var r3 = viewProj.R3;
return(new Hull3d(new[]
{
new Plane3d(-(r3 + r0)).Normalized, // left
new Plane3d(-(r3 - r0)).Normalized, // right
new Plane3d(-(r3 + r1)).Normalized, // bottom
new Plane3d(-(r3 - r1)).Normalized, // top
new Plane3d(-(r3 + r2)).Normalized, // near
new Plane3d(-(r3 - r2)).Normalized, // far
}));
}
/// <summary>
/// Computes a Coordiante Frame Transformation (Basis) from current CS into
/// the (X, Y, Z)-System at a given Origin.
/// Note: you can use it, to transform from RH to LH and vice-versa, all depending
/// how you will specifie your new basis-vectors.
/// </summary>
/// <param name="xVec">New X Vector</param>
/// <param name="yVec">New Y Vector</param>
/// <param name="zVec">New Z vector</param>
/// <param name="oVec">New Origin.</param>
/// <param name="viewTrafo"></param>
/// <param name="viewTrafoInverse"></param>
public static void CoordinateFrameTransform(V3d xVec, V3d yVec, V3d zVec, V3d oVec,
out M44d viewTrafo, out M44d viewTrafoInverse)
{
oVec = -oVec;
viewTrafo = new M44d(
xVec.X, xVec.Y, xVec.Z, xVec.X * oVec.X + xVec.Y * oVec.Y + xVec.Z * oVec.Z,
yVec.X, yVec.Y, yVec.Z, yVec.X * oVec.X + yVec.Y * oVec.Y + yVec.Z * oVec.Z,
zVec.X, zVec.Y, zVec.Z, zVec.X * oVec.X + zVec.Y * oVec.Y + zVec.Z * oVec.Z,
0, 0, 0, 1
);
viewTrafoInverse = new M44d(
xVec.X, yVec.X, zVec.X, -oVec.X,
xVec.Y, yVec.Y, zVec.Y, -oVec.Y,
xVec.Z, yVec.Z, zVec.Z, -oVec.Z,
0, 0, 0, 1
);
}
public static List <int[]> ComputeNonConcaveSubPolygons(
this V3d[] vertexArray, int polyCount,
V3d[] normalArray, int[] firstIndices, int[] vertexIndices,
List <int> faceBackMap,
double absoluteEpsilon)
{
var polyList = new List <int[]>();
var eps2 = absoluteEpsilon * absoluteEpsilon;
for (int fvi = 0, fi = 0; fi < polyCount; fi++)
{
int fve = firstIndices[fi + 1], fvc = fve - fvi;
var n = normalArray[fi];
var l2 = n.LengthSquared;
if (l2 < eps2)
{
polyList.Add(new int[fvc].SetByIndex(i => vertexIndices[fvi + i]));
if (faceBackMap != null)
{
faceBackMap.Add(fi);
}
fvi = fve;
continue;
}
M44d local2global, global2local;
M44d.NormalFrame(V3d.Zero, n, out local2global, out global2local);
var polygon = new Polygon2d(fvc,
i => global2local.TransformPos(vertexArray[vertexIndices[fvi + i]]).XY);
var subPolyList = polygon.ComputeNonConcaveSubPolygons(absoluteEpsilon);
foreach (var poly in subPolyList)
{
polyList.Add(poly.Map(i => fvi + i));
}
if (faceBackMap != null)
{
for (int i = 0; i < subPolyList.Count; i++)
{
faceBackMap.Add(fi);
}
}
fvi = fve;
}
return(polyList);
}
public static M44d Multiply(Rot2d rot, M44d mat)
{
double a = (double)System.Math.Cos(rot.Angle);
double b = (double)System.Math.Sin(rot.Angle);
return(new M44d(a * mat.M00 +
b * mat.M10,
a * mat.M01 +
b * mat.M11,
a * mat.M02 +
b * mat.M12,
a * mat.M03 +
b * mat.M13,
-b * mat.M00 +
a * mat.M10,
-b * mat.M01 +
a * mat.M11,
-b * mat.M02 +
a * mat.M12,
-b * mat.M03 +
a * mat.M13,
mat.M20,
mat.M21,
mat.M22,
mat.M23,
mat.M30,
mat.M31,
mat.M32,
mat.M33));
}
/// <summary>
/// Provides perspective projection matrix in terms of the vertical field of view angle a and the aspect ratio r.
/// </summary>
public static M44d PerspectiveProjectionTransformRH(double a, double r, double n, double f)
{
//F / r 0 0 0
// 0 F 0 0
// 0 0 A B
// 0 0 -1 0
double F = 1 / Fun.Tan(a / 2);
double A = f / (n - f);
double B = f * n / (n - f);
M44d P = new M44d(
F / r, 0, 0, 0,
0, F, 0, 0,
0, 0, A, B,
0, 0, -1, 0);
return(P);
}
/// <summary>
/// Provides perspective projection matrix.
/// The parameters describe the dimensions of the view volume.
/// </summary>
public static M44d PerspectiveProjectionTransformRH(V2d size, double n, double f)
{
double w = size.X;
double h = size.Y;
// Fx 0 0 0
// 0 Fy 0 0
// 0 0 A B
// 0 0 -1 0
double Fx = 2 * n / w;
double Fy = 2 * n / h;
double A = f / (n - f);
double B = n * f / (n - f);
M44d P = new M44d(
Fx, 0, 0, 0,
0, Fy, 0, 0,
0, 0, A, B,
0, 0, -1, 0);
return(P);
}
public static Array BackwardIndexedTransformPosAndCopyTo(
this Array source, Array target,
int[] backwardMap, int offset,
M44d m44d)
{
Type type = source.GetType();
if (type == typeof(V3f[]))
{
return(BackwardIndexedTransformPosAndCopyTo((V3f[])source, (V3f[])target,
backwardMap, offset, m44d));
}
if (type == typeof(V3d[]))
{
return(BackwardIndexedTransformPosAndCopyTo((V3d[])source, (V3d[])target,
backwardMap, offset, m44d));
}
throw new InvalidOperationException();
}
/// <summary>
/// Builds a customized, left-handed perspective Off-Center projection matrix.
/// </summary>
public static M44d PerspectiveProjectionTransformLH(double l, double r, double t, double b, double n, double f)
{
// Fx 0 0 0
// 0 Fy 0 0
// Sx Sy A 1
// 0 0 B 0
double Fx = 2 * n / (r - l);
double Fy = 2 * n / (t - b);
double Sx = (l + r) / (l - r);
double Sy = (t + b) / (b - t);
double A = f / (f - n);
double B = n * f / (n - f);
M44d P = new M44d(
Fx, 0, 0, 0,
0, Fy, 0, 0,
Sx, Sy, A, 1,
0, 0, B, 0);
return(P);
}
/// <summary>
/// Builds a customized, right-handed perspective Off-Center projection matrix.
/// </summary>
public static M44d PerspectiveProjectionTransformRH(double l, double r, double t, double b, double n, double f)
{
// Fx 0 Sx 0
// 0 Fy Sy 0
// 0 0 A B
// 0 0 -1 0
double Fx = 2 * n / (r - l);
double Fy = 2 * n / (t - b);
double Sx = (l + r) / (r - l);
double Sy = (t + b) / (t - b);
double A = f / (n - f);
double B = n * f / (n - f);
M44d P = new M44d(
Fx, 0, Sx, 0,
0, Fy, Sy, 0,
0, 0, A, B,
0, 0, -1, 0);
return(P);
}