static void SplitReposition(Entity a, Entity b, ref ShapeDistributionInformation distributionInfoA, ref ShapeDistributionInformation distributionInfoB, float weightA, float weightB)
{
//The compounds are not aligned with the original's position yet.
//In order to align them, first look at the centers the split method computed.
//They are offsets from the center of the original shape in local space.
//These can be used to reposition the objects in world space.
Vector3 weightedA, weightedB;
Vector3.Multiply(ref distributionInfoA.Center, weightA, out weightedA);
Vector3.Multiply(ref distributionInfoB.Center, weightB, out weightedB);
Vector3 newLocalCenter;
Vector3.Add(ref weightedA, ref weightedB, out newLocalCenter);
Vector3.Divide(ref newLocalCenter, weightA + weightB, out newLocalCenter);
Vector3 localOffsetA;
Vector3 localOffsetB;
Vector3.Subtract(ref distributionInfoA.Center, ref newLocalCenter, out localOffsetA);
Vector3.Subtract(ref distributionInfoB.Center, ref newLocalCenter, out localOffsetB);
Vector3 originalPosition = a.position;
b.Orientation = a.Orientation;
Vector3 offsetA = Quaternion.Transform(localOffsetA, a.Orientation);
Vector3 offsetB = Quaternion.Transform(localOffsetB, a.Orientation);
a.Position = originalPosition + offsetA;
b.Position = originalPosition + offsetB;
Vector3 originalLinearVelocity = a.linearVelocity;
Vector3 originalAngularVelocity = a.angularVelocity;
a.AngularVelocity = originalAngularVelocity;
b.AngularVelocity = originalAngularVelocity;
a.LinearVelocity = originalLinearVelocity + Vector3.Cross(originalAngularVelocity, offsetA);
b.LinearVelocity = originalLinearVelocity + Vector3.Cross(originalAngularVelocity, offsetB);
}
/// <summary>
/// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation.
/// </summary>
/// <param name="childContributions">List of distribution information associated with each child shape of the whole compound shape used by the compound being split.</param>
/// <param name="splitPredicate">Delegate which determines if a child in the original compound should be moved to the new compound.</param>
/// <param name="a">Original compound to be split. Children in this compound will be removed and added to the other compound.</param>
/// <param name="b">Compound to receive children removed from the original compound.</param>
/// <param name="distributionInfoA">Volume, volume distribution, and center information about the new form of the original compound collidable.</param>
/// <param name="distributionInfoB">Volume, volume distribution, and center information about the new compound collidable.</param>
/// <returns>Whether or not the predicate returned true for any element in the original compound and split the compound.</returns>
public static bool SplitCompound(IList<ShapeDistributionInformation> childContributions, Func<CompoundChild, bool> splitPredicate,
Entity<CompoundCollidable> a, out Entity<CompoundCollidable> b,
out ShapeDistributionInformation distributionInfoA, out ShapeDistributionInformation distributionInfoB)
{
var bCollidable = new CompoundCollidable { Shape = a.CollisionInformation.Shape };
b = null;
float weightA, weightB;
if (SplitCompound(childContributions, splitPredicate, a.CollisionInformation, bCollidable, out distributionInfoA, out distributionInfoB, out weightA, out weightB))
{
//Reconfigure the entities using the data computed in the split.
float originalMass = a.mass;
if (a.CollisionInformation.children.Count > 0)
{
float newMassA = (weightA / (weightA + weightB)) * originalMass;
Matrix3x3.Multiply(ref distributionInfoA.VolumeDistribution, newMassA * InertiaHelper.InertiaTensorScale, out distributionInfoA.VolumeDistribution);
a.Initialize(a.CollisionInformation, newMassA, distributionInfoA.VolumeDistribution, distributionInfoA.Volume);
}
if (bCollidable.children.Count > 0)
{
float newMassB = (weightB / (weightA + weightB)) * originalMass;
Matrix3x3.Multiply(ref distributionInfoB.VolumeDistribution, newMassB * InertiaHelper.InertiaTensorScale, out distributionInfoB.VolumeDistribution);
b = new Entity<CompoundCollidable>();
b.Initialize(bCollidable, newMassB, distributionInfoB.VolumeDistribution, distributionInfoB.Volume);
}
SplitReposition(a, b, ref distributionInfoA, ref distributionInfoB, weightA, weightB);
return true;
}
else
return false;
}
/// <summary>
/// Computes and returns the volume, volume distribution, and center contributions from each child shape in the compound shape.
/// </summary>
/// <returns>Volume, volume distribution, and center contributions from each child shape in the compound shape.</returns>
public ShapeDistributionInformation[] ComputeChildContributions()
{
var toReturn = new ShapeDistributionInformation[shapes.count];
for (int i = 0; i < shapes.count; i++)
{
shapes.Elements[i].Shape.ComputeDistributionInformation(out toReturn[i]);
}
return(toReturn);
}
/// <summary>
/// Removes a child from a compound body.
/// </summary>
/// <param name="childContributions">List of distribution information associated with each child shape of the whole compound shape used by the compound being split.</param>
/// <param name="removalPredicate">Delegate which determines if a child in the original compound should be moved to the new compound.</param>
/// <param name="distributionInfo">Volume, volume distribution, and center information about the new form of the original compound collidable.</param>
/// <param name="compound">Original compound to have a child removed.</param>
/// <returns>Whether or not the predicate returned true for any element in the original compound and split the compound.</returns>
public static bool RemoveChildFromCompound(Entity<CompoundCollidable> compound, Func<CompoundChild, bool> removalPredicate, IList<ShapeDistributionInformation> childContributions,
out ShapeDistributionInformation distributionInfo)
{
float weight;
float removedWeight;
Vector3 removedCenter;
if (RemoveChildFromCompound(compound.CollisionInformation, removalPredicate, childContributions, out distributionInfo, out weight, out removedWeight, out removedCenter))
{
//Reconfigure the entities using the data computed in the split.
//Only bother if there are any children left in the compound!
if (compound.CollisionInformation.Children.Count > 0)
{
float originalMass = compound.mass;
float newMass = (weight / (weight + removedWeight)) * originalMass;
Matrix3x3.Multiply(ref distributionInfo.VolumeDistribution, newMass * InertiaHelper.InertiaTensorScale, out distributionInfo.VolumeDistribution);
compound.Initialize(compound.CollisionInformation, newMass, distributionInfo.VolumeDistribution, distributionInfo.Volume);
RemoveReposition(compound, ref distributionInfo, weight, removedWeight, ref removedCenter);
}
return true;
}
else
return false;
}
static void RemoveReposition(Entity compound, ref ShapeDistributionInformation distributionInfo, float weight, float removedWeight, ref Vector3 removedCenter)
{
//The compounds are not aligned with the original's position yet.
//In order to align them, first look at the centers the split method computed.
//They are offsets from the center of the original shape in local space.
//These can be used to reposition the objects in world space.
Vector3 weightedA, weightedB;
Vector3.Multiply(ref distributionInfo.Center, weight, out weightedA);
Vector3.Multiply(ref removedCenter, removedWeight, out weightedB);
Vector3 newLocalCenter;
Vector3.Add(ref weightedA, ref weightedB, out newLocalCenter);
Vector3.Divide(ref newLocalCenter, weight + removedWeight, out newLocalCenter);
Vector3 localOffset;
Vector3.Subtract(ref distributionInfo.Center, ref newLocalCenter, out localOffset);
Vector3 originalPosition = compound.position;
Vector3 offset = Vector3.Transform(localOffset, compound.orientation);
compound.Position = originalPosition + offset;
Vector3 originalLinearVelocity = compound.linearVelocity;
Vector3 originalAngularVelocity = compound.angularVelocity;
compound.AngularVelocity = originalAngularVelocity;
compound.LinearVelocity = originalLinearVelocity + Vector3.Cross(originalAngularVelocity, offset);
}
/// <summary>
/// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation.
/// </summary>
/// <param name="childContributions">List of distribution information associated with each child shape of the whole compound shape used by the compound being split.</param>
/// <param name="splitPredicate">Delegate which determines if a child in the original compound should be moved to the new compound.</param>
/// <param name="a">Original compound to be split. Children in this compound will be removed and added to the other compound.</param>
/// <param name="b">Compound to receive children removed from the original compound.</param>
/// <param name="distributionInfoA">Volume, volume distribution, and center information about the new form of the original compound collidable.</param>
/// <param name="distributionInfoB">Volume, volume distribution, and center information about the new compound collidable.</param>
/// <param name="weightA">Total weight associated with the new form of the original compound collidable.</param>
/// <param name="weightB">Total weight associated with the new compound collidable.</param>
/// <returns>Whether or not the predicate returned true for any element in the original compound and split the compound.</returns>
public static bool SplitCompound(IList<ShapeDistributionInformation> childContributions, Func<CompoundChild, bool> splitPredicate,
CompoundCollidable a, CompoundCollidable b,
out ShapeDistributionInformation distributionInfoA, out ShapeDistributionInformation distributionInfoB,
out float weightA, out float weightB)
{
bool splitOccurred = false;
for (int i = a.children.Count - 1; i >= 0; i--)
{
//The shape doesn't change during this process. The entity could, though.
//All of the other collidable information, like the Tag, CollisionRules, Events, etc. all stay the same.
var child = a.children.Elements[i];
if (splitPredicate(child))
{
splitOccurred = true;
a.children.FastRemoveAt(i);
b.children.Add(child);
//The child event handler must be unhooked from the old compound and given to the new one.
child.CollisionInformation.events.Parent = b.Events;
}
}
if (!splitOccurred)
{
//No split occurred, so we cannot proceed.
distributionInfoA = new ShapeDistributionInformation();
distributionInfoB = new ShapeDistributionInformation();
weightA = 0;
weightB = 0;
return false;
}
//Compute the contributions from the original shape to the new form of the original collidable.
distributionInfoA = new ShapeDistributionInformation();
weightA = 0;
distributionInfoB = new ShapeDistributionInformation();
weightB = 0;
for (int i = a.children.Count - 1; i >= 0; i--)
{
var child = a.children.Elements[i];
var entry = child.Entry;
var contribution = childContributions[child.shapeIndex];
Vector3.Add(ref contribution.Center, ref entry.LocalTransform.Position, out contribution.Center);
Vector3.Multiply(ref contribution.Center, child.Entry.Weight, out contribution.Center);
Vector3.Add(ref contribution.Center, ref distributionInfoA.Center, out distributionInfoA.Center);
distributionInfoA.Volume += contribution.Volume;
weightA += entry.Weight;
}
for (int i = b.children.Count - 1; i >= 0; i--)
{
var child = b.children.Elements[i];
var entry = child.Entry;
var contribution = childContributions[child.shapeIndex];
Vector3.Add(ref contribution.Center, ref entry.LocalTransform.Position, out contribution.Center);
Vector3.Multiply(ref contribution.Center, child.Entry.Weight, out contribution.Center);
Vector3.Add(ref contribution.Center, ref distributionInfoB.Center, out distributionInfoB.Center);
distributionInfoB.Volume += contribution.Volume;
weightB += entry.Weight;
}
//Average the center out.
if (weightA > 0)
Vector3.Divide(ref distributionInfoA.Center, weightA, out distributionInfoA.Center);
if (weightB > 0)
Vector3.Divide(ref distributionInfoB.Center, weightB, out distributionInfoB.Center);
//Note that the 'entry' is from the Shape, and so the translations are local to the shape's center.
//That is not technically the center of the new collidable- distributionInfoA.Center is.
//Offset the child collidables by -distributionInfoA.Center using their local offset.
Vector3 offsetA;
Vector3.Negate(ref distributionInfoA.Center, out offsetA);
Vector3 offsetB;
Vector3.Negate(ref distributionInfoB.Center, out offsetB);
//Compute the unscaled inertia tensor.
for (int i = a.children.Count - 1; i >= 0; i--)
{
var child = a.children.Elements[i];
var entry = child.Entry;
Vector3 transformedOffset;
Quaternion conjugate;
Quaternion.Conjugate(ref entry.LocalTransform.Orientation, out conjugate);
Vector3.Transform(ref offsetA, ref conjugate, out transformedOffset);
child.CollisionInformation.localPosition = transformedOffset;
var contribution = childContributions[child.shapeIndex];
CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfoA.Center, ref contribution.VolumeDistribution, entry.Weight, out contribution.VolumeDistribution);
//Vector3.Add(ref entry.LocalTransform.Position, ref offsetA, out entry.LocalTransform.Position);
Matrix3x3.Add(ref contribution.VolumeDistribution, ref distributionInfoA.VolumeDistribution, out distributionInfoA.VolumeDistribution);
}
for (int i = b.children.Count - 1; i >= 0; i--)
{
var child = b.children.Elements[i];
var entry = child.Entry;
Vector3 transformedOffset;
Quaternion conjugate;
Quaternion.Conjugate(ref entry.LocalTransform.Orientation, out conjugate);
Vector3.Transform(ref offsetB, ref conjugate, out transformedOffset);
child.CollisionInformation.localPosition = transformedOffset;
var contribution = childContributions[child.shapeIndex];
CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfoB.Center, ref contribution.VolumeDistribution, entry.Weight, out contribution.VolumeDistribution);
//Vector3.Add(ref entry.LocalTransform.Position, ref offsetB, out entry.LocalTransform.Position);
Matrix3x3.Add(ref contribution.VolumeDistribution, ref distributionInfoB.VolumeDistribution, out distributionInfoB.VolumeDistribution);
}
//Normalize the volume distribution.
Matrix3x3.Multiply(ref distributionInfoA.VolumeDistribution, 1 / weightA, out distributionInfoA.VolumeDistribution);
Matrix3x3.Multiply(ref distributionInfoB.VolumeDistribution, 1 / weightB, out distributionInfoB.VolumeDistribution);
//Update the hierarchies of the compounds.
//TODO: Create a new method that does this quickly without garbage. Requires a new Reconstruct method which takes a pool which stores the appropriate node types.
a.hierarchy.Tree.Reconstruct(a.children);
b.hierarchy.Tree.Reconstruct(b.children);
return true;
}
/// <summary>
/// Computes the volume, center of mass, and volume distribution of the shape.
/// </summary>
/// <param name="shapeInfo">Data about the shape.</param>
public override void ComputeDistributionInformation(out ShapeDistributionInformation shapeInfo)
{
ComputeShapeInformation(this.TriangleMesh.Data as TransformableMeshData, out shapeInfo);
}
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>
/// 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, int[] indices, AffineTransform localTransform, MobileMeshSolidity solidity, out ShapeDistributionInformation distributionInfo)
{
this.solidity = solidity;
var data = new TransformableMeshData(vertices, indices, 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();
}
static void Reposition(Entity a, Entity b, ref ShapeDistributionInformation distributionInfoA, ref ShapeDistributionInformation distributionInfoB, float weightA, float weightB)
{
//The compounds are not aligned with the original's position yet.
//In order to align them, first look at the centers the split method computed.
//They are offsets from the center of the original shape in local space.
//These can be used to reposition the objects in world space.
Vector3 weightedA, weightedB;
Vector3.Multiply(ref distributionInfoA.Center, weightA, out weightedA);
Vector3.Multiply(ref distributionInfoB.Center, weightB, out weightedB);
Vector3 newLocalCenter;
Vector3.Add(ref weightedA, ref weightedB, out newLocalCenter);
Vector3.Divide(ref newLocalCenter, weightA + weightB, out newLocalCenter);
Vector3 localOffsetA;
Vector3 localOffsetB;
Vector3.Subtract(ref distributionInfoA.Center, ref newLocalCenter, out localOffsetA);
Vector3.Subtract(ref distributionInfoB.Center, ref newLocalCenter, out localOffsetB);
Vector3 originalPosition = a.position;
b.Orientation = a.Orientation;
//Imagine a split that does not occur right down the middle, but along a far side: (oo - (o - o))
//Both distributions will be positive. Only the first split works properly because there the position
//was aligned with the local space origin.
Vector3 offsetA = Vector3.Transform(localOffsetA, a.Orientation);
Vector3 offsetB = Vector3.Transform(localOffsetB, a.Orientation);
a.Position = originalPosition + offsetA;
b.Position = originalPosition + offsetB;
Vector3 originalLinearVelocity = a.linearVelocity;
Vector3 originalAngularVelocity = a.angularVelocity;
a.AngularVelocity = originalAngularVelocity;
b.AngularVelocity = originalAngularVelocity;
a.LinearVelocity = originalLinearVelocity + Vector3.Cross(originalAngularVelocity, offsetA);
b.LinearVelocity = originalLinearVelocity + Vector3.Cross(originalAngularVelocity, offsetB);
}
/// <summary> /// Computes a variety of shape information all at once. /// </summary> /// <param name="shapeInfo">Properties of the shape.</param> public abstract void ComputeDistributionInformation(out ShapeDistributionInformation shapeInfo);
/// <summary>
/// Removes a child from a compound collidable.
/// </summary>
/// <param name="compound">Compound collidable to remove a child from.</param>
/// <param name="removalPredicate">Callback which analyzes a child and determines if it should be removed from the compound.</param>
/// <param name="childContributions">Distribution contributions from all shapes in the compound shape. This can include shapes which are not represented in the compound.</param>
/// <param name="distributionInfo">Distribution information of the new compound.</param>
/// <param name="weight">Total weight of the new compound.</param>
/// <param name="removedWeight">Weight removed from the compound.</param>
/// <param name="removedCenter">Center of the chunk removed from the compound.</param>
/// <returns>Whether or not any removal took place.</returns>
public static bool RemoveChildFromCompound(CompoundCollidable compound, Func<CompoundChild, bool> removalPredicate, IList<ShapeDistributionInformation> childContributions,
out ShapeDistributionInformation distributionInfo, out float weight, out float removedWeight, out Vector3 removedCenter)
{
bool removalOccurred = false;
removedWeight = 0;
removedCenter = new Vector3();
for (int i = compound.children.Count - 1; i >= 0; i--)
{
//The shape doesn't change during this process. The entity could, though.
//All of the other collidable information, like the Tag, CollisionRules, Events, etc. all stay the same.
var child = compound.children.Elements[i];
if (removalPredicate(child))
{
removalOccurred = true;
var entry = child.Entry;
removedWeight += entry.Weight;
Vector3 toAdd;
Vector3.Multiply(ref entry.LocalTransform.Position, entry.Weight, out toAdd);
Vector3.Add(ref removedCenter, ref toAdd, out removedCenter);
//The child event handler must be unhooked from the compound.
child.CollisionInformation.events.Parent = null;
compound.children.FastRemoveAt(i);
}
}
if (!removalOccurred)
{
//No removal occurred, so we cannot proceed.
distributionInfo = new ShapeDistributionInformation();
weight = 0;
return false;
}
if (removedWeight > 0)
{
Vector3.Divide(ref removedCenter, removedWeight, out removedCenter);
}
//Compute the contributions from the original shape to the new form of the original collidable.
distributionInfo = new ShapeDistributionInformation();
weight = 0;
for (int i = compound.children.Count - 1; i >= 0; i--)
{
var child = compound.children.Elements[i];
var entry = child.Entry;
var contribution = childContributions[child.shapeIndex];
Vector3.Add(ref contribution.Center, ref entry.LocalTransform.Position, out contribution.Center);
Vector3.Multiply(ref contribution.Center, child.Entry.Weight, out contribution.Center);
Vector3.Add(ref contribution.Center, ref distributionInfo.Center, out distributionInfo.Center);
distributionInfo.Volume += contribution.Volume;
weight += entry.Weight;
}
//Average the center out.
Vector3.Divide(ref distributionInfo.Center, weight, out distributionInfo.Center);
//Note that the 'entry' is from the Shape, and so the translations are local to the shape's center.
//That is not technically the center of the new collidable- distributionInfo.Center is.
//Offset the child collidables by -distributionInfo.Center using their local offset.
Vector3 offset;
Vector3.Negate(ref distributionInfo.Center, out offset);
//Compute the unscaled inertia tensor.
for (int i = compound.children.Count - 1; i >= 0; i--)
{
var child = compound.children.Elements[i];
var entry = child.Entry;
Vector3 transformedOffset;
Quaternion conjugate;
Quaternion.Conjugate(ref entry.LocalTransform.Orientation, out conjugate);
Vector3.Transform(ref offset, ref conjugate, out transformedOffset);
child.CollisionInformation.localPosition = transformedOffset;
var contribution = childContributions[child.shapeIndex];
CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfo.Center, ref contribution.VolumeDistribution, entry.Weight, out contribution.VolumeDistribution);
//Vector3.Add(ref entry.LocalTransform.Position, ref offsetA, out entry.LocalTransform.Position);
Matrix3x3.Add(ref contribution.VolumeDistribution, ref distributionInfo.VolumeDistribution, out distributionInfo.VolumeDistribution);
}
//Normalize the volume distribution.
Matrix3x3.Multiply(ref distributionInfo.VolumeDistribution, 1 / weight, out distributionInfo.VolumeDistribution);
//Update the hierarchies of the compounds.
//TODO: Create a new method that does this quickly without garbage. Requires a new Reconstruct method which takes a pool which stores the appropriate node types.
compound.hierarchy.Tree.Reconstruct(compound.children);
return true;
}
/// <summary>
/// Computes the volume, center of mass, and volume distribution of the shape.
/// </summary>
/// <param name="shapeInfo">Data about the shape.</param>
public override void ComputeDistributionInformation(out ShapeDistributionInformation shapeInfo)
{
ComputeShapeInformation(this.TriangleMesh.Data as TransformableMeshData, out shapeInfo);
}
void ComputeShapeInformation(TransformableMeshData data, out ShapeDistributionInformation shapeInformation)
{
var indices = Resources.GetIntList();
surfaceVertices.Clear();
Toolbox.GetConvexHull(data.vertices, indices, surfaceVertices);
for (int i = 0; i < surfaceVertices.count; i++)
{
AffineTransform.Transform(ref surfaceVertices.Elements[i], ref data.worldTransform, out surfaceVertices.Elements[i]);
}
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>
/// 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, int[] indices, AffineTransform localTransform, MobileMeshSolidity solidity, out ShapeDistributionInformation distributionInfo)
{
this.solidity = solidity;
var data = new TransformableMeshData(vertices, indices, 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>
/// Computes and returns the volume, volume distribution, and center contributions from each child shape in the compound shape.
/// </summary>
/// <returns>Volume, volume distribution, and center contributions from each child shape in the compound shape.</returns>
public ShapeDistributionInformation[] ComputeChildContributions()
{
var toReturn = new ShapeDistributionInformation[shapes.Count];
for (int i = 0; i < shapes.Count; i++)
{
shapes.Elements[i].Shape.ComputeDistributionInformation(out toReturn[i]);
}
return toReturn;
}
/// <summary>
/// Computes a variety of shape information all at once.
/// </summary>
/// <param name="shapeInfo">Properties of the shape.</param>
public override void ComputeDistributionInformation(out ShapeDistributionInformation shapeInfo)
{
shapeInfo.VolumeDistribution = ComputeVolumeDistribution(out shapeInfo.Volume);
shapeInfo.Center = ComputeCenter();
}
/// <summary>
/// Computes a variety of shape information all at once.
/// </summary>
/// <param name="shapeInfo">Properties of the shape.</param>
public override void ComputeDistributionInformation(out ShapeDistributionInformation shapeInfo)
{
shapeInfo.VolumeDistribution = ComputeVolumeDistribution(out shapeInfo.Volume);
shapeInfo.Center = ComputeCenter();
}
/// <summary> /// Computes a variety of shape information all at once. /// </summary> /// <param name="shapeInfo">Properties of the shape.</param> public abstract void ComputeDistributionInformation(out ShapeDistributionInformation shapeInfo);
