/// <summary>
/// Identifies the points on the surface of hull.
/// </summary>
/// <param name="points">List of points in the set.</param>
/// <param name="outputSurfacePoints">Unique points on the surface of the convex hull.</param>
public static void GetConvexHull(RawList <Vector3> points, IList <Vector3> outputSurfacePoints)
{
var indices = CommonResources.GetIntList();
GetConvexHull(points, indices, outputSurfacePoints);
CommonResources.GiveBack(indices);
}
/// <summary>
/// Rescales a convex hull shape.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="scaleFactor">The scaling factor.</param>
/// <returns>The new hull.</returns>
public static ConvexHullShape Rescale(this ConvexHullShape shape, Vector3 scaleFactor)
{
ReadOnlyList <Vector3> verts = shape.Vertices;
List <Vector3> newlist = new List <Vector3>(verts.Count);
foreach (Vector3 vert in verts)
{
newlist.Add(vert * scaleFactor);
}
double len = scaleFactor.Length();
RawList <int> triangles = CommonResources.GetIntList();
ConvexHullHelper.GetConvexHull(newlist, triangles);
InertiaHelper.ComputeShapeDistribution(newlist, triangles, out double volume, out Matrix3x3 volumeDistribution);
ConvexShapeDescription csd = new ConvexShapeDescription()
{
CollisionMargin = shape.CollisionMargin,
EntityShapeVolume = new BEPUphysics.CollisionShapes.EntityShapeVolumeDescription()
{
Volume = volume,
VolumeDistribution = volumeDistribution
},
MaximumRadius = shape.MaximumRadius * len,
MinimumRadius = shape.MinimumRadius * len
};
CommonResources.GiveBack(triangles);
return(new ConvexHullShape(newlist, csd));
}
/// <summary>
/// Computes a convex shape description for a TransformableShape and applies it.
/// </summary>
public void UpdateConvexShapeInfo()
{
//Compute the volume distribution.
var samples = CommonResources.GetVectorList();
if (samples.Capacity < InertiaHelper.SampleDirections.Length)
{
samples.Capacity = InertiaHelper.SampleDirections.Length;
}
samples.Count = InertiaHelper.SampleDirections.Length;
for (int i = 0; i < InertiaHelper.SampleDirections.Length; ++i)
{
shape.GetLocalExtremePointWithoutMargin(ref InertiaHelper.SampleDirections[i], out samples.Elements[i]);
}
var triangles = CommonResources.GetIntList();
ConvexHullHelper.GetConvexHull(samples, triangles);
Fix64 volume;
InertiaHelper.ComputeShapeDistribution(samples, triangles, out volume, out volumeDistribution);
Volume = volume;
//Estimate the minimum radius based on the surface mesh.
MinimumRadius = InertiaHelper.ComputeMinimumRadius(samples, triangles, ref Toolbox.ZeroVector) + collisionMargin;
MaximumRadius = ComputeMaximumRadius();
CommonResources.GiveBack(samples);
CommonResources.GiveBack(triangles);
}
/// <summary>
/// Computes and applies a convex shape description for this WrappedShape.
/// </summary>
/// <param name="center">Computed center of the shape before recentering.</param>
public void UpdateConvexShapeInfo(out Vector3 center)
{
//Compute the volume distribution.
var samples = CommonResources.GetVectorList();
if (samples.Capacity < InertiaHelper.SampleDirections.Length)
{
samples.Capacity = InertiaHelper.SampleDirections.Length;
}
samples.Count = InertiaHelper.SampleDirections.Length;
for (int i = 0; i < InertiaHelper.SampleDirections.Length; ++i)
{
GetLocalExtremePoint(InertiaHelper.SampleDirections[i], out samples.Elements[i]);
}
var triangles = CommonResources.GetIntList();
ConvexHullHelper.GetConvexHull(samples, triangles);
float volume;
InertiaHelper.ComputeShapeDistribution(samples, triangles, out center, out volume, out volumeDistribution);
Volume = volume;
CommonResources.GiveBack(samples);
CommonResources.GiveBack(triangles);
//Now recenter the shape and compute the radii estimates.
for (int i = 0; i < shapes.Count; i++)
{
shapes.WrappedList.Elements[i].Transform.Position -= center;
}
MinimumRadius = ComputeMinimumRadius();
MaximumRadius = ComputeMaximumRadius();
}
/// <summary>
/// Updates the detector volume's interpretation of the mesh. This should be called when the the TriangleMesh is changed significantly. This is called automatically if the TriangleMesh property is set.
/// </summary>
public void Reinitialize()
{
//Pick a point that is known to be outside the mesh as the origin.
Vector3f origin = (triangleMesh.Tree.BoundingBox.Max - triangleMesh.Tree.BoundingBox.Min) * 1.5f + triangleMesh.Tree.BoundingBox.Min;
//Pick a direction which will definitely hit the mesh.
Vector3f a, b, c;
triangleMesh.Data.GetTriangle(0, out a, out b, out c);
var direction = (a + b + c) / 3 - origin;
var ray = new Ray(origin, direction);
var triangles = CommonResources.GetIntList();
triangleMesh.Tree.GetOverlaps(ray, triangles);
float minimumT = float.MaxValue;
for (int i = 0; i < triangles.Count; i++)
{
triangleMesh.Data.GetTriangle(triangles.Elements[i], out a, out b, out c);
RayHit hit;
bool hitClockwise;
if (Toolbox.FindRayTriangleIntersection(ref ray, float.MaxValue, ref a, ref b, ref c, out hitClockwise, out hit))
{
if (hit.T < minimumT)
{
minimumT = hit.T;
innerFacingIsClockwise = !hitClockwise;
}
}
}
CommonResources.GiveBack(triangles);
}
private static void RemoveInsidePoints(RawList <Vector3> points, RawList <int> triangleIndices, RawList <int> outsidePoints)
{
var insidePoints = CommonResources.GetIntList();
//We're going to remove points from this list as we go to prune it down to the truly inner points.
insidePoints.AddRange(outsidePoints);
outsidePoints.Clear();
for (int i = 0; i < triangleIndices.Count && insidePoints.Count > 0; i += 3)
{
//Compute the triangle's plane in point-normal representation to test other points against.
Vector3 normal;
FindNormal(triangleIndices, points, i, out normal);
Vector3 p = points.Elements[triangleIndices.Elements[i]];
for (int j = insidePoints.Count - 1; j >= 0; --j)
{
//Offset from the triangle to the current point, tested against the normal, determines if the current point is visible
//from the triangle face.
Vector3 offset;
Vector3.Subtract(ref points.Elements[insidePoints.Elements[j]], ref p, out offset);
float dot;
Vector3.Dot(ref offset, ref normal, out dot);
//If it's visible, then it's outside!
if (dot > 0)
{
//This point is known to be on the outside; put it on the outside!
outsidePoints.Add(insidePoints.Elements[j]);
insidePoints.FastRemoveAt(j);
}
}
}
CommonResources.GiveBack(insidePoints);
}
/// <summary>
/// Tests to see if a ray's origin is contained within the mesh.
/// If it is, the hit location is found.
/// If it isn't, the hit location is still valid if a hit occurred.
/// If the origin isn't inside and there was no hit, the hit has a T value of Fix64.MaxValue.
/// </summary>
/// <param name="ray">Ray in the local space of the shape to test.</param>
/// <param name="hit">The first hit against the mesh, if any.</param>
/// <returns>Whether or not the ray origin was in the mesh.</returns>
public bool IsLocalRayOriginInMesh(ref Ray ray, out RayHit hit)
{
var overlapList = CommonResources.GetIntList();
hit = new RayHit();
hit.T = Fix64.MaxValue;
if (triangleMesh.Tree.GetOverlaps(ray, overlapList))
{
bool minimumClockwise = false;
for (int i = 0; i < overlapList.Count; i++)
{
Vector3 vA, vB, vC;
triangleMesh.Data.GetTriangle(overlapList[i], out vA, out vB, out vC);
bool hitClockwise;
RayHit tempHit;
if (Toolbox.FindRayTriangleIntersection(ref ray, Fix64.MaxValue, ref vA, ref vB, ref vC, out hitClockwise, out tempHit) &&
tempHit.T < hit.T)
{
hit = tempHit;
minimumClockwise = hitClockwise;
}
}
CommonResources.GiveBack(overlapList);
//If the mesh is hit from behind by the ray on the first hit, then the ray is inside.
return(hit.T < Fix64.MaxValue && ((SidednessWhenSolid == TriangleSidedness.Clockwise && !minimumClockwise) || (SidednessWhenSolid == TriangleSidedness.Counterclockwise && minimumClockwise)));
}
CommonResources.GiveBack(overlapList);
return(false);
}
/// <summary>
/// Determines if a point is contained by the detector volume.
/// </summary>
/// <param name="point">Point to check for containment.</param>
/// <returns>Whether or not the point is contained by the detector volume.</returns>
public bool IsPointContained(Vector3f point)
{
var triangles = CommonResources.GetIntList();
bool contained = IsPointContained(ref point, triangles);
CommonResources.GiveBack(triangles);
return(contained);
}
///<summary>
/// Computes the center and surface triangles of a convex hull defined by a point set.
///</summary>
///<param name="vertices">Point set defining the convex hull.</param>
///<param name="outputLocalSurfaceVertices">Local positions of vertices on the convex hull.</param>
///<returns>Center of the convex hull.</returns>
public static Vector3 ComputeCenter(IList <Vector3> vertices, IList <Vector3> outputLocalSurfaceVertices)
{
float volume;
var indices = CommonResources.GetIntList();
Vector3 toReturn = ComputeCenter(vertices, out volume, indices, outputLocalSurfaceVertices);
CommonResources.GiveBack(indices);
return(toReturn);
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
hit = new RayHit();
BoundingBox boundingBox;
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out boundingBox);
var tri = PhysicsThreadResources.GetTriangle();
var hitElements = CommonResources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref worldTransform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref worldTransform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref worldTransform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.MaximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
radius = tri.vC.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform {
Orientation = Quaternion.Identity, Position = center
};
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.MaximumRadius = 0;
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(hit.T != float.MaxValue);
}
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(false);
}
///<summary>
/// Computes the center and volume of a convex hull defined by a pointset.
///</summary>
///<param name="vertices">Point set defining the convex hull.</param>
///<param name="volume">Volume of the convex hull.</param>
///<returns>Center of the convex hull.</returns>
public static Vector3 ComputeCenter(IList <Vector3> vertices, out float volume)
{
var localSurfaceVertices = CommonResources.GetVectorList();
var surfaceTriangles = CommonResources.GetIntList();
Vector3 toReturn = ComputeCenter(vertices, out volume, surfaceTriangles, localSurfaceVertices);
CommonResources.GiveBack(localSurfaceVertices);
CommonResources.GiveBack(surfaceTriangles);
return(toReturn);
}
protected override void UpdateContainedPairs(Fix64 dt)
{
var overlappedElements = CommonResources.GetIntList();
mesh.Mesh.Tree.GetOverlaps(mobileMesh.boundingBox, overlappedElements);
for (int i = 0; i < overlappedElements.Count; i++)
{
TryToAdd(overlappedElements.Elements[i]);
}
CommonResources.GiveBack(overlappedElements);
}
/// <summary>
/// Computes the volume distribution of the shape as well as its volume.
/// The volume distribution can be used to compute inertia tensors when
/// paired with mass and other tuning factors.
/// </summary>
/// <param name="volume">Volume of the shape.</param>
/// <returns>Volume distribution of the shape.</returns>
public override Matrix3x3 ComputeVolumeDistribution(out float volume)
{
var surfaceTriangles = CommonResources.GetIntList();
var surfaceVertices = CommonResources.GetVectorList();
ComputeCenter(out volume, surfaceTriangles, surfaceVertices);
Matrix3x3 toReturn = ComputeVolumeDistribution(volume, surfaceTriangles);
CommonResources.GiveBack(surfaceTriangles);
CommonResources.GiveBack(surfaceVertices);
return(toReturn);
}
/// <summary>
/// Identifies the points on the surface of hull.
/// </summary>
/// <param name="points">List of points in the set.</param>
/// <param name="outputTriangleIndices">List of indices into the input point set composing the triangulated surface of the convex hull.
/// Each group of 3 indices represents a triangle on the surface of the hull.</param>
/// <param name="outputSurfacePoints">Unique points on the surface of the convex hull.</param>
public static void GetConvexHull(IList <Vector3> points, IList <int> outputTriangleIndices, IList <Vector3> outputSurfacePoints)
{
var rawPoints = CommonResources.GetVectorList();
var rawIndices = CommonResources.GetIntList();
rawPoints.AddRange(points);
GetConvexHull(rawPoints, rawIndices, outputSurfacePoints);
CommonResources.GiveBack(rawPoints);
for (int i = 0; i < rawIndices.Count; i++)
{
outputTriangleIndices.Add(rawIndices[i]);
}
CommonResources.GiveBack(rawIndices);
}
protected override void UpdateContainedPairs(float dt)
{
var overlappedElements = CommonResources.GetIntList();
BoundingBox localBoundingBox;
System.Numerics.Vector3 sweep;
Vector3Ex.Multiply(ref mobileMesh.entity.linearVelocity, dt, out sweep);
mobileMesh.Shape.GetSweptLocalBoundingBox(ref mobileMesh.worldTransform, ref mesh.worldTransform, ref sweep, out localBoundingBox);
mesh.Shape.TriangleMesh.Tree.GetOverlaps(localBoundingBox, overlappedElements);
for (int i = 0; i < overlappedElements.Count; i++)
{
TryToAdd(overlappedElements.Elements[i]);
}
CommonResources.GiveBack(overlappedElements);
}
///<summary>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
/// <param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
/// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
///<param name="hits">Hit data for the ray, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, IList <RayHit> hits)
{
var hitElements = CommonResources.GetIntList();
tree.GetOverlaps(ray, maximumLength, hitElements);
for (int i = 0; i < hitElements.Count; i++)
{
Vector3 v1, v2, v3;
data.GetTriangle(hitElements[i], out v1, out v2, out v3);
RayHit hit;
if (Toolbox.FindRayTriangleIntersection(ref ray, maximumLength, sidedness, ref v1, ref v2, ref v3, out hit))
{
hits.Add(hit);
}
}
CommonResources.GiveBack(hitElements);
return(hits.Count > 0);
}
/// <summary>
/// Computes and applies a convex shape description for this MinkowskiSumShape.
/// </summary>
/// <returns>Description required to define a convex shape.</returns>
public void UpdateConvexShapeInfo()
{
//Compute the volume distribution.
RawList <Vector3> samples = CommonResources.GetVectorList();
if (samples.Capacity < InertiaHelper.SampleDirections.Length)
{
samples.Capacity = InertiaHelper.SampleDirections.Length;
}
samples.Count = InertiaHelper.SampleDirections.Length;
for (int i = 0; i < InertiaHelper.SampleDirections.Length; ++i)
{
GetLocalExtremePoint(InertiaHelper.SampleDirections[i], out samples.Elements[i]);
}
RawList <int> triangles = CommonResources.GetIntList();
ConvexHullHelper.GetConvexHull(samples, triangles);
float volume;
Vector3 center;
InertiaHelper.ComputeShapeDistribution(samples, triangles, out center, out volume, out volumeDistribution);
Volume = volume;
//Recenter the shape.
localOffset = -center;
CommonResources.GiveBack(samples);
CommonResources.GiveBack(triangles);
//Compute the radii.
float minRadius = 0, maxRadius = 0;
for (int i = 0; i < Shapes.Count; i++)
{
minRadius += Shapes.WrappedList.Elements[i].CollisionShape.MinimumRadius;
maxRadius += Shapes.WrappedList.Elements[i].CollisionShape.MaximumRadius;
}
MinimumRadius = minRadius + collisionMargin;
MaximumRadius = maxRadius + collisionMargin;
}
///<summary>
/// Constructs a new convex hull shape.
/// The point set will be recentered on the local origin.
///</summary>
///<param name="vertices">Point set to use to construct the convex hull.</param>
/// <param name="center">Computed center of the convex hull shape prior to recentering.</param>
///<exception cref="ArgumentException">Thrown when the point set is empty.</exception>
public ConvexHullShape(IList <Vector3> vertices, out Vector3 center)
{
if (vertices.Count == 0)
{
throw new ArgumentException("Vertices list used to create a ConvexHullShape cannot be empty.");
}
var surfaceVertices = CommonResources.GetVectorList();
var hullTriangleIndices = CommonResources.GetIntList();
UpdateConvexShapeInfo(ComputeDescription(vertices, collisionMargin, out center, hullTriangleIndices, surfaceVertices));
this.vertices = surfaceVertices.ToArray();
CommonResources.GiveBack(hullTriangleIndices);
CommonResources.GiveBack(surfaceVertices);
unexpandedMaximumRadius = MaximumRadius - collisionMargin;
unexpandedMinimumRadius = MinimumRadius - collisionMargin;
}
protected override void UpdateContainedPairs(Fix64 dt)
{
var overlappedElements = CommonResources.GetIntList();
BoundingBox localBoundingBox;
AffineTransform meshTransform;
AffineTransform.CreateFromRigidTransform(ref mesh.worldTransform, out meshTransform);
Vector3 sweep;
Vector3.Subtract(ref mobileMesh.entity.linearVelocity, ref mesh.entity.linearVelocity, out sweep);
Vector3.Multiply(ref sweep, dt, out sweep);
mobileMesh.Shape.GetSweptLocalBoundingBox(ref mobileMesh.worldTransform, ref meshTransform, ref sweep, out localBoundingBox);
mesh.Shape.TriangleMesh.Tree.GetOverlaps(localBoundingBox, overlappedElements);
for (int i = 0; i < overlappedElements.Count; i++)
{
TryToAdd(overlappedElements.Elements[i]);
}
CommonResources.GiveBack(overlappedElements);
}
TriangleSidedness ComputeSolidSidednessHelper(Ray ray)
{
TriangleSidedness toReturn;
var hitList = CommonResources.GetIntList();
if (triangleMesh.Tree.GetOverlaps(ray, hitList))
{
Vector3 vA, vB, vC;
var hits = CommonResources.GetRayHitList();
//Identify the first and last hits.
int minimum = 0;
int maximum = 0;
Fix64 minimumT = Fix64.MaxValue;
Fix64 maximumT = -1;
for (int i = 0; i < hitList.Count; i++)
{
triangleMesh.Data.GetTriangle(hitList[i], out vA, out vB, out vC);
RayHit hit;
if (Toolbox.FindRayTriangleIntersection(ref ray, Fix64.MaxValue, TriangleSidedness.DoubleSided, ref vA, ref vB, ref vC, out hit) &&
IsHitUnique(hits, ref hit))
{
if (hit.T < minimumT)
{
minimumT = hit.T;
minimum = hitList[i];
}
if (hit.T > maximumT)
{
maximumT = hit.T;
maximum = hitList[i];
}
}
}
if (hits.Count % 2 == 0)
{
//Since we were outside, the first hit triangle should be calibrated
//such that it faces towards us.
triangleMesh.Data.GetTriangle(minimum, out vA, out vB, out vC);
var normal = Vector3.Cross(vA - vB, vA - vC);
if (Vector3.Dot(normal, ray.Direction) < F64.C0)
{
toReturn = TriangleSidedness.Clockwise;
}
else
{
toReturn = TriangleSidedness.Counterclockwise;
}
}
else
{
//Since we were inside, the last hit triangle should be calibrated
//such that it faces away from us.
triangleMesh.Data.GetTriangle(maximum, out vA, out vB, out vC);
var normal = Vector3.Cross(vA - vB, vA - vC);
if (Vector3.Dot(normal, ray.Direction) < F64.C0)
{
toReturn = TriangleSidedness.Counterclockwise;
}
else
{
toReturn = TriangleSidedness.Clockwise;
}
}
CommonResources.GiveBack(hits);
}
else
{
toReturn = TriangleSidedness.DoubleSided; //This is a problem...
}
CommonResources.GiveBack(hitList);
return(toReturn);
}
/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//If the convex cast is inside the mesh and the mesh is solid, it should return t = 0.
var ray = new Ray()
{
Position = startingTransform.Position, Direction = Toolbox.UpVector
};
if (Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
hit = new RayHit()
{
Location = startingTransform.Position, Normal = new Vector3(), T = F64.C0
};
return(true);
}
}
hit = new RayHit();
BoundingBox boundingBox;
var transform = new AffineTransform {
Translation = worldTransform.Position
};
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform);
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox);
var tri = PhysicsThreadResources.GetTriangle();
var hitElements = CommonResources.GetIntList();
if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
{
hit.T = Fix64.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
AffineTransform.Transform(ref tri.vA, ref transform, out tri.vA);
AffineTransform.Transform(ref tri.vB, ref transform, out tri.vB);
AffineTransform.Transform(ref tri.vC, ref transform, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, F64.OneThird, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.MaximumRadius = tri.vA.LengthSquared();
Fix64 radius = tri.vB.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
radius = tri.vC.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
tri.MaximumRadius = Fix64.Sqrt(tri.MaximumRadius);
tri.collisionMargin = F64.C0;
var triangleTransform = new RigidTransform {
Orientation = Quaternion.Identity, Position = center
};
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.MaximumRadius = F64.C0;
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(hit.T != Fix64.MaxValue);
}
PhysicsThreadResources.GiveBack(tri);
CommonResources.GiveBack(hitElements);
return(false);
}
/// <summary>
/// Identifies the indices of points in a set which are on the outer convex hull of the set.
/// </summary>
/// <param name="points">List of points in the set.</param>
/// <param name="outputTriangleIndices">List of indices into the input point set composing the triangulated surface of the convex hull.
/// Each group of 3 indices represents a triangle on the surface of the hull.</param>
public static void GetConvexHull(RawList <Vector3> points, RawList <int> outputTriangleIndices)
{
if (points.Count == 0)
{
throw new ArgumentException("Point set must have volume.");
}
RawList <int> outsidePoints = CommonResources.GetIntList();
if (outsidePoints.Capacity < points.Count - 4)
{
outsidePoints.Capacity = points.Count - 4;
}
//Build the initial tetrahedron.
//It will also give us the location of a point which is guaranteed to be within the
//final convex hull. We can use this point to calibrate the winding of triangles.
//A set of outside point candidates (all points other than those composing the tetrahedron) will be returned in the outsidePoints list.
//That list will then be further pruned by the RemoveInsidePoints call.
Vector3 insidePoint;
ComputeInitialTetrahedron(points, outsidePoints, outputTriangleIndices, out insidePoint);
//Compute outside points.
RemoveInsidePoints(points, outputTriangleIndices, outsidePoints);
var edges = CommonResources.GetIntList();
var toRemove = CommonResources.GetIntList();
var newTriangles = CommonResources.GetIntList();
//We're now ready to begin the main loop.
while (outsidePoints.Count > 0)
{
//While the convex hull is incomplete...
for (int k = 0; k < outputTriangleIndices.Count; k += 3)
{
//Find the normal of the triangle
Vector3 normal;
FindNormal(outputTriangleIndices, points, k, out normal);
//Get the furthest point in the direction of the normal.
int maxIndexInOutsideList = GetExtremePoint(ref normal, points, outsidePoints);
int maxIndex = outsidePoints.Elements[maxIndexInOutsideList];
Vector3 maximum = points.Elements[maxIndex];
//If the point is beyond the current triangle, continue.
Vector3 offset;
Vector3.Subtract(ref maximum, ref points.Elements[outputTriangleIndices.Elements[k]], out offset);
float dot;
Vector3.Dot(ref normal, ref offset, out dot);
if (dot > 0)
{
//It's been picked! Remove the maximum point from the outside.
outsidePoints.FastRemoveAt(maxIndexInOutsideList);
//Remove any triangles that can see the point, including itself!
edges.Clear();
toRemove.Clear();
for (int n = outputTriangleIndices.Count - 3; n >= 0; n -= 3)
{
//Go through each triangle, if it can be seen, delete it and use maintainEdge on its edges.
if (IsTriangleVisibleFromPoint(outputTriangleIndices, points, n, ref maximum))
{
//This triangle can see it!
//TODO: CONSIDER CONSISTENT WINDING HAPPYTIMES
MaintainEdge(outputTriangleIndices[n], outputTriangleIndices[n + 1], edges);
MaintainEdge(outputTriangleIndices[n], outputTriangleIndices[n + 2], edges);
MaintainEdge(outputTriangleIndices[n + 1], outputTriangleIndices[n + 2], edges);
//Because fast removals are being used, the order is very important.
//It's pulling indices in from the end of the list in order, and also ensuring
//that we never issue a removal order beyond the end of the list.
outputTriangleIndices.FastRemoveAt(n + 2);
outputTriangleIndices.FastRemoveAt(n + 1);
outputTriangleIndices.FastRemoveAt(n);
}
}
//Create new triangles.
for (int n = 0; n < edges.Count; n += 2)
{
//For each edge, create a triangle with the extreme point.
newTriangles.Add(edges[n]);
newTriangles.Add(edges[n + 1]);
newTriangles.Add(maxIndex);
}
//Only verify the windings of the new triangles.
VerifyWindings(newTriangles, points, ref insidePoint);
outputTriangleIndices.AddRange(newTriangles);
newTriangles.Clear();
//Remove all points from the outsidePoints if they are inside the polyhedron
RemoveInsidePoints(points, outputTriangleIndices, outsidePoints);
//The list has been significantly messed with, so restart the loop.
break;
}
}
}
CommonResources.GiveBack(outsidePoints);
CommonResources.GiveBack(edges);
CommonResources.GiveBack(toRemove);
CommonResources.GiveBack(newTriangles);
}
private static void ComputeInitialTetrahedron(RawList <Vector3> points, RawList <int> outsidePointCandidates, RawList <int> triangleIndices, out Vector3 centroid)
{
//Find four points on the hull.
//We'll start with using the x axis to identify two points on the hull.
int a, b, c, d;
Vector3 direction;
//Find the extreme points along the x axis.
float minimumX = float.MaxValue, maximumX = -float.MaxValue;
int minimumXIndex = 0, maximumXIndex = 0;
for (int i = 0; i < points.Count; ++i)
{
var v = points.Elements[i];
if (v.X > maximumX)
{
maximumX = v.X;
maximumXIndex = i;
}
else if (v.X < minimumX)
{
minimumX = v.X;
minimumXIndex = i;
}
}
a = minimumXIndex;
b = maximumXIndex;
//Check for redundancies..
if (a == b)
{
throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
}
//Now, use a second axis perpendicular to the two points we found.
Vector3 ab;
Vector3.Subtract(ref points.Elements[b], ref points.Elements[a], out ab);
Vector3.Cross(ref ab, ref Toolbox.UpVector, out direction);
if (direction.LengthSquared() < Toolbox.Epsilon)
{
Vector3.Cross(ref ab, ref Toolbox.RightVector, out direction);
}
float minimumDot, maximumDot;
int minimumIndex, maximumIndex;
GetExtremePoints(ref direction, points, out maximumDot, out minimumDot, out maximumIndex, out minimumIndex);
//Compare the location of the extreme points to the location of the axis.
float dot;
Vector3.Dot(ref direction, ref points.Elements[a], out dot);
//Use the point further from the axis.
if (Math.Abs(dot - minimumDot) > Math.Abs(dot - maximumDot))
{
//In this case, we should use the minimum index.
c = minimumIndex;
}
else
{
//In this case, we should use the maximum index.
c = maximumIndex;
}
//Check for redundancies..
if (a == c || b == c)
{
throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
}
//Use a third axis perpendicular to the plane defined by the three unique points a, b, and c.
Vector3 ac;
Vector3.Subtract(ref points.Elements[c], ref points.Elements[a], out ac);
Vector3.Cross(ref ab, ref ac, out direction);
GetExtremePoints(ref direction, points, out maximumDot, out minimumDot, out maximumIndex, out minimumIndex);
//Compare the location of the extreme points to the location of the plane.
Vector3.Dot(ref direction, ref points.Elements[a], out dot);
//Use the point further from the plane.
if (Math.Abs(dot - minimumDot) > Math.Abs(dot - maximumDot))
{
//In this case, we should use the minimum index.
d = minimumIndex;
}
else
{
//In this case, we should use the maximum index.
d = maximumIndex;
}
//Check for redundancies..
if (a == d || b == d || c == d)
{
throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
}
//Add the triangles.
triangleIndices.Add(a);
triangleIndices.Add(b);
triangleIndices.Add(c);
triangleIndices.Add(a);
triangleIndices.Add(b);
triangleIndices.Add(d);
triangleIndices.Add(a);
triangleIndices.Add(c);
triangleIndices.Add(d);
triangleIndices.Add(b);
triangleIndices.Add(c);
triangleIndices.Add(d);
//The centroid is guaranteed to be within the convex hull. It will be used to verify the windings of triangles throughout the hull process.
Vector3.Add(ref points.Elements[a], ref points.Elements[b], out centroid);
Vector3.Add(ref centroid, ref points.Elements[c], out centroid);
Vector3.Add(ref centroid, ref points.Elements[d], out centroid);
Vector3.Multiply(ref centroid, 0.25f, out centroid);
for (int i = 0; i < triangleIndices.Count; i += 3)
{
var vA = points.Elements[triangleIndices.Elements[i]];
var vB = points.Elements[triangleIndices.Elements[i + 1]];
var vC = points.Elements[triangleIndices.Elements[i + 2]];
//Check the signed volume of a parallelepiped with the edges of this triangle and the centroid.
Vector3 cross;
Vector3.Subtract(ref vB, ref vA, out ab);
Vector3.Subtract(ref vC, ref vA, out ac);
Vector3.Cross(ref ac, ref ab, out cross);
Vector3 offset;
Vector3.Subtract(ref vA, ref centroid, out offset);
float volume;
Vector3.Dot(ref offset, ref cross, out volume);
//This volume/cross product could also be used to check for degeneracy, but we already tested for that.
if (Math.Abs(volume) < Toolbox.BigEpsilon)
{
throw new ArgumentException("Point set is degenerate; convex hulls must have volume.");
}
if (volume < 0)
{
//If the signed volume is negative, that means the triangle's winding is opposite of what we want.
//Flip it around!
var temp = triangleIndices.Elements[i];
triangleIndices.Elements[i] = triangleIndices.Elements[i + 1];
triangleIndices.Elements[i + 1] = temp;
}
}
//Points which belong to the tetrahedra are guaranteed to be 'in' the convex hull. Do not allow them to be considered.
var tetrahedronIndices = CommonResources.GetIntList();
tetrahedronIndices.Add(a);
tetrahedronIndices.Add(b);
tetrahedronIndices.Add(c);
tetrahedronIndices.Add(d);
//Sort the indices to allow a linear time loop.
Array.Sort(tetrahedronIndices.Elements, 0, 4);
int tetrahedronIndex = 0;
for (int i = 0; i < points.Count; ++i)
{
if (tetrahedronIndex < 4 && i == tetrahedronIndices[tetrahedronIndex])
{
//Don't add a tetrahedron index. Now that we've found this index, though, move on to the next one.
++tetrahedronIndex;
}
else
{
outsidePointCandidates.Add(i);
}
}
CommonResources.GiveBack(tetrahedronIndices);
}