/// <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 a convex shape description for a ConvexHullShape.
/// </summary>
/// <param name="vertices">Vertices describing the convex hull shape.</param>
/// <param name="collisionMargin">Collision margin of the shape.</param>
/// <param name="center">Computed center of the convex hull shape. Used as the origin of the outputUniqueSurfaceVertices.</param>
/// <param name="outputHullTriangleIndices">Computed 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="outputUniqueSurfaceVertices">Computed nonredundant list of vertices composing the outer shell of the input point set. Recentered on the local origin.</param>
/// <returns>Description required to define a convex shape.</returns>
public static ConvexShapeDescription ComputeDescription(IList <Vector3> vertices, float collisionMargin, out Vector3 center, IList <int> outputHullTriangleIndices, IList <Vector3> outputUniqueSurfaceVertices)
{
if (outputHullTriangleIndices.Count != 0 || outputUniqueSurfaceVertices.Count != 0)
{
throw new ArgumentException("Output lists must start empty.");
}
ConvexShapeDescription description;
ConvexHullHelper.GetConvexHull(vertices, outputHullTriangleIndices, outputUniqueSurfaceVertices);
InertiaHelper.ComputeShapeDistribution(vertices, outputHullTriangleIndices, out center, out description.EntityShapeVolume.Volume, out description.EntityShapeVolume.VolumeDistribution);
//Recenter the surface vertices.
for (int i = 0; i < outputUniqueSurfaceVertices.Count; ++i)
{
outputUniqueSurfaceVertices[i] -= center;
}
description.MinimumRadius = InertiaHelper.ComputeMinimumRadius(vertices, outputHullTriangleIndices, ref center) + collisionMargin;
description.MaximumRadius = ComputeMaximumRadius(outputUniqueSurfaceVertices, collisionMargin);
description.CollisionMargin = collisionMargin;
return(description);
}
public static void GetShapeMeshData(EntityCollidable collidable, List <VertexPositionNormalTexture> vertices, List <ushort> indices)
{
var convexHullShape = collidable.Shape as ConvexHullShape;
if (convexHullShape == null)
{
throw new ArgumentException("Wrong shape type.");
}
var hullTriangleVertices = new List <Vector3>();
var hullTriangleIndices = new List <int>();
ConvexHullHelper.GetConvexHull(convexHullShape.Vertices, hullTriangleIndices, hullTriangleVertices);
//The hull triangle vertices are used as a dummy to get the unnecessary hull vertices, which are cleared afterwards.
hullTriangleVertices.Clear();
foreach (int i in hullTriangleIndices)
{
hullTriangleVertices.Add(convexHullShape.Vertices[i]);
}
var toReturn = new VertexPositionNormalTexture[hullTriangleVertices.Count];
Vector3 normal;
for (ushort i = 0; i < hullTriangleVertices.Count; i += 3)
{
normal = Vector3.Normalize(Vector3.Cross(hullTriangleVertices[i + 2] - hullTriangleVertices[i], hullTriangleVertices[i + 1] - hullTriangleVertices[i]));
vertices.Add(new VertexPositionNormalTexture(hullTriangleVertices[i], normal, new Vector2(0, 0)));
vertices.Add(new VertexPositionNormalTexture(hullTriangleVertices[i + 1], normal, new Vector2(1, 0)));
vertices.Add(new VertexPositionNormalTexture(hullTriangleVertices[i + 2], normal, new Vector2(0, 1)));
indices.Add(i);
indices.Add((ushort)(i + 1));
indices.Add((ushort)(i + 2));
}
}
/// <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();
}
public static void GetShapeMeshData(EntityCollidable collidable, List <VertexPositionNormalTexture> vertices, List <ushort> indices)
{
var transformableShape = collidable.Shape as TransformableShape;
if (transformableShape == null)
{
throw new ArgumentException("Wrong shape type.");
}
var points = new List <Vector3>();
Vector3 max;
var direction = new Vector3();
float angleChange = MathHelper.TwoPi / NumSamples;
for (int i = 1; i < NumSamples / 2 - 1; i++)
{
float phi = MathHelper.PiOver2 - i * angleChange;
var sinPhi = (float)Math.Sin(phi);
var cosPhi = (float)Math.Cos(phi);
for (int j = 0; j < NumSamples; j++)
{
float theta = j * angleChange;
direction.X = (float)Math.Cos(theta) * cosPhi;
direction.Y = sinPhi;
direction.Z = (float)Math.Sin(theta) * cosPhi;
transformableShape.GetLocalExtremePoint(direction, out max);
points.Add(max);
}
}
transformableShape.GetLocalExtremePoint(Toolbox.UpVector, out max);
points.Add(max);
transformableShape.GetLocalExtremePoint(Toolbox.DownVector, out max);
points.Add(max);
var hullTriangleVertices = new List <Vector3>();
var hullTriangleIndices = new List <int>();
ConvexHullHelper.GetConvexHull(points, hullTriangleIndices, hullTriangleVertices);
//The hull triangle vertices are used as a dummy to get the unnecessary hull vertices, which are cleared afterwards.
hullTriangleVertices.Clear();
foreach (int i in hullTriangleIndices)
{
hullTriangleVertices.Add(points[i]);
}
Vector3 normal;
for (ushort i = 0; i < hullTriangleVertices.Count; i += 3)
{
normal = Vector3.Normalize(Vector3.Cross(hullTriangleVertices[i + 2] - hullTriangleVertices[i], hullTriangleVertices[i + 1] - hullTriangleVertices[i]));
vertices.Add(new VertexPositionNormalTexture(hullTriangleVertices[i], normal, new Vector2(0, 0)));
vertices.Add(new VertexPositionNormalTexture(hullTriangleVertices[i + 1], normal, new Vector2(1, 0)));
vertices.Add(new VertexPositionNormalTexture(hullTriangleVertices[i + 2], normal, new Vector2(0, 1)));
indices.Add(i);
indices.Add((ushort)(i + 1));
indices.Add((ushort)(i + 2));
}
}
/// <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>
/// Converts a mesh to a BEPU convex mesh.
/// </summary>
/// <param name="input">The model.</param>
/// <param name="verts">The vertice count if needed.</param>
/// <param name="center">The center output.</param>
/// <returns>The BEPU mesh.</returns>
public ConvexHullShape MeshToBepuConvex(Model3D input, out int verts, out Vector3 center)
{
List <Vector3> vertices = new List <Vector3>(GetCollisionVertices(input));
ConvexHullHelper.RemoveRedundantPoints(vertices);
verts = vertices.Count;
return(new ConvexHullShape(vertices, out center));
}
//List<DebugStep> debugSteps;
public override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(0, -2.5f, 10);
camera.Yaw = 0;
camera.Pitch = 0;
Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)));
const int pointCount = 128;
points = new QuickList <Vector3>(pointCount * 2, BufferPool);
//points.Allocate(BufferPool) = new Vector3(0, 0, 0);
//points.Allocate(BufferPool) = new Vector3(0, 0, 1);
//points.Allocate(BufferPool) = new Vector3(0, 1, 0);
//points.Allocate(BufferPool) = new Vector3(0, 1, 1);
//points.Allocate(BufferPool) = new Vector3(1, 0, 0);
//points.Allocate(BufferPool) = new Vector3(1, 0, 1);
//points.Allocate(BufferPool) = new Vector3(1, 1, 0);
//points.Allocate(BufferPool) = new Vector3(1, 1, 1);
var random = new Random(5);
for (int i = 0; i < pointCount; ++i)
{
points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 3 * (float)random.NextDouble());
//points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1)) * (float)random.NextDouble();
}
var pointsBuffer = points.Span.Slice(0, points.Count);
ConvexHullHelper.CreateShape(pointsBuffer, BufferPool, out _, out var hullShape);
const int iterationCount = 100;
var start = Stopwatch.GetTimestamp();
for (int i = 0; i < iterationCount; ++i)
{
ConvexHullHelper.CreateShape(pointsBuffer, BufferPool, out _, out var perfTestShape);
perfTestShape.Dispose(BufferPool);
}
var end = Stopwatch.GetTimestamp();
Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}");
hullShape.ComputeInertia(1, out var inertia);
Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 0), inertia, new CollidableDescription(Simulation.Shapes.Add(hullShape), 10.1f), new BodyActivityDescription(0.01f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(2)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 2)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-15, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2f, 2f, 2f)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-10, -5, 5), new CollidableDescription(Simulation.Shapes.Add(new Triangle {
A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5)
}), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 1)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 1)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), new CollidableDescription(Simulation.Shapes.Add(hullShape), 0.1f)));
}
/// <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>
/// 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 = Resources.GetFloatList();
var centroids = Resources.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;
}
Resources.GiveBack(centroids);
Resources.GiveBack(volumes);
return(center);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public ConvexHullTestDemo(DemosGame game)
: base(game)
{
var random = new Random(5);
for (int i = 0; i < 500000; ++i)
{
List <Vector3> points = new List <Vector3>();
for (int k = 0; k < random.Next(8, 60); k++)
{
points.Add(new Vector3(-100 + 30 * (Fix64)random.NextDouble(), 100 + 500 * (Fix64)random.NextDouble(), 100 + 30 * (Fix64)random.NextDouble()));
}
var convexHull = new ConvexHull(new Vector3(0, 7, 0), points, 10);
Console.WriteLine(convexHull.CollisionInformation.Shape.Vertices.Count);
}
var vertices = new[]
{
new Vector3(0, -1.750886E-9m, -1.5m),
new Vector3(1, 1, 0.5m),
new Vector3(1, -1, 0.5m),
new Vector3(-1, 1, 0.5m),
new Vector3(-1, -1, 0.5m),
};
var hullVertices = new RawList <Vector3>();
ConvexHullHelper.GetConvexHull(vertices, hullVertices);
ConvexHull hull = new ConvexHull(vertices, 5);
Space.Add(hull);
Box ground = new Box(new Vector3(0, -.5m, 0), 50, 1, 50);
Space.Add(ground);
game.Camera.Position = new Vector3(0, 6, 15);
}
public static void GetShapeMeshData(EntityCollidable collidable, List <VertexPositionNormalTexture> vertices, List <ushort> indices)
{
var shape = collidable.Shape as ConvexShape;
if (shape == null)
{
throw new ArgumentException("Wrong shape type for this helper.");
}
var vertexPositions = new BEPUutilities.Vector3[SampleDirections.Length];
for (int i = 0; i < SampleDirections.Length; ++i)
{
shape.GetLocalExtremePoint(SampleDirections[i], out vertexPositions[i]);
}
var hullIndices = new RawList <int>();
ConvexHullHelper.GetConvexHull(vertexPositions, hullIndices);
var hullTriangleVertices = new RawList <BEPUutilities.Vector3>();
foreach (int i in hullIndices)
{
hullTriangleVertices.Add(vertexPositions[i]);
}
for (ushort i = 0; i < hullTriangleVertices.Count; i += 3)
{
Vector3 normal = MathConverter.Convert(BEPUutilities.Vector3.Normalize(BEPUutilities.Vector3.Cross(hullTriangleVertices[i + 2] - hullTriangleVertices[i], hullTriangleVertices[i + 1] - hullTriangleVertices[i])));
vertices.Add(new VertexPositionNormalTexture(MathConverter.Convert(hullTriangleVertices[i]), normal, new Vector2(0, 0)));
vertices.Add(new VertexPositionNormalTexture(MathConverter.Convert(hullTriangleVertices[i + 1]), normal, new Vector2(1, 0)));
vertices.Add(new VertexPositionNormalTexture(MathConverter.Convert(hullTriangleVertices[i + 2]), normal, new Vector2(0, 1)));
indices.Add(i);
indices.Add((ushort)(i + 1));
indices.Add((ushort)(i + 2));
}
}
void ComputeShapeInformation(TransformableMeshData data, out ShapeDistributionInformation shapeInformation)
{
//Compute the surface vertices of the shape.
surfaceVertices.Clear();
try
{
ConvexHullHelper.GetConvexHull(data.vertices, surfaceVertices);
for (int i = 0; i < surfaceVertices.count; i++)
{
AffineTransform.Transform(ref surfaceVertices.Elements[i], ref data.worldTransform, out surfaceVertices.Elements[i]);
}
}
catch
{
surfaceVertices.Clear();
//If the convex hull failed, then the point set has no volume. A mobile mesh is allowed to have zero volume, however.
//In this case, compute the bounding box of all points.
BoundingBox box = new BoundingBox();
for (int i = 0; i < data.vertices.Length; i++)
{
Vector3 v;
data.GetVertexPosition(i, out v);
if (v.X > box.Max.X)
{
box.Max.X = v.X;
}
if (v.X < box.Min.X)
{
box.Min.X = v.X;
}
if (v.Y > box.Max.Y)
{
box.Max.Y = v.Y;
}
if (v.Y < box.Min.Y)
{
box.Min.Y = v.Y;
}
if (v.Z > box.Max.Z)
{
box.Max.Z = v.Z;
}
if (v.Z < box.Min.Z)
{
box.Min.Z = v.Z;
}
}
//Add the corners. This will overestimate the size of the surface a bit.
surfaceVertices.Add(box.Min);
surfaceVertices.Add(box.Max);
surfaceVertices.Add(new Vector3(box.Min.X, box.Min.Y, box.Max.Z));
surfaceVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Min.Z));
surfaceVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Min.Z));
surfaceVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Max.Z));
surfaceVertices.Add(new Vector3(box.Max.X, box.Max.Y, box.Min.Z));
surfaceVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Max.Z));
}
shapeInformation.Center = new Vector3();
if (solidity == MobileMeshSolidity.Solid)
{
//The following inertia tensor calculation assumes a closed mesh.
shapeInformation.Volume = 0;
for (int i = 0; i < data.indices.Length; i += 3)
{
Vector3 v2, v3, v4;
data.GetTriangle(i, out v2, out v3, out v4);
//Determinant is 6 * volume. It's signed, though; this is because the mesh isn't necessarily convex nor centered on the origin.
float tetrahedronVolume = v2.X * (v3.Y * v4.Z - v3.Z * v4.Y) -
v3.X * (v2.Y * v4.Z - v2.Z * v4.Y) +
v4.X * (v2.Y * v3.Z - v2.Z * v3.Y);
shapeInformation.Volume += tetrahedronVolume;
shapeInformation.Center += tetrahedronVolume * (v2 + v3 + v4);
}
shapeInformation.Center /= shapeInformation.Volume * 4;
shapeInformation.Volume /= 6;
shapeInformation.Volume = Math.Abs(shapeInformation.Volume);
data.worldTransform.Translation -= shapeInformation.Center;
//Source: Explicit Exact Formulas for the 3-D Tetrahedron Inertia Tensor in Terms of its Vertex Coordinates
//http://www.scipub.org/fulltext/jms2/jms2118-11.pdf
//x1, x2, x3, x4 are origin, triangle1, triangle2, triangle3
//Looking to find inertia tensor matrix of the form
// [ a -b' -c' ]
// [ -b' b -a' ]
// [ -c' -a' c ]
float a = 0, b = 0, c = 0, ao = 0, bo = 0, co = 0;
float totalWeight = 0;
for (int i = 0; i < data.indices.Length; i += 3)
{
Vector3 v2, v3, v4;
data.GetTriangle(i, out v2, out v3, out v4);
//Determinant is 6 * volume. It's signed, though; this is because the mesh isn't necessarily convex nor centered on the origin.
float tetrahedronVolume = v2.X * (v3.Y * v4.Z - v3.Z * v4.Y) -
v3.X * (v2.Y * v4.Z - v2.Z * v4.Y) +
v4.X * (v2.Y * v3.Z - v2.Z * v3.Y);
totalWeight += tetrahedronVolume;
a += tetrahedronVolume * (v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y +
v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z);
b += tetrahedronVolume * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X +
v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z);
c += tetrahedronVolume * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X +
v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y);
ao += tetrahedronVolume * (2 * v2.Y * v2.Z + v3.Y * v2.Z + v4.Y * v2.Z + v2.Y * v3.Z + 2 * v3.Y * v3.Z + v4.Y * v3.Z + v2.Y * v4.Z + v3.Y * v4.Z + 2 * v4.Y * v4.Z);
bo += tetrahedronVolume * (2 * v2.X * v2.Z + v3.X * v2.Z + v4.X * v2.Z + v2.X * v3.Z + 2 * v3.X * v3.Z + v4.X * v3.Z + v2.X * v4.Z + v3.X * v4.Z + 2 * v4.X * v4.Z);
co += tetrahedronVolume * (2 * v2.X * v2.Y + v3.X * v2.Y + v4.X * v2.Y + v2.X * v3.Y + 2 * v3.X * v3.Y + v4.X * v3.Y + v2.X * v4.Y + v3.X * v4.Y + 2 * v4.X * v4.Y);
}
float density = 1 / totalWeight;
float diagonalFactor = density / 10;
float offFactor = -density / 20;
a *= diagonalFactor;
b *= diagonalFactor;
c *= diagonalFactor;
ao *= offFactor;
bo *= offFactor;
co *= offFactor;
shapeInformation.VolumeDistribution = new Matrix3X3(a, bo, co,
bo, b, ao,
co, ao, c);
}
else
{
shapeInformation.Center = new Vector3();
float totalWeight = 0;
for (int i = 0; i < data.indices.Length; i += 3)
{ //Configure the inertia tensor to be local.
Vector3 vA, vB, vC;
data.GetTriangle(i, out vA, out vB, out vC);
Vector3 vAvB;
Vector3 vAvC;
Vector3.Subtract(ref vB, ref vA, out vAvB);
Vector3.Subtract(ref vC, ref vA, out vAvC);
Vector3 cross;
Vector3.Cross(ref vAvB, ref vAvC, out cross);
float weight = cross.Length();
totalWeight += weight;
shapeInformation.Center += weight * (vA + vB + vC) / 3;
}
shapeInformation.Center /= totalWeight;
shapeInformation.Volume = 0;
data.worldTransform.Translation -= shapeInformation.Center;
shapeInformation.VolumeDistribution = new Matrix3X3();
for (int i = 0; i < data.indices.Length; i += 3)
{ //Configure the inertia tensor to be local.
Vector3 vA, vB, vC;
data.GetTriangle(i, out vA, out vB, out vC);
Vector3 vAvB;
Vector3 vAvC;
Vector3.Subtract(ref vB, ref vA, out vAvB);
Vector3.Subtract(ref vC, ref vA, out vAvC);
Vector3 cross;
Vector3.Cross(ref vAvB, ref vAvC, out cross);
float weight = cross.Length();
totalWeight += weight;
Matrix3X3 innerProduct;
Matrix3X3.CreateScale(vA.LengthSquared(), out innerProduct);
Matrix3X3 outerProduct;
Matrix3X3.CreateOuterProduct(ref vA, ref vA, out outerProduct);
Matrix3X3 contribution;
Matrix3X3.Subtract(ref innerProduct, ref outerProduct, out contribution);
Matrix3X3.Multiply(ref contribution, weight, out contribution);
Matrix3X3.Add(ref shapeInformation.VolumeDistribution, ref contribution, out shapeInformation.VolumeDistribution);
Matrix3X3.CreateScale(vB.LengthSquared(), out innerProduct);
Matrix3X3.CreateOuterProduct(ref vB, ref vB, out outerProduct);
Matrix3X3.Subtract(ref innerProduct, ref outerProduct, out outerProduct);
Matrix3X3.Multiply(ref contribution, weight, out contribution);
Matrix3X3.Add(ref shapeInformation.VolumeDistribution, ref contribution, out shapeInformation.VolumeDistribution);
Matrix3X3.CreateScale(vC.LengthSquared(), out innerProduct);
Matrix3X3.CreateOuterProduct(ref vC, ref vC, out outerProduct);
Matrix3X3.Subtract(ref innerProduct, ref outerProduct, out contribution);
Matrix3X3.Multiply(ref contribution, weight, out contribution);
Matrix3X3.Add(ref shapeInformation.VolumeDistribution, ref contribution, out shapeInformation.VolumeDistribution);
}
Matrix3X3.Multiply(ref shapeInformation.VolumeDistribution, 1 / (6 * totalWeight), out shapeInformation.VolumeDistribution);
}
////Configure the inertia tensor to be local.
//Vector3 finalOffset = shapeInformation.Center;
//Matrix3X3 finalInnerProduct;
//Matrix3X3.CreateScale(finalOffset.LengthSquared(), out finalInnerProduct);
//Matrix3X3 finalOuterProduct;
//Matrix3X3.CreateOuterProduct(ref finalOffset, ref finalOffset, out finalOuterProduct);
//Matrix3X3 finalContribution;
//Matrix3X3.Subtract(ref finalInnerProduct, ref finalOuterProduct, out finalContribution);
//Matrix3X3.Subtract(ref shapeInformation.VolumeDistribution, ref finalContribution, out shapeInformation.VolumeDistribution);
}
public override void Update(Fix64 dt)
{
if (Game.KeyboardInput.IsKeyDown(Keys.Left))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Forward, .01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Right))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Forward, -.01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Down))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Right, .01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.Up))
{
rayCastDirection = Matrix3x3.Transform(rayCastDirection, Matrix3x3.CreateFromAxisAngle(Vector3.Right, -.01m));
}
if (Game.KeyboardInput.IsKeyDown(Keys.P))
{
Debug.WriteLine("Break.");
}
base.Update(dt);
RigidTransform localTransformB;
RigidTransform aTransform = a.CollisionInformation.WorldTransform, bTransform = b.CollisionInformation.WorldTransform;
MinkowskiToolbox.GetLocalTransform(ref aTransform, ref bTransform, out localTransformB);
Vector3 position;
if (MPRToolbox.GetLocalOverlapPosition((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, out position))
{
//Vector3 rayCastDirection = new Vector3(1,0,0);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2;
Fix64 previousT;
Vector3 previousNormal;
Fix64 t;
Vector3 normal;
rayCastDirection = localTransformB.Position;
MPRToolbox.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref rayCastDirection, out previousT, out previousNormal);
//Vector3 secondDirection = Vector3.Cross(rayCastDirection, Vector3.Up);
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref secondDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 thirdDirection = Vector3.Cross(secondDirection, rayCastDirection);
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref thirdDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 fourthDirection = -secondDirection;
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref fourthDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Vector3 fifthDirection = -thirdDirection;
//MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref fifthDirection, out t, out normal);
//if (t < previousT)
//{
// previousNormal = normal;
// previousT = t;
//}
//Correct the penetration depth.
MPRToolbox.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref previousNormal, out t, out normal);
contactDepth = t;
contactNormal = previousNormal;
////Converge to local minimum.
//while (true)
//{
// MPRTesting.LocalSurfaceCast((a.CollisionInformation.Shape as ConvexShape), (b.CollisionInformation.Shape as ConvexShape), ref localTransformB, ref previousNormal, out t, out normal);
// if (previousT - t <= Toolbox.BigEpsilon)
// break;
// previousT = t;
// previousNormal = normal;
//}
}
#region Box Box minkowski sum
////Construct explicit minkowski sum.
//Vector3[] aLines = new Vector3[8];
//aLines[0] = new Vector3(-boxWidth / 2, -boxHeight / 2, -boxLength / 2);
//aLines[1] = new Vector3(-boxWidth / 2, -boxHeight / 2, boxLength / 2);
//aLines[2] = new Vector3(-boxWidth / 2, boxHeight / 2, -boxLength / 2);
//aLines[3] = new Vector3(-boxWidth / 2, boxHeight / 2, boxLength / 2);
//aLines[4] = new Vector3(boxWidth / 2, -boxHeight / 2, -boxLength / 2);
//aLines[5] = new Vector3(boxWidth / 2, -boxHeight / 2, boxLength / 2);
//aLines[6] = new Vector3(boxWidth / 2, boxHeight / 2, -boxLength / 2);
//aLines[7] = new Vector3(boxWidth / 2, boxHeight / 2, boxLength / 2);
//Vector3[] bLines = new Vector3[8];
//bLines[0] = new Vector3(-groundWidth / 2, -groundHeight / 2, -groundLength / 2);
//bLines[1] = new Vector3(-groundWidth / 2, -groundHeight / 2, groundLength / 2);
//bLines[2] = new Vector3(-groundWidth / 2, groundHeight / 2, -groundLength / 2);
//bLines[3] = new Vector3(-groundWidth / 2, groundHeight / 2, groundLength / 2);
//bLines[4] = new Vector3(groundWidth / 2, -groundHeight / 2, -groundLength / 2);
//bLines[5] = new Vector3(groundWidth / 2, -groundHeight / 2, groundLength / 2);
//bLines[6] = new Vector3(groundWidth / 2, groundHeight / 2, -groundLength / 2);
//bLines[7] = new Vector3(groundWidth / 2, groundHeight / 2, groundLength / 2);
//for (int i = 0; i < 8; i++)
// aLines[i] = Vector3.Transform(aLines[i], localTransformB.Matrix);
//List<Vector3> vertices = new List<Vector3>();
//for (int i = 0; i < 8; i++)
//{
// for (int j = 0; j < 8; j++)
// {
// if (b.CollisionInformation.Pairs.Count > 0)
// {
// if (b.CollisionInformation.Pairs[0].BroadPhaseOverlap.EntryA == b.CollisionInformation)
// vertices.Add(aLines[i] - bLines[j]);
// else
// vertices.Add(bLines[i] - aLines[j]);
// }
// else
// {
// vertices.Add(bLines[i] - aLines[j]);
// }
// }
//}
//var indices = new List<int>();
//Toolbox.GetConvexHull(vertices, indices);
#endregion
#region Arbitrary minkowski sum
var vertices = new List <Vector3>();
Vector3 max;
var direction = new Vector3();
int NumSamples = 16;
Fix64 angleChange = MathHelper.TwoPi / NumSamples;
for (int i = 1; i < NumSamples / 2 - 1; i++)
{
Fix64 phi = MathHelper.PiOver2 - i * angleChange;
var sinPhi = Fix64.Sin(phi);
var cosPhi = Fix64.Cos(phi);
for (int j = 0; j < NumSamples; j++)
{
Fix64 theta = j * angleChange;
direction.X = Fix64.Cos(theta) * cosPhi;
direction.Y = sinPhi;
direction.Z = Fix64.Sin(theta) * cosPhi;
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref direction, ref localTransformB, out max);
vertices.Add(max);
}
}
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref Toolbox.UpVector, ref localTransformB, out max);
vertices.Add(max);
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(a.CollisionInformation.Shape as ConvexShape, b.CollisionInformation.Shape as ConvexShape, ref Toolbox.DownVector, ref localTransformB, out max);
vertices.Add(max);
var indices = new List <int>();
ConvexHullHelper.GetConvexHull(vertices, indices);
#endregion
minkowskiLines.Clear();
for (int i = 0; i < indices.Count; i += 3)
{
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 1]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 1]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 2]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i + 2]]), Microsoft.Xna.Framework.Color.Blue));
minkowskiLines.Add(new VertexPositionColor(MathConverter.Convert(vertices[indices[i]]), Microsoft.Xna.Framework.Color.Blue));
}
}
private void UpdateSurfaceVertices()
{
hullVertices.Clear();
if (Volume > F64.C0)
{
ConvexHullHelper.GetConvexHull(triangleMesh.Data.vertices, hullVertices);
var transformableData = triangleMesh.Data as TransformableMeshData;
if (transformableData != null)
{
var transform = transformableData.worldTransform;
for (int i = 0; i < hullVertices.Count; i++)
{
AffineTransform.Transform(ref hullVertices.Elements[i], ref transform, out hullVertices.Elements[i]);
}
}
}
else
{
hullVertices.Clear();
//A mobile mesh is allowed to have zero volume, so long as it isn't solid.
//In this case, compute the bounding box of all points.
BoundingBox box = new BoundingBox();
for (int i = 0; i < triangleMesh.Data.vertices.Length; i++)
{
Vector3 v;
triangleMesh.Data.GetVertexPosition(i, out v);
if (v.X > box.Max.X)
{
box.Max.X = v.X;
}
if (v.X < box.Min.X)
{
box.Min.X = v.X;
}
if (v.Y > box.Max.Y)
{
box.Max.Y = v.Y;
}
if (v.Y < box.Min.Y)
{
box.Min.Y = v.Y;
}
if (v.Z > box.Max.Z)
{
box.Max.Z = v.Z;
}
if (v.Z < box.Min.Z)
{
box.Min.Z = v.Z;
}
}
//Add the corners. This will overestimate the size of the surface a bit.
hullVertices.Add(box.Min);
hullVertices.Add(box.Max);
hullVertices.Add(new Vector3(box.Min.X, box.Min.Y, box.Max.Z));
hullVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Min.Z));
hullVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Min.Z));
hullVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Max.Z));
hullVertices.Add(new Vector3(box.Max.X, box.Max.Y, box.Min.Z));
hullVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Max.Z));
}
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(0, 10, 40);
camera.Yaw = 0;
camera.Pitch = 0;
Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)));
var box = new Box(2f, 2f, 2f);
var capsule = new Capsule(1f, 1f);
var sphere = new Sphere(1.5f);
box.ComputeInertia(1, out var boxInertia);
capsule.ComputeInertia(1, out var capsuleInertia);
sphere.ComputeInertia(1, out var sphereInertia);
var boxIndex = Simulation.Shapes.Add(box);
var capsuleIndex = Simulation.Shapes.Add(capsule);
var sphereIndex = Simulation.Shapes.Add(sphere);
const int width = 12;
const int height = 3;
const int length = 12;
for (int i = 0; i < width; ++i)
{
for (int j = 0; j < height; ++j)
{
for (int k = 0; k < length; ++k)
{
var location = new Vector3(5, 5, 5) * new Vector3(i, j, k) + new Vector3(-width * 2.5f, 2.5f, -length * 2.5f);
var bodyDescription = new BodyDescription
{
Activity = new BodyActivityDescription {
MinimumTimestepCountUnderThreshold = 32, SleepThreshold = 0.1f
},
Pose = new RigidPose
{
Orientation = Quaternion.Identity,
Position = location
},
Collidable = new CollidableDescription
{
Continuity = new ContinuousDetectionSettings {
Mode = ContinuousDetectionMode.Discrete
},
SpeculativeMargin = 0.1f
}
};
switch (j % 3)
{
case 0:
bodyDescription.Collidable.Shape = boxIndex;
bodyDescription.LocalInertia = boxInertia;
break;
case 1:
bodyDescription.Collidable.Shape = capsuleIndex;
bodyDescription.LocalInertia = capsuleInertia;
break;
case 2:
bodyDescription.Collidable.Shape = sphereIndex;
bodyDescription.LocalInertia = sphereInertia;
break;
}
Simulation.Bodies.Add(bodyDescription);
}
}
}
//Don't really want to regenerate a convex hull every frame; just cache one out.
const int pointCount = 32;
var points = new QuickList <Vector3>(pointCount, BufferPool);
var random = new Random(5);
for (int i = 0; i < pointCount; ++i)
{
points.AllocateUnsafely() = new Vector3((float)random.NextDouble() - 0.5f, (float)random.NextDouble() - 0.5f, (float)random.NextDouble() - 0.5f);
}
ConvexHullHelper.CreateShape(points.Span.Slice(0, points.Count), BufferPool, out _, out hull);
points.Dispose(BufferPool);
//var staticShapeIndex = Simulation.Shapes.Add(new Box(100, 1, 100));
//var staticDescription = new StaticDescription
//{
// Collidable = new CollidableDescription
// {
// Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete },
// Shape = Simulation.Shapes.Add(new Box(100, 1, 100)),
// SpeculativeMargin = 0.1f
// },
// Pose = new RigidPose { Position = new Vector3(0, -1, 0), Orientation = Quaternion.Identity }
//};
//Simulation.Statics.Add(staticDescription);
const int planeWidth = 64;
const int planeHeight = 64;
DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight,
(int x, int y) =>
{
return(new Vector3(x, 1 * (float)Math.Cos(x / 4f) * (float)Math.Sin(y / 4f), y));
}, new Vector3(2, 3, 2), BufferPool, out var planeMesh);
Simulation.Statics.Add(new StaticDescription(new Vector3(-64, -10, -64), new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));
}