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);