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>
/// 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>
/// 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>
/// 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);
}
/// <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>
/// 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(IList <Vector3> points, IList <Vector3> outputSurfacePoints)
{
var rawPoints = CommonResources.GetVectorList();
rawPoints.AddRange(points);
GetConvexHull(rawPoints, outputSurfacePoints);
CommonResources.GiveBack(rawPoints);
}
/// <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>
/// Tests a ray against the triangle mesh.
///</summary>
///<param name="ray">Ray to test against the mesh.</param>
///<param name="hitCount">Number of hits between the ray and the mesh.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, out int hitCount)
{
var rayHits = CommonResources.GetRayHitList();
bool toReturn = RayCast(ray, rayHits);
hitCount = rayHits.Count;
CommonResources.GiveBack(rayHits);
return(toReturn);
}
///<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 volume and volume distribution of a shape based on a given center.
///</summary>
///<param name="shape">Shape to compute the volume information of.</param>
///<param name="center">Location to use as the center of the shape when computing the volume distribution.</param>
///<param name="volume">Volume of the shape.</param>
///<returns>Volume distribution of the shape.</returns>
public static Matrix3x3 ComputeVolumeDistribution(ConvexShape shape, ref Vector3 center, out float volume)
{
var pointContributions = CommonResources.GetVectorList();
GetPoints(shape, out volume, pointContributions);
Matrix3x3 volumeDistribution = ComputeVolumeDistribution(pointContributions, ref center);
CommonResources.GiveBack(pointContributions);
return(volumeDistribution);
}
///<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);
}
///<summary>
/// Computes the center and volume of a convex shape.
///</summary>
///<param name="shape">Shape to compute the center of.</param>
///<param name="volume">Volume of the shape.</param>
///<returns>Center of the shape.</returns>
public static Vector3 ComputeCenter(ConvexShape shape, out float volume)
{
var pointContributions = CommonResources.GetVectorList();
GetPoints(shape, out volume, pointContributions);
Vector3 center = AveragePoints(pointContributions);
CommonResources.GiveBack(pointContributions);
MathChecker.Validate(center);
return(center);
}
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>
/// Removes redundant points. Two points are redundant if they occupy the same hash grid cell.
/// </summary>
/// <param name="points">List of points to prune.</param>
/// <param name="cellSize">Size of cells to determine redundancy.</param>
public static void RemoveRedundantPoints(IList <Vector3> points, double cellSize)
{
var rawPoints = CommonResources.GetVectorList();
rawPoints.AddRange(points);
RemoveRedundantPoints(rawPoints, cellSize);
points.Clear();
for (int i = 0; i < rawPoints.Count; ++i)
{
points.Add(rawPoints.Elements[i]);
}
CommonResources.GiveBack(rawPoints);
}
/// <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);
}
///<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();
center = ComputeCenter(vertices, surfaceVertices);
this.vertices = new RawList <Vector3>(surfaceVertices);
CommonResources.GiveBack(surfaceVertices);
OnShapeChanged();
}
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>
/// 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(RawList <Vector3> points, RawList <int> outputTriangleIndices, IList <Vector3> outputSurfacePoints)
{
GetConvexHull(points, outputTriangleIndices);
var alreadyContainedIndices = CommonResources.GetIntSet();
for (int i = outputTriangleIndices.Count - 1; i >= 0; i--)
{
int index = outputTriangleIndices[i];
if (alreadyContainedIndices.Add(index))
{
outputSurfacePoints.Add(points[index]);
}
}
CommonResources.GiveBack(alreadyContainedIndices);
}
///<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;
}
///<summary>
/// Computes the center, volume, 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="volume">Volume of the convex hull.</param>
///<param name="outputSurfaceTriangles">Indices of surface triangles of 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, out float volume, IList <int> outputSurfaceTriangles, IList <Vector3> outputLocalSurfaceVertices)
{
Vector3 centroid = Toolbox.ZeroVector;
for (int k = 0; k < vertices.Count; k++)
{
centroid += vertices[k];
}
centroid /= vertices.Count;
//Toolbox.GetConvexHull(vertices, outputSurfaceTriangles, outputLocalSurfaceVertices);
ConvexHullHelper.GetConvexHull(vertices, outputSurfaceTriangles, outputLocalSurfaceVertices);
volume = 0;
var volumes = CommonResources.GetFloatList();
var centroids = CommonResources.GetVectorList();
for (int k = 0; k < outputSurfaceTriangles.Count; k += 3)
{
volumes.Add(Vector3.Dot(
Vector3.Cross(vertices[outputSurfaceTriangles[k + 1]] - vertices[outputSurfaceTriangles[k]],
vertices[outputSurfaceTriangles[k + 2]] - vertices[outputSurfaceTriangles[k]]),
centroid - vertices[outputSurfaceTriangles[k]]));
volume += volumes[k / 3];
centroids.Add((vertices[outputSurfaceTriangles[k]] + vertices[outputSurfaceTriangles[k + 1]] + vertices[outputSurfaceTriangles[k + 2]] + centroid) / 4);
}
Vector3 center = Toolbox.ZeroVector;
for (int k = 0; k < centroids.Count; k++)
{
center += centroids[k] * (volumes[k] / volume);
}
volume /= 6;
for (int k = 0; k < outputLocalSurfaceVertices.Count; k++)
{
outputLocalSurfaceVertices[k] -= center;
}
CommonResources.GiveBack(centroids);
CommonResources.GiveBack(volumes);
return(center);
}
/// <summary>
/// Recalculates the bounding box of the fluid based on its depth, surface normal, and surface triangles.
/// </summary>
public void RecalculateBoundingBox()
{
var points = CommonResources.GetVectorList();
foreach (var tri in SurfaceTriangles)
{
points.Add(tri[0]);
points.Add(tri[1]);
points.Add(tri[2]);
points.Add(tri[0] - upVector * MaxDepth);
points.Add(tri[1] - upVector * MaxDepth);
points.Add(tri[2] - upVector * MaxDepth);
}
boundingBox = BoundingBox.CreateFromPoints(points);
CommonResources.GiveBack(points);
//Compute the transforms used to pull objects into fluid local space.
Quaternion.GetQuaternionBetweenNormalizedVectors(ref Toolbox.UpVector, ref upVector, out surfaceTransform.Orientation);
Matrix3f.FromQuaternion(ref surfaceTransform.Orientation, out toSurfaceRotationMatrix);
surfaceTransform.Position = surfaceTriangles[0][0];
}
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);
}
///<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="rayHit">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, out RayHit rayHit)
{
var rayHits = CommonResources.GetRayHitList();
bool toReturn = RayCast(ray, maximumLength, sidedness, rayHits);
if (toReturn)
{
rayHit = rayHits[0];
for (int i = 1; i < rayHits.Count; i++)
{
RayHit hit = rayHits[i];
if (hit.T < rayHit.T)
{
rayHit = hit;
}
}
}
else
{
rayHit = new RayHit();
}
CommonResources.GiveBack(rayHits);
return(toReturn);
}
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);
}
