///<summary>
/// Constructs a new mobile mesh shape.
///</summary>
///<param name="vertices">Vertices of the mesh.</param>
///<param name="indices">Indices of the mesh.</param>
///<param name="localTransform">Local transform to apply to the shape.</param>
///<param name="solidity">Solidity state of the shape.</param>
///<param name="distributionInfo">Information computed about the shape during construction.</param>
public MobileMeshShape(Vector3[] vertices, uint[] indices, AffineTransform localTransform, MobileMeshSolidity solidity, out ShapeDistributionInformation distributionInfo)
{
this.solidity = solidity;
var data = new TransformableMeshData(vertices, indices, indices.Length, localTransform);
ComputeShapeInformation(data, out distributionInfo);
for (int i = 0; i < surfaceVertices.Count; i++)
{
Vector3.Subtract(ref surfaceVertices.Elements[i], ref distributionInfo.Center, out surfaceVertices.Elements[i]);
}
triangleMesh = new TriangleMesh(data);
ComputeSolidSidedness();
//ComputeBoundingHull();
}
/// <summary>
/// Precomputes the transform to bring triangles from their native local space to the local space of the convex.
/// </summary>
/// <param name="convexInverseWorldTransform">Inverse of the world transform of the convex shape.</param>
/// <param name="fromMeshLocalToConvexLocal">Transform to apply to native local triangles to bring them into the local space of the convex.</param>
protected override void PrecomputeTriangleTransform(ref AffineTransform convexInverseWorldTransform,
out AffineTransform fromMeshLocalToConvexLocal)
{
//MobileMeshes only have TransformableMeshData sources.
TransformableMeshData data = (TransformableMeshData)mesh.Shape.TriangleMesh.Data;
//The mobile mesh has a shape-based transform followed by the rigid body transform.
AffineTransform mobileMeshWorldTransform;
AffineTransform.CreateFromRigidTransform(ref mesh.worldTransform, out mobileMeshWorldTransform);
AffineTransform combinedMobileMeshWorldTransform;
AffineTransform.Multiply(ref data.worldTransform, ref mobileMeshWorldTransform,
out combinedMobileMeshWorldTransform);
AffineTransform.Multiply(ref combinedMobileMeshWorldTransform, ref convexInverseWorldTransform,
out fromMeshLocalToConvexLocal);
}
///<summary>
/// Constructs a new mobile mesh shape.
///</summary>
///<param name="vertices">Vertices of the mesh.</param>
///<param name="indices">Indices of the mesh.</param>
///<param name="localTransform">Local transform to apply to the shape.</param>
///<param name="solidity">Solidity state of the shape.</param>
public MobileMeshShape(Vector3[] vertices, int[] indices, AffineTransform localTransform, MobileMeshSolidity solidity)
{
this.solidity = solidity;
var data = new TransformableMeshData(vertices, indices, localTransform);
var shapeDistributionInformation = ComputeVolumeDistribution(data);
data.worldTransform.Translation -= shapeDistributionInformation.Center;
triangleMesh = new TriangleMesh(data);
UpdateEntityShapeVolume(new EntityShapeVolumeDescription {
Volume = shapeDistributionInformation.Volume, VolumeDistribution = shapeDistributionInformation.VolumeDistribution
});
ComputeSolidSidedness();
UpdateSurfaceVertices();
}
///<summary>
/// Constructs a new StaticMeshShape.
///</summary>
///<param name="vertices">Vertices of the mesh.</param>
///<param name="indices">Indices of the mesh.</param>
public StaticMeshShape(Vector3[] vertices, int[] indices)
{
triangleMeshData = new TransformableMeshData(vertices, indices);
}
///<summary>
/// Constructs a new StaticMeshShape.
///</summary>
///<param name="vertices">Vertices of the mesh.</param>
///<param name="indices">Indices of the mesh.</param>
///<param name="worldTransform">World transform to use in the local space data.</param>
public StaticMeshShape(Vector3[] vertices, int[] indices, AffineTransform worldTransform)
{
triangleMeshData = new TransformableMeshData(vertices, indices, worldTransform);
}
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);
}
/// <summary>
/// Recenters the triangle data and computes the volume distribution.
/// </summary>
/// <param name="data">Mesh data to analyze.</param>
/// <returns>Computed center, volume, and volume distribution.</returns>
private ShapeDistributionInformation ComputeVolumeDistribution(TransformableMeshData data)
{
//Compute the surface vertices of the shape.
ShapeDistributionInformation shapeInformation;
if (solidity == MobileMeshSolidity.Solid)
{
//The following inertia tensor calculation assumes a closed mesh.
var transformedVertices = CommonResources.GetVectorList();
if (transformedVertices.Capacity < data.vertices.Length)
{
transformedVertices.Capacity = data.vertices.Length;
}
transformedVertices.Count = data.vertices.Length;
for (int i = 0; i < data.vertices.Length; ++i)
{
data.GetVertexPosition(i, out transformedVertices.Elements[i]);
}
InertiaHelper.ComputeShapeDistribution(transformedVertices, data.indices, out shapeInformation.Center, out shapeInformation.Volume, out shapeInformation.VolumeDistribution);
CommonResources.GiveBack(transformedVertices);
if (shapeInformation.Volume > F64.C0)
{
return(shapeInformation);
}
throw new ArgumentException("A solid mesh must have volume.");
}
shapeInformation.Center = new Vector3();
shapeInformation.VolumeDistribution = new Matrix3x3();
Fix64 totalWeight = F64.C0;
for (int i = 0; i < data.indices.Length; i += 3)
{
//Compute the center contribution.
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);
Fix64 weight = cross.Length();
totalWeight += weight;
Fix64 perVertexWeight = weight * F64.OneThird;
shapeInformation.Center += perVertexWeight * (vA + vB + vC);
//Compute the inertia contribution of this triangle.
//Approximate it using pointmasses positioned at the triangle vertices.
//(There exists a direct solution, but this approximation will do plenty fine.)
Matrix3x3 aContribution, bContribution, cContribution;
InertiaHelper.GetPointContribution(perVertexWeight, ref Toolbox.ZeroVector, ref vA, out aContribution);
InertiaHelper.GetPointContribution(perVertexWeight, ref Toolbox.ZeroVector, ref vB, out bContribution);
InertiaHelper.GetPointContribution(perVertexWeight, ref Toolbox.ZeroVector, ref vC, out cContribution);
Matrix3x3.Add(ref aContribution, ref shapeInformation.VolumeDistribution, out shapeInformation.VolumeDistribution);
Matrix3x3.Add(ref bContribution, ref shapeInformation.VolumeDistribution, out shapeInformation.VolumeDistribution);
Matrix3x3.Add(ref cContribution, ref shapeInformation.VolumeDistribution, out shapeInformation.VolumeDistribution);
}
shapeInformation.Center /= totalWeight;
//The extra factor of 2 is used because the cross product length was twice the actual area.
Matrix3x3.Multiply(ref shapeInformation.VolumeDistribution, F64.C1 / (F64.C2 * totalWeight), out shapeInformation.VolumeDistribution);
//Move the inertia tensor into position according to the center.
Matrix3x3 additionalInertia;
InertiaHelper.GetPointContribution(F64.C0p5, ref Toolbox.ZeroVector, ref shapeInformation.Center, out additionalInertia);
Matrix3x3.Subtract(ref shapeInformation.VolumeDistribution, ref additionalInertia, out shapeInformation.VolumeDistribution);
shapeInformation.Volume = F64.C0;
return(shapeInformation);
}
///<summary>
/// Constructs a new StaticMeshShape.
///</summary>
///<param name="vertices">Vertices of the mesh.</param>
///<param name="indices">Indices of the mesh.</param>
public StaticMeshShape(Vector3 globalMove, List <VertexCubeSolid> vertices, List <ushort> indices)
{
triangleMeshData = new TransformableMeshData(globalMove, vertices, indices);
}
