diff --git a/TessellationAndVoxelizationGeometryLibrary/Implicits and Primitives/Primitive Surfaces/GeneralQuadric.cs b/TessellationAndVoxelizationGeometryLibrary/Implicits and Primitives/Primitive Surfaces/GeneralQuadric.cs
index 70d02d1c..3f90f98e 100644
--- a/TessellationAndVoxelizationGeometryLibrary/Implicits and Primitives/Primitive Surfaces/GeneralQuadric.cs
+++ b/TessellationAndVoxelizationGeometryLibrary/Implicits and Primitives/Primitive Surfaces/GeneralQuadric.cs
@@ -315,30 +315,38 @@ public Vector3 GetPointNearQuadric(Vector3 anchor)
return intersections.MinBy(x => x.intersection.DistanceSquared(anchor)).intersection;
}
- public double DistanceToPointSQP(Vector3 point)
+ ///
+ /// Finds the closest signed distance between the given point and the quadric.
+ ///
+ ///
+ ///
+ public override double DistanceToPoint(Vector3 point)
{
- Vector3 startPoint = GetPointNearQuadric(point);
- double[] u = { startPoint.X, startPoint.Y, startPoint.Z, 0 };
- double[] du = { double.PositiveInfinity, double.PositiveInfinity, double.PositiveInfinity, double.PositiveInfinity };
- int iters = 0;
- while (iters < 1000 && (du[0] * du[0] + du[1] * du[1] + du[2] * du[2] + du[3] * du[3]) < 1E-6) {
- double[,] H = { { 2 + 2 * XSqdCoeff * u[3], XYCoeff * u[3], XZCoeff * u[3], 2 * XSqdCoeff * u[0] + XYCoeff * u[1] + XZCoeff * u[2] + XCoeff },
- { XYCoeff * u[3], 2 + 2 * YSqdCoeff * u[3], YZCoeff * u[3], 2 * YSqdCoeff * u[1] + XYCoeff * u[0] + YZCoeff * u[2] + YCoeff },
- { XZCoeff * u[3], YZCoeff * u[3], 2 + 2 * ZSqdCoeff * u[3], 2 * YSqdCoeff * u[2] + XZCoeff * u[0] + YZCoeff * u[1] + ZCoeff },
- { 2 * XSqdCoeff * u[0] + XYCoeff * u[1] + XZCoeff * u[2] + XCoeff, 2 * YSqdCoeff * u[1] + XYCoeff * u[0] + YZCoeff * u[2] + YCoeff, 2 * YSqdCoeff * u[2] + XZCoeff * u[0] + YZCoeff * u[1] + ZCoeff, 0} };
- double[] RHS = { 2 * (u[0] - point.X) + u[3] * (2 * XSqdCoeff * u[0] + XYCoeff * u[1] + XZCoeff * u[2] + XCoeff),
- 2 * (u[1] - point.Y) + 2 * YSqdCoeff * u[1] + XYCoeff * u[0] + YZCoeff * u[2] + YCoeff,
- 2 * (u[2] - point.Z) + 2 * YSqdCoeff * u[2] + XZCoeff * u[0] + YZCoeff * u[1] + ZCoeff,
- QuadricValue(new Vector3(u[..3]))};
- StarMath.solve(H, RHS, out du, true);
- u[0] += du[0];
- u[1] += du[1];
- u[2] += du[2];
- u[3] += du[3];
+ var closestPt = GetPointOnQuadric(point);
+ var closestPt4 = new Vector4(closestPt, 0); // the fourth number "w" here represents the Lagrange multiplier
+ var delta = Vector4.Zero;
+ int iterations = 0;
+ do // run a condensed SQP algorithm
+ {
+ var hessian = new Matrix4x4(
+ 2 + 2 * XSqdCoeff * closestPt4.W, XYCoeff * closestPt4.W, XZCoeff * closestPt4.W, 2 * XSqdCoeff * closestPt4.X + XYCoeff * closestPt4.Y + XZCoeff * closestPt4.Z + XCoeff,
+ XYCoeff * closestPt4.W, 2 + 2 * YSqdCoeff * closestPt4.W, YZCoeff * closestPt4.W, 2 * YSqdCoeff * closestPt4.Y + XYCoeff * closestPt4.X + YZCoeff * closestPt4.Z + YCoeff,
+ XZCoeff * closestPt4.W, YZCoeff * closestPt4.W, 2 + 2 * ZSqdCoeff * closestPt4.W, 2 * YSqdCoeff * closestPt4.Z + XZCoeff * closestPt4.X + YZCoeff * closestPt4.Y + ZCoeff,
+ 2 * XSqdCoeff * closestPt4.X + XYCoeff * closestPt4.Y + XZCoeff * closestPt4.Z + XCoeff, 2 * YSqdCoeff * closestPt4.Y + XYCoeff * closestPt4.X + YZCoeff * closestPt4.Z + YCoeff, 2 * YSqdCoeff * closestPt4.Z + XZCoeff * closestPt4.X + YZCoeff * closestPt4.Y + ZCoeff, 0);
+ var rhs = new Vector4(
+ 2 * (closestPt4.X - point.X) + closestPt4.W * (2 * XSqdCoeff * closestPt4.X + XYCoeff * closestPt4.Y + XZCoeff * closestPt4.Z + XCoeff),
+ 2 * (closestPt4.Y - point.Y) + 2 * YSqdCoeff * closestPt4.Y + XYCoeff * closestPt4.X + YZCoeff * closestPt4.Z + YCoeff,
+ 2 * (closestPt4.Z - point.Z) + 2 * YSqdCoeff * closestPt4.Z + XZCoeff * closestPt4.X + YZCoeff * closestPt4.Y + ZCoeff,
+ QuadricValue(closestPt));
+ delta = hessian.Solve(rhs);
+ // add the negative to the previous point to get an updated point
+ closestPt4 -= delta;
}
- Vector3 newPoint = new Vector3(u[..3]);
- return QuadricValue(point)/Math.Abs(QuadricValue(point)) * point.Distance(newPoint);
+ while (iterations++ < 1000 && delta.LengthSquared() > 1E-6); // while less than 1000 iterations or the delta is small
+ closestPt = closestPt4.ToVector3(false);
+ return Math.Sign(QuadricValue(point)) * point.Distance(closestPt);
}
+
protected override void CalculateIsPositive()
{
if (Faces == null || !Faces.Any()) return;
diff --git a/TessellationAndVoxelizationGeometryLibrary/MarchingCubes/MarchingCubes.Base.cs b/TessellationAndVoxelizationGeometryLibrary/MarchingCubes/MarchingCubes.Base.cs
index 35281854..f2fa2ef5 100644
--- a/TessellationAndVoxelizationGeometryLibrary/MarchingCubes/MarchingCubes.Base.cs
+++ b/TessellationAndVoxelizationGeometryLibrary/MarchingCubes/MarchingCubes.Base.cs
@@ -230,6 +230,20 @@ protected long getIdentifier(int x, int y, int z)
return (long)x + yMultiplier * y + zMultiplier * z;
}
+ ///
+ /// Extracts the x, y, and z indices from a given identifier.
+ ///
+ /// The identifier from which to extract the indices.
+ /// A tuple containing the x, y, and z indices derived from the identifier.
+ protected (int, int, int) getIndicesFromIdentifier(long identifier)
+ {
+ var z = (int)(identifier / zMultiplier);
+ identifier -= z * zMultiplier;
+ var y = (int)(identifier / yMultiplier);
+ var x = (int)(identifier - y * yMultiplier);
+ return (x, y, z);
+ }
+
///
/// Gets the value.
///
@@ -242,9 +256,9 @@ protected StoredValue GetValue(int x, int y, int z, long identifier)
{
if (valueDictionary.TryGetValue(identifier, out var prevValue))
{
- if (prevValue.NumTimesCalled < 7)
- prevValue.NumTimesCalled++;
- else valueDictionary.Remove(identifier);
+ //if (prevValue.NumTimesCalled < 7)
+ // prevValue.NumTimesCalled++;
+ //else valueDictionary.Remove(identifier);
return prevValue;
}
var newValue = new StoredValue
@@ -266,7 +280,7 @@ protected StoredValue GetValue(int x, int y, int z, long identifier)
/// Index of the x.
/// Index of the y.
/// Index of the z.
- protected void MakeFacesInCube(int xIndex, int yIndex, int zIndex)
+ protected virtual void MakeFacesInCube(int xIndex, int yIndex, int zIndex)
{
// first solve for the eight values at the vertices of the cubes. The "GetValue" function
// will either grab the value from the StoredValues or will invoke the "GetValueFromSolid"
diff --git a/TessellationAndVoxelizationGeometryLibrary/MarchingCubes/MarchingCubes.Implicit.cs b/TessellationAndVoxelizationGeometryLibrary/MarchingCubes/MarchingCubes.Implicit.cs
index 1909cad2..4c554567 100644
--- a/TessellationAndVoxelizationGeometryLibrary/MarchingCubes/MarchingCubes.Implicit.cs
+++ b/TessellationAndVoxelizationGeometryLibrary/MarchingCubes/MarchingCubes.Implicit.cs
@@ -13,6 +13,7 @@
// ***********************************************************************
using System;
using System.Collections.Generic;
+using System.Linq;
namespace TVGL
{
@@ -54,6 +55,7 @@ internal MarchingCubesImplicit(ImplicitSolid solid, double discretization)
/// ValueT.
protected override double GetValueFromSolid(int x, int y, int z)
{
+ //return 0;
return solid[
_xMin + x * gridToCoordinateFactor,
_yMin + y * gridToCoordinateFactor,
@@ -94,14 +96,16 @@ internal override TessellatedSolid Generate()
FindZPointFromXandY();
FindXPointFromYandZ();
FindYPointFromXandZ();
- for (var i = 0; i < numGridX - 1; i++)
- for (var j = 0; j < numGridY - 1; j++)
- for (var k = 0; k < numGridZ - 1; k++)
- MakeFacesInCube(i, j, k);
- var comments = new List(solid.Comments)
+
+ var ids = new HashSet(vertexDictionaries.SelectMany(vertexDictionary => vertexDictionary.Keys));
+ foreach (var id in ids)
{
- "tessellation (via marching cubes) of the voxelized solid, " + solid.Name
- };
+ var (xIndex, yIndex, zIndex) = getIndicesFromIdentifier(id);
+ if (xIndex + 1 != numGridX && yIndex + 1 != numGridY && zIndex + 1 != numGridZ)
+ MakeFacesInCube(xIndex, yIndex, zIndex);
+ }
+ //var comments = new List(solid.Comments
+ // .Concat("tessellation (via marching cubes) of the implicit solid, " + solid.Name));
for (int i = 0; i < faces.Count; i++)
faces[i].IndexInList = i;
if (faces.Count == 0)
@@ -111,10 +115,89 @@ internal override TessellatedSolid Generate()
//new[] { solid.SolidColor }, solid.Units, solid.Name + "TS", solid.FileName, comments, solid.Language);
}
+
+ ///
+ /// MakeFacesInCube is the main/difficult function in the Marching Cubes algorithm
+ ///
+ /// Index of the x.
+ /// Index of the y.
+ /// Index of the z.
+ protected override void MakeFacesInCube(int xIndex, int yIndex, int zIndex)
+ {
+ // first solve for the eight values at the vertices of the cubes. The "GetValue" function
+ // will either grab the value from the StoredValues or will invoke the "GetValueFromSolid"
+ // which is a necessary function of inherited classes. For each one of the eight that is
+ // inside the solid, the cubeType is updated to reflect this. Each of the eight bits in the
+ // byte will correspond to the "inside" or "outside" of the vertex.
+ int cubeType = 0;
+ var cube = new StoredValue[8];
+ //Find which vertices are inside of the surface and which are outside
+ for (var i = 0; i < 8; i++)
+ {
+ var thisX = xIndex + _unitOffsetTable[i][0];
+ var thisY = yIndex + _unitOffsetTable[i][1];
+ var thisZ = zIndex + _unitOffsetTable[i][2];
+ var id = getIdentifier((int)thisX, (int)thisY, (int)thisZ);
+ var v = cube[i] = GetValue((int)thisX, (int)thisY, (int)thisZ, id);
+ if (!IsInside(v.Value))
+ cubeType |= 1 << i;
+ }
+ // Based upon the cubeType, the CubeEdgeFlagsTable will tell us which of the 12 edges of the cube
+ // intersect with the surface of the solid
+ int edgeFlags = CubeEdgeFlagsTable[cubeType];
+
+ //If the cube is entirely inside or outside of the surface, then there will be no intersections
+ if (edgeFlags == 0) return;
+ var EdgeVertex = new Vertex[12];
+ //this loop creates or retrieves the vertices that are on the edges of the
+ //marching cube. These are stored in the EdgeVertexIndexTable
+ for (var i = 0; i < 12; i++)
+ {
+ //if there is an intersection on this edge
+ if ((edgeFlags & 1) != 0)
+ {
+ var direction = (int)directionTable[i] - 1;
+ var fromCorner = cube[EdgeCornerIndexTable[i][0]];
+ var toCorner = cube[EdgeCornerIndexTable[i][1]];
+ if (vertexDictionaries[direction].TryGetValue(fromCorner.ID, out var value))
+ EdgeVertex[i] = value;
+ else
+ {
+ //return;
+ var coord = new Vector3(
+ _xMin + fromCorner.X * gridToCoordinateFactor,
+ _yMin + fromCorner.Y * gridToCoordinateFactor,
+ _zMin + fromCorner.Z * gridToCoordinateFactor);
+ var offSetUnitVector = (direction == 0) ? Vector3.UnitX :
+ (direction == 1) ? Vector3.UnitY : Vector3.UnitZ;
+ double offset = GetOffset(fromCorner, toCorner, direction);
+ coord = coord + (offSetUnitVector * offset);
+ EdgeVertex[i] = new Vertex(coord);
+ vertexDictionaries[direction].Add(fromCorner.ID, EdgeVertex[i]);
+ }
+ }
+ edgeFlags >>= 1;
+ }
+ //now the triangular faces are created that connect the vertices identified above.
+ //based on the ones that were found. There can be up to five per cube
+ var faceVertices = new Vertex[3];
+ for (var i = 0; i < NumFacesTable[cubeType]; i++)
+ {
+ for (var j = 0; j < 3; j++)
+ {
+ var vertexIndex = FaceVertexIndicesTable[cubeType][3 * i + j];
+ faceVertices[j] = EdgeVertex[vertexIndex];
+ }
+ faces.Add(new TriangleFace(faceVertices));
+ }
+ }
+
+
+
private void FindZPointFromXandY()
{
- for (var i = 0; i < numGridX - 1; i++)
- for (var j = 0; j < numGridY - 1; j++)
+ for (var i = 0; i < numGridX; i++)
+ for (var j = 0; j < numGridY; j++)
{
var anchor = new Vector3(_xMin + i * gridToCoordinateFactor,
_yMin + j * gridToCoordinateFactor, 0.0);
@@ -211,7 +294,7 @@ private void FindZPointFromXandY()
hashID += zMultiplier;
valueDictionary.TryAdd(hashID, new StoredValue
{
- Value = posDir1 ? 1 : -1,
+ Value = posDir2 ? 1 : -1,
X = i,
Y = j,
Z = zLowIndex2 + 1,
@@ -222,11 +305,229 @@ private void FindZPointFromXandY()
}
}
}
+
private void FindYPointFromXandZ()
{
+ for (var i = 0; i < numGridX; i++)
+ for (var k = 0; k < numGridZ; k++)
+ {
+ var anchor = new Vector3(_xMin + i * gridToCoordinateFactor,
+ 0.0, _zMin + k * gridToCoordinateFactor);
+ var intersectionEnumerator = surface.LineIntersection(anchor, Vector3.UnitY).GetEnumerator();
+ if (!intersectionEnumerator.MoveNext()) continue;
+ (Vector3 p1, _) = intersectionEnumerator.Current;
+ if (p1.Y < _yMin || p1.Y > _yMax)
+ {
+ if (!intersectionEnumerator.MoveNext()) continue;
+ (p1, _) = intersectionEnumerator.Current;
+ if (p1.Y < _yMin || p1.Y > _yMax)
+ continue;
+ }
+ var posDir1 = surface.GetNormalAtPoint(p1).Y > 0;
+ var yLowIndex1 = (int)((p1.Y - _yMin) * coordToGridFactor);
+ var p2 = Vector3.Null;
+ var hashID = getIdentifier(i, yLowIndex1, k);
+ if (intersectionEnumerator.MoveNext())
+ {
+ (p2, _) = intersectionEnumerator.Current;
+ if (p2.Y < _yMin || p2.Y > _yMax)
+ p2 = Vector3.Null;
+ }
+ if (p2.IsNull())
+ {
+ vertexDictionaries[1].Add(hashID, new Vertex(p1));
+ valueDictionary.TryAdd(hashID, new StoredValue
+ {
+ Value = posDir1 ? -1 : 1,
+ X = i,
+ Y = yLowIndex1,
+ Z = k,
+ NumTimesCalled = 0,
+ ID = hashID
+ });
+ if (yLowIndex1 != numGridY - 1)
+ {
+ hashID += yMultiplier;
+ valueDictionary.TryAdd(hashID, new StoredValue
+ {
+ Value = posDir1 ? 1 : -1,
+ X = i,
+ Y = yLowIndex1 + 1,
+ Z = k,
+ NumTimesCalled = 0,
+ ID = hashID
+ });
+ }
+ }
+ else
+ {
+ var posDir2 = surface.GetNormalAtPoint(p2).Y > 0;
+ var yLowIndex2 = (int)((p2.Y - _yMin) * coordToGridFactor);
+ if (yLowIndex1 == yLowIndex2 || (posDir1 == posDir2 &&
+ (yLowIndex1 + 1 == yLowIndex2 || yLowIndex1 == yLowIndex2 + 1)))
+ continue; // can't have two vertices at the same y level or adjacent y levels
+
+ vertexDictionaries[1].Add(hashID, new Vertex(p1));
+ valueDictionary.TryAdd(hashID, new StoredValue
+ {
+ Value = posDir1 ? -1 : 1,
+ X = i,
+ Y = yLowIndex1,
+ Z = k,
+ NumTimesCalled = 0,
+ ID = hashID
+ });
+ if (yLowIndex1 != numGridY - 1)
+ {
+ hashID += yMultiplier;
+ valueDictionary.TryAdd(hashID, new StoredValue
+ {
+ Value = posDir1 ? 1 : -1,
+ X = i,
+ Y = yLowIndex1 + 1,
+ Z = k,
+ NumTimesCalled = 0,
+ ID = hashID
+ });
+ }
+ hashID = getIdentifier(i, yLowIndex2, k);
+ vertexDictionaries[1].Add(hashID, new Vertex(p2));
+ valueDictionary.TryAdd(hashID, new StoredValue
+ {
+ Value = posDir2 ? -1 : 1,
+ X = i,
+ Y = yLowIndex2,
+ Z = k,
+ NumTimesCalled = 0,
+ ID = hashID
+ });
+ if (yLowIndex2 != numGridY - 1)
+ {
+ hashID += yMultiplier;
+ valueDictionary.TryAdd(hashID, new StoredValue
+ {
+ Value = posDir2 ? 1 : -1,
+ X = i,
+ Y = yLowIndex2 + 1,
+ Z = k,
+ NumTimesCalled = 0,
+ ID = hashID
+ });
+ }
+ }
+ }
}
+
private void FindXPointFromYandZ()
{
+ for (var j = 0; j < numGridY; j++)
+ for (var k = 0; k < numGridZ; k++)
+ {
+ var anchor = new Vector3(0.0, _yMin + j * gridToCoordinateFactor,
+ _zMin + k * gridToCoordinateFactor);
+ var intersectionEnumerator = surface.LineIntersection(anchor, Vector3.UnitX).GetEnumerator();
+ if (!intersectionEnumerator.MoveNext()) continue;
+ (Vector3 p1, _) = intersectionEnumerator.Current;
+ if (p1.X < _xMin || p1.X > _xMax)
+ {
+ if (!intersectionEnumerator.MoveNext()) continue;
+ (p1, _) = intersectionEnumerator.Current;
+ if (p1.X < _xMin || p1.X > _xMax)
+ continue;
+ }
+ var posDir1 = surface.GetNormalAtPoint(p1).X > 0;
+ var xLowIndex1 = (int)((p1.X - _xMin) * coordToGridFactor);
+ var p2 = Vector3.Null;
+ var hashID = getIdentifier(xLowIndex1, j, k);
+ if (intersectionEnumerator.MoveNext())
+ {
+ (p2, _) = intersectionEnumerator.Current;
+ if (p2.X < _xMin || p2.X > _xMax)
+ p2 = Vector3.Null;
+ }
+ if (p2.IsNull())
+ {
+ vertexDictionaries[0].Add(hashID, new Vertex(p1));
+ valueDictionary.TryAdd(hashID, new StoredValue
+ {
+ Value = posDir1 ? -1 : 1,
+ X = xLowIndex1,
+ Y = j,
+ Z = k,
+ NumTimesCalled = 0,
+ ID = hashID
+ });
+ if (xLowIndex1 != numGridX - 1)
+ {
+ hashID += 1; // x multiplier is 1
+ valueDictionary.TryAdd(hashID, new StoredValue
+ {
+ Value = posDir1 ? 1 : -1,
+ X = xLowIndex1 + 1,
+ Y = j,
+ Z = k,
+ NumTimesCalled = 0,
+ ID = hashID
+ });
+ }
+ }
+ else
+ {
+ var posDir2 = surface.GetNormalAtPoint(p2).X > 0;
+ var xLowIndex2 = (int)((p2.X - _xMin) * coordToGridFactor);
+ if (xLowIndex1 == xLowIndex2 || (posDir1 == posDir2 &&
+ (xLowIndex1 + 1 == xLowIndex2 || xLowIndex1 == xLowIndex2 + 1)))
+ continue; // can't have two vertices at the same x level or adjacent x levels
+
+ vertexDictionaries[0].Add(hashID, new Vertex(p1));
+ valueDictionary.TryAdd(hashID, new StoredValue
+ {
+ Value = posDir1 ? -1 : 1,
+ X = xLowIndex1,
+ Y = j,
+ Z = k,
+ NumTimesCalled = 0,
+ ID = hashID
+ });
+ if (xLowIndex1 != numGridX - 1)
+ {
+ hashID += 1; // x multiplier is 1
+ valueDictionary.TryAdd(hashID, new StoredValue
+ {
+ Value = posDir1 ? 1 : -1,
+ X = xLowIndex1 + 1,
+ Y = j,
+ Z = k,
+ NumTimesCalled = 0,
+ ID = hashID
+ });
+ }
+ hashID = getIdentifier(xLowIndex2, j, k);
+ vertexDictionaries[0].Add(hashID, new Vertex(p2));
+ valueDictionary.TryAdd(hashID, new StoredValue
+ {
+ Value = posDir2 ? -1 : 1,
+ X = xLowIndex2,
+ Y = j,
+ Z = k,
+ NumTimesCalled = 0,
+ ID = hashID
+ });
+ if (xLowIndex2 != numGridX - 1)
+ {
+ hashID += 1; // x multiplier is 1
+ valueDictionary.TryAdd(hashID, new StoredValue
+ {
+ Value = posDir2 ? 1 : -1,
+ X = xLowIndex2 + 1,
+ Y = j,
+ Z = k,
+ NumTimesCalled = 0,
+ ID = hashID
+ });
+ }
+ }
+ }
}
}
}
\ No newline at end of file
diff --git a/TessellationAndVoxelizationGeometryLibrary/Numerics/Extensions.cs b/TessellationAndVoxelizationGeometryLibrary/Numerics/Extensions.cs
index a3b999fc..eebcc8f0 100644
--- a/TessellationAndVoxelizationGeometryLibrary/Numerics/Extensions.cs
+++ b/TessellationAndVoxelizationGeometryLibrary/Numerics/Extensions.cs
@@ -589,6 +589,23 @@ public static Vector4 Transform(this Vector4 position, Matrix4x4 matrix)
public static Vector4 Multiply(this Vector4 position, Matrix4x4 matrix)
{ return Vector4.Multiply(position, matrix); }
+ ///
+ /// Multiplies a matrix by a vector. Note that the matrix is before the vector, so each term
+ /// is the dot product of a row of the matrix with the vector.
+ ///
+ /// The source vector.
+ /// The transformation matrix.
+ /// The transformed vector.
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static Vector4 Multiply(this Matrix4x4 A, Vector4 b)
+ {
+ return new Vector4(
+ b.X * A.M11 + b.Y * A.M12 + b.Z * A.M13 + b.W * A.M14,
+ b.X * A.M21 + b.Y * A.M22 + b.Z * A.M23 + b.W * A.M24,
+ b.X * A.M31 + b.Y * A.M32 + b.Z * A.M33 + b.W * A.M34,
+ b.X * A.M41 + b.Y * A.M42 + b.Z * A.M43 + b.W * A.M44);
+ }
+
///
/// Transforms a vector by the given matrix without the translation component.
/// This is often used for transforming normals, however note that proper transformations
@@ -683,6 +700,22 @@ public static Vector3 Solve(this Matrix3x3 matrix, Vector3 b)
return Vector3.Null;
return invert.Multiply(b);
}
+
+
+ ///
+ /// Solves for the value x in Ax=b. This is also represented as the
+ /// backslash operation
+ ///
+ /// The matrix.
+ /// The b.
+ /// Vector4.
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static Vector4 Solve(this Matrix4x4 matrix, Vector4 b)
+ {
+ if (!Matrix4x4.Invert(matrix, out var invert))
+ return Vector4.Null;
+ return invert.Multiply(b);
+ }
#endregion
///