public void Add(double m1_11, double m1_12, double m1_13, double m1_21, double m1_22, double m1_23, double m1_31, double m1_32, double m1_33, double m2_11, double m2_12, double m2_13, double m2_21, double m2_22, double m2_23, double m2_31, double m2_32, double m2_33, double m3_11, double m3_12, double m3_13, double m3_21, double m3_22, double m3_23, double m3_31, double m3_32, double m3_33) { // Arrange var m1 = new Matrix3x3(m1_11, m1_12, m1_13, m1_21, m1_22, m1_23, m1_31, m1_32, m1_33); var m2 = new Matrix3x3(m2_11, m2_12, m2_13, m2_21, m2_22, m2_23, m2_31, m2_32, m2_33); // Act var m3 = m1.Add(m2); // Assert Assert.That(m3.M11, Is.EqualTo(m3_11)); Assert.That(m3.M12, Is.EqualTo(m3_12)); Assert.That(m3.M13, Is.EqualTo(m3_13)); Assert.That(m3.M21, Is.EqualTo(m3_21)); Assert.That(m3.M22, Is.EqualTo(m3_22)); Assert.That(m3.M23, Is.EqualTo(m3_23)); Assert.That(m3.M31, Is.EqualTo(m3_31)); Assert.That(m3.M32, Is.EqualTo(m3_32)); Assert.That(m3.M33, Is.EqualTo(m3_33)); }
/// <summary> /// Computes the volume distribution and center of the shape. /// </summary> /// <param name="entries">Mass-weighted entries of the compound.</param> /// <param name="volume">Summed volume of the constituent shapes. Intersecting volumes get double counted.</param> /// <param name="volumeDistribution">Volume distribution of the shape.</param> /// <param name="center">Center of the compound.</param> public static void ComputeVolumeDistribution(IList <CompoundShapeEntry> entries, out float volume, out Matrix3x3 volumeDistribution, out Vector3 center) { center = new Vector3(); float totalWeight = 0; volume = 0; for (int i = 0; i < entries.Count; i++) { center += entries[i].LocalTransform.Position * entries[i].Weight; volume += entries[i].Shape.Volume; totalWeight += entries[i].Weight; } if (totalWeight <= 0) { throw new NotFiniteNumberException("Cannot compute distribution; the total weight of a compound shape must be positive."); } float totalWeightInverse = 1 / totalWeight; totalWeightInverse.Validate(); center *= totalWeightInverse; volumeDistribution = new Matrix3x3(); for (int i = 0; i < entries.Count; i++) { RigidTransform transform = entries[i].LocalTransform; Matrix3x3 contribution; TransformContribution(ref transform, ref center, ref entries[i].Shape.volumeDistribution, entries[i].Weight, out contribution); Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution); } Matrix3x3.Multiply(ref volumeDistribution, totalWeightInverse, out volumeDistribution); volumeDistribution.Validate(); }
/// <summary> /// Modifies a contribution using a transform, position, and weight. /// </summary> /// <param name="transform">Transform to use to modify the contribution.</param> /// <param name="center">Center to use to modify the contribution.</param> /// <param name="baseContribution">Original unmodified contribution.</param> /// <param name="weight">Weight of the contribution.</param> /// <param name="contribution">Transformed contribution.</param> public static void TransformContribution(ref RigidTransform transform, ref Vector3 center, ref Matrix3x3 baseContribution, float weight, out Matrix3x3 contribution) { Matrix3x3 rotation; Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out rotation); Matrix3x3 temp; //Do angular transformed contribution first... Matrix3x3.MultiplyTransposed(ref rotation, ref baseContribution, out temp); Matrix3x3.Multiply(ref temp, ref rotation, out temp); contribution = temp; //Now add in the offset from the origin. Vector3 offset; Vector3.Subtract(ref transform.Position, ref center, out offset); Matrix3x3 innerProduct; Matrix3x3.CreateScale(offset.LengthSquared(), out innerProduct); Matrix3x3 outerProduct; Matrix3x3.CreateOuterProduct(ref offset, ref offset, out outerProduct); Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out temp); Matrix3x3.Add(ref contribution, ref temp, out contribution); Matrix3x3.Multiply(ref contribution, weight, out contribution); }
///<summary> /// Computes a volume distribution based on a bunch of point contributions. ///</summary> ///<param name="pointContributions">Point contributions to the volume distribution.</param> ///<param name="center">Location to use as the center for purposes of computing point contributions.</param> ///<returns>Volume distribution of the point contributions.</returns> public static Matrix3x3 ComputeVolumeDistribution(RawList <Vector3> pointContributions, ref Vector3 center) { var volumeDistribution = new Matrix3x3(); float pointWeight = 1f / pointContributions.Count; for (int i = 0; i < pointContributions.Count; i++) { Matrix3x3 contribution; GetPointContribution(pointWeight, ref center, pointContributions[i], out contribution); Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution); } return(volumeDistribution); }
/// <summary> /// Computes the volume distribution of the shape. /// </summary> /// <returns>Volume distribution of the shape.</returns> public override Matrix3x3 ComputeVolumeDistribution() { var volumeDistribution = new Matrix3x3(); float totalWeight = 0; for (int i = 0; i < shapes.Count; i++) { totalWeight += shapes.Elements[i].Weight; Matrix3x3 contribution; GetContribution(shapes.Elements[i].Shape, ref shapes.Elements[i].LocalTransform, ref Toolbox.ZeroVector, shapes.Elements[i].Weight, out contribution); Matrix3x3.Add(ref contribution, ref volumeDistribution, out volumeDistribution); } Matrix3x3.Multiply(ref volumeDistribution, 1 / totalWeight, out volumeDistribution); return(volumeDistribution); }
/// <summary> /// Initializes the constraint for the current frame. /// </summary> /// <param name="dt">Time between frames.</param> public override void Update(float dt) { Quaternion quaternionA; Quaternion.Multiply(ref connectionA.orientation, ref initialQuaternionConjugateA, out quaternionA); Quaternion quaternionB; Quaternion.Multiply(ref connectionB.orientation, ref initialQuaternionConjugateB, out quaternionB); Quaternion.Conjugate(ref quaternionB, out quaternionB); Quaternion intermediate; Quaternion.Multiply(ref quaternionA, ref quaternionB, out intermediate); float angle; Vector3 axis; Quaternion.GetAxisAngleFromQuaternion(ref intermediate, out axis, out angle); error.X = axis.X * angle; error.Y = axis.Y * angle; error.Z = axis.Z * angle; float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness); errorReduction = -errorReduction; biasVelocity.X = errorReduction * error.X; biasVelocity.Y = errorReduction * error.Y; biasVelocity.Z = errorReduction * error.Z; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > maxCorrectiveVelocitySquared) { float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; biasVelocity.Z *= multiplier; } Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out effectiveMassMatrix); effectiveMassMatrix.M11 += softness; effectiveMassMatrix.M22 += softness; effectiveMassMatrix.M33 += softness; Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix); }
protected internal override void ComputeEffectiveMass() { //For all constraints, the effective mass matrix is 1 / (J * M^-1 * JT). //For single bone constraints, J has 2 3x3 matrices. M^-1 (W below) is a 6x6 matrix with 2 3x3 block diagonal matrices. //To compute the whole denominator, Matrix3x3 linearW; Matrix3x3.CreateScale(TargetBone.inverseMass, out linearW); Matrix3x3 linear; Matrix3x3.Multiply(ref linearJacobian, ref linearW, out linear); //Compute J * M^-1 for linear component Matrix3x3.MultiplyByTransposed(ref linear, ref linearJacobian, out linear); //Compute (J * M^-1) * JT for linear component Matrix3x3 angular; Matrix3x3.Multiply(ref angularJacobian, ref TargetBone.inertiaTensorInverse, out angular); //Compute J * M^-1 for angular component Matrix3x3.MultiplyByTransposed(ref angular, ref angularJacobian, out angular); //Compute (J * M^-1) * JT for angular component //A nice side effect of the block diagonal nature of M^-1 is that the above separated components are now combined into the complete denominator matrix by addition! Matrix3x3.Add(ref linear, ref angular, out effectiveMass); //Incorporate the constraint softness into the effective mass denominator. This pushes the matrix away from singularity. //Softness will also be incorporated into the velocity solve iterations to complete the implementation. if (effectiveMass.M11 != 0) { effectiveMass.M11 += softness; } if (effectiveMass.M22 != 0) { effectiveMass.M22 += softness; } if (effectiveMass.M33 != 0) { effectiveMass.M33 += softness; } //Invert! Takes us from J * M^-1 * JT to 1 / (J * M^-1 * JT). Matrix3x3.AdaptiveInvert(ref effectiveMass, out effectiveMass); }
public void CanAddTwoMatrices() { var actual = Matrix3x3.Add(m1, m2); var expected = new Matrix3x3( 4d, 20.6d, 14.42d, 83.3d, 17.8d, 25.1316783, 34.02, 64d, -0.00137 ); Assert.AreEqual(expected.A11, actual.A11, 0.00001d); Assert.AreEqual(expected.A12, actual.A12, 0.00001d); Assert.AreEqual(expected.A13, actual.A13, 0.00001d); Assert.AreEqual(expected.A21, actual.A21, 0.00001d); Assert.AreEqual(expected.A22, actual.A22, 0.00001d); Assert.AreEqual(expected.A23, actual.A23, 0.00001d); Assert.AreEqual(expected.A31, actual.A31, 0.00001d); Assert.AreEqual(expected.A32, actual.A32, 0.00001d); Assert.AreEqual(expected.A33, actual.A33, 0.00001d); }
/// <summary> /// Computes the volume distribution of the shape. /// </summary> /// <returns>Volume distribution of the shape.</returns> public override Matrix3x3 ComputeVolumeDistribution() { var volumeDistribution = new Matrix3x3(); float totalWeight = 0; for (int i = 0; i < shapes.Count; i++) { totalWeight += shapes.Elements[i].Weight; Matrix3x3 contribution; GetContribution(shapes.Elements[i].Shape, ref shapes.Elements[i].LocalTransform, ref Toolbox.ZeroVector, shapes.Elements[i].Weight, out contribution); Matrix3x3.Add(ref contribution, ref volumeDistribution, out volumeDistribution); } if (totalWeight <= 0) { throw new NotFiniteNumberException("Cannot compute distribution; the total weight of a compound shape must be positive."); } Matrix3x3.Multiply(ref volumeDistribution, 1 / totalWeight, out volumeDistribution); volumeDistribution.Validate(); return(volumeDistribution); }
/// <summary> /// Computes the volume distribution of the shape as well as its volume. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// </summary> /// <param name="volume">Volume of the shape.</param> /// <returns>Volume distribution of the shape.</returns> public override Matrix3x3 ComputeVolumeDistribution(out float volume) { Vector3 center = ComputeCenter(); volume = ComputeVolume(); //Just approximate. //Calculate distribution of mass. const float massPerPoint = .333333333f; //Subtract the position from the distribution, moving into a 'body space' relative to itself. // [ (j * j + z * z) (-j * j) (-j * z) ] //I = I + [ (-j * j) (j * j + z * z) (-j * z) ] // [ (-j * z) (-j * z) (j * j + j * j) ] float i = vA.X - center.X; float j = vA.Y - center.Y; float k = vA.Z - center.Z; //localInertiaTensor += new Matrix(j * j + k * k, -j * j, -j * k, 0, -j * j, j * j + k * k, -j * k, 0, -j * k, -j * k, j * j + j * j, 0, 0, 0, 0, 0); //No mass per point. var volumeDistribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k), massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k), massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j)); i = vB.X - center.X; j = vB.Y - center.Y; k = vB.Z - center.Z; var pointContribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k), massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k), massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j)); Matrix3x3.Add(ref volumeDistribution, ref pointContribution, out volumeDistribution); i = vC.X - center.X; j = vC.Y - center.Y; k = vC.Z - center.Z; pointContribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k), massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k), massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j)); Matrix3x3.Add(ref volumeDistribution, ref pointContribution, out volumeDistribution); return(volumeDistribution); }
public MobileChunkShape(Vector3i csize, BlockInternal[] blocks, out Vector3 center) { Matrix3x3 boxMat = new BoxShape(csize.X, csize.Y, csize.Z).VolumeDistribution; ChunkSize = csize; Blocks = blocks; double weightInv = 1f / blocks.Length; center = new Vector3(csize.X / 2f, csize.Y / 2f, csize.Z / 2f); // TODO: More accurately get center of weight based on which blocks are solid or not!? Matrix3x3 volumeDistribution = new Matrix3x3(); RigidTransform transform = new RigidTransform(center); CompoundShape.TransformContribution(ref transform, ref center, ref boxMat, blocks.Length, out Matrix3x3 contribution); Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution); Matrix3x3.Multiply(ref volumeDistribution, weightInv, out volumeDistribution); UpdateEntityShapeVolume(new EntityShapeVolumeDescription() { Volume = csize.X * csize.Y * csize.Z, VolumeDistribution = volumeDistribution }); Center = center; }
public void Matrix3x3AddTest() { Matrix3x3 a = GenerateIncrementalMatrixNumber(); Matrix3x3 b = GenerateIncrementalMatrixNumber(-8.0f); Matrix3x3 expected = new Matrix3x3 { M11 = a.M11 + b.M11, M12 = a.M12 + b.M12, M13 = a.M13 + b.M13, M21 = a.M21 + b.M21, M22 = a.M22 + b.M22, M23 = a.M23 + b.M23, M31 = a.M31 + b.M31, M32 = a.M32 + b.M32, M33 = a.M33 + b.M33 }; Matrix3x3 actual = Matrix3x3.Add(a, b); Assert.AreEqual(expected, actual); }
/// <summary> /// Computes the volume distribution of the triangle. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// </summary> ///<param name="vA">First vertex in the triangle.</param> ///<param name="vB">Second vertex in the triangle.</param> ///<param name="vC">Third vertex in the triangle.</param> /// <returns>Volume distribution of the shape.</returns> public static Matrix3x3 ComputeVolumeDistribution(System.Numerics.Vector3 vA, System.Numerics.Vector3 vB, System.Numerics.Vector3 vC) { System.Numerics.Vector3 center = (vA + vB + vC) * (1 / 3f); //Calculate distribution of mass. const float massPerPoint = .333333333f; //Subtract the position from the distribution, moving into a 'body space' relative to itself. // [ (j * j + z * z) (-j * j) (-j * z) ] //I = I + [ (-j * j) (j * j + z * z) (-j * z) ] // [ (-j * z) (-j * z) (j * j + j * j) ] float i = vA.X - center.X; float j = vA.Y - center.Y; float k = vA.Z - center.Z; //localInertiaTensor += new System.Numerics.Matrix4x4(j * j + k * k, -j * j, -j * k, 0, -j * j, j * j + k * k, -j * k, 0, -j * k, -j * k, j * j + j * j, 0, 0, 0, 0, 0); //No mass per point. var volumeDistribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k), massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k), massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j)); i = vB.X - center.X; j = vB.Y - center.Y; k = vB.Z - center.Z; var pointContribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k), massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k), massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j)); Matrix3x3.Add(ref volumeDistribution, ref pointContribution, out volumeDistribution); i = vC.X - center.X; j = vC.Y - center.Y; k = vC.Z - center.Z; pointContribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k), massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k), massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j)); Matrix3x3.Add(ref volumeDistribution, ref pointContribution, out volumeDistribution); return(volumeDistribution); }
/// <summary> /// Computes the volume distribution of the triangle. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// </summary> ///<param name="vA">First vertex in the triangle.</param> ///<param name="vB">Second vertex in the triangle.</param> ///<param name="vC">Third vertex in the triangle.</param> /// <returns>Volume distribution of the shape.</returns> public static Matrix3x3 ComputeVolumeDistribution(Vector3 vA, Vector3 vB, Vector3 vC) { Vector3 center = (vA + vB + vC) * F64.OneThird; //Calculate distribution of mass. Fix64 massPerPoint = F64.OneThird; //Subtract the position from the distribution, moving into a 'body space' relative to itself. // [ (j * j + z * z) (-j * j) (-j * z) ] //I = I + [ (-j * j) (j * j + z * z) (-j * z) ] // [ (-j * z) (-j * z) (j * j + j * j) ] Fix64 i = vA.X - center.X; Fix64 j = vA.Y - center.Y; Fix64 k = vA.Z - center.Z; //localInertiaTensor += new Matrix(j * j + k * k, -j * j, -j * k, 0, -j * j, j * j + k * k, -j * k, 0, -j * k, -j * k, j * j + j * j, 0, 0, 0, 0, 0); //No mass per point. var volumeDistribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k), massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k), massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j)); i = vB.X - center.X; j = vB.Y - center.Y; k = vB.Z - center.Z; var pointContribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k), massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k), massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j)); Matrix3x3.Add(ref volumeDistribution, ref pointContribution, out volumeDistribution); i = vC.X - center.X; j = vC.Y - center.Y; k = vC.Z - center.Z; pointContribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k), massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k), massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j)); Matrix3x3.Add(ref volumeDistribution, ref pointContribution, out volumeDistribution); return(volumeDistribution); }
/// <summary> /// Computes the volume distribution and center of the shape. /// </summary> /// <param name="entries">Mass-weighted entries of the compound.</param> /// <param name="center">Center of the compound.</param> /// <returns>Volume distribution of the shape.</returns> public static Matrix3x3 ComputeVolumeDistribution(IList <CompoundShapeEntry> entries, out Vector3 center) { center = new Vector3(); float totalWeight = 0; for (int i = 0; i < entries.Count; i++) { center += entries[i].LocalTransform.Position * entries[i].Weight; totalWeight += entries[i].Weight; } center /= totalWeight; var volumeDistribution = new Matrix3x3(); for (int i = 0; i < entries.Count; i++) { RigidTransform transform = entries[i].LocalTransform; Matrix3x3 contribution; GetContribution(entries[i].Shape, ref transform, ref center, entries[i].Weight, out contribution); Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution); } Matrix3x3.Multiply(ref volumeDistribution, 1 / totalWeight, out volumeDistribution); return(volumeDistribution); }
/// <summary> /// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation. /// </summary> /// <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(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; Vector3 weightedCenter; Vector3.Multiply(ref entry.LocalTransform.Position, entry.Weight, out weightedCenter); Vector3.Add(ref weightedCenter, ref distributionInfoA.Center, out distributionInfoA.Center); distributionInfoA.Volume += entry.Shape.Volume; weightA += entry.Weight; } for (int i = b.children.Count - 1; i >= 0; i--) { var child = b.children.Elements[i]; var entry = child.Entry; Vector3 weightedCenter; Vector3.Multiply(ref entry.LocalTransform.Position, entry.Weight, out weightedCenter); Vector3.Add(ref weightedCenter, ref distributionInfoB.Center, out distributionInfoB.Center); distributionInfoB.Volume += entry.Shape.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; Matrix3x3 contribution; CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfoA.Center, ref entry.Shape.volumeDistribution, entry.Weight, out contribution); Matrix3x3.Add(ref contribution, 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; Matrix3x3 contribution; CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfoB.Center, ref entry.Shape.volumeDistribution, entry.Weight, out contribution); Matrix3x3.Add(ref contribution, 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> /// Calculates the inertia of the shape relative to the center of mass. /// </summary> /// <param name="shape"></param> /// <param name="centerOfMass"></param> /// <param name="inertia">Returns the inertia relative to the center of mass, not to the origin</param> /// <returns></returns> #region public static float CalculateMassInertia(Shape shape, out JVector centerOfMass, out JMatrix inertia) public static float CalculateMassInertia(Shape shape, out Vector3 centerOfMass, out Matrix3x3 inertia) { float mass = 0.0f; centerOfMass = Vector3.zero; inertia = Matrix3x3.Zero; if (shape is Multishape) { throw new ArgumentException("Can't calculate inertia of multishapes.", "shape"); } // build a triangle hull around the shape List <Vector3> hullTriangles = new List <Vector3>(); shape.MakeHull(ref hullTriangles, 3); // create inertia of tetrahedron with vertices at // (0,0,0) (1,0,0) (0,1,0) (0,0,1) float a = 1.0f / 60.0f, b = 1.0f / 120.0f; Matrix3x3 C = new Matrix3x3(a, b, b, b, a, b, b, b, a); for (int i = 0; i < hullTriangles.Count; i += 3) { Vector3 column0 = hullTriangles[i + 0]; Vector3 column1 = hullTriangles[i + 1]; Vector3 column2 = hullTriangles[i + 2]; Matrix3x3 A = new Matrix3x3(column0.x, column1.x, column2.x, column0.y, column1.y, column2.y, column0.z, column1.z, column2.z); float detA = A.Determinant(); // now transform this canonical tetrahedron to the target tetrahedron // inertia by a linear transformation A Matrix3x3 tetrahedronInertia = Matrix3x3.Multiply(A * C * Matrix3x3.Transpose(A), detA); Vector3 tetrahedronCOM = (1.0f / 4.0f) * (hullTriangles[i + 0] + hullTriangles[i + 1] + hullTriangles[i + 2]); float tetrahedronMass = (1.0f / 6.0f) * detA; inertia += tetrahedronInertia; centerOfMass += tetrahedronMass * tetrahedronCOM; mass += tetrahedronMass; } inertia = Matrix3x3.Multiply(Matrix3x3.Identity, inertia.Trace()) - inertia; centerOfMass = centerOfMass * (1.0f / mass); float x = centerOfMass.x; float y = centerOfMass.y; float z = centerOfMass.z; // now translate the inertia by the center of mass Matrix3x3 t = new Matrix3x3( -mass * (y * y + z * z), mass * x * y, mass * x * z, mass * y * x, -mass * (z * z + x * x), mass * y * z, mass * z * x, mass * z * y, -mass * (x * x + y * y)); Matrix3x3.Add(ref inertia, ref t, out inertia); return(mass); }
///<summary> /// Performs the frame's configuration step. ///</summary> ///<param name="dt">Timestep duration.</param> public override void Update(float dt) { Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA); Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB); Matrix3x3.Transform(ref localConstrainedAxis1, ref connectionA.orientationMatrix, out worldConstrainedAxis1); Matrix3x3.Transform(ref localConstrainedAxis2, ref connectionA.orientationMatrix, out worldConstrainedAxis2); Vector3 error; Vector3.Cross(ref worldAxisA, ref worldAxisB, out error); Vector3.Dot(ref error, ref worldConstrainedAxis1, out this.error.X); Vector3.Dot(ref error, ref worldConstrainedAxis2, out this.error.Y); float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness); errorReduction = -errorReduction; biasVelocity.X = errorReduction * this.error.X; biasVelocity.Y = errorReduction * this.error.Y; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > maxCorrectiveVelocitySquared) { float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; } Vector3 axis1I, axis2I; if (connectionA.isDynamic && connectionB.isDynamic) { Matrix3x3 inertiaTensorSum; Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out inertiaTensorSum); Matrix3x3.Transform(ref worldConstrainedAxis1, ref inertiaTensorSum, out axis1I); Matrix3x3.Transform(ref worldConstrainedAxis2, ref inertiaTensorSum, out axis2I); } else if (connectionA.isDynamic && !connectionB.isDynamic) { Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionA.inertiaTensorInverse, out axis1I); Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionA.inertiaTensorInverse, out axis2I); } else if (!connectionA.isDynamic && connectionB.isDynamic) { Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionB.inertiaTensorInverse, out axis1I); Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionB.inertiaTensorInverse, out axis2I); } else { throw new InvalidOperationException("Cannot constrain two kinematic bodies."); } Vector3.Dot(ref axis1I, ref worldConstrainedAxis1, out effectiveMassMatrix.M11); Vector3.Dot(ref axis1I, ref worldConstrainedAxis2, out effectiveMassMatrix.M12); Vector3.Dot(ref axis2I, ref worldConstrainedAxis1, out effectiveMassMatrix.M21); Vector3.Dot(ref axis2I, ref worldConstrainedAxis2, out effectiveMassMatrix.M22); effectiveMassMatrix.M11 += softness; effectiveMassMatrix.M22 += softness; Matrix2x2.Invert(ref effectiveMassMatrix, out effectiveMassMatrix); Matrix2x2.Negate(ref effectiveMassMatrix, out effectiveMassMatrix); }
/// <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); }
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); }
protected internal override void ComputeEffectiveMass() { //For all constraints, the effective mass matrix is 1 / (J * M^-1 * JT). //For two bone constraints, J has 4 3x3 matrices. M^-1 (W below) is a 12x12 matrix with 4 3x3 block diagonal matrices. //To compute the whole denominator, Matrix3x3 linearW; Matrix3x3 linearA, angularA, linearB, angularB; if (!ConnectionA.Pinned) { Matrix3x3.CreateScale(ConnectionA.inverseMass, out linearW); Matrix3x3.Multiply(ref linearJacobianA, ref linearW, out linearA); //Compute J * M^-1 for linear component Matrix3x3.MultiplyByTransposed(ref linearA, ref linearJacobianA, out linearA); //Compute (J * M^-1) * JT for linear component Matrix3x3.Multiply(ref angularJacobianA, ref ConnectionA.inertiaTensorInverse, out angularA); //Compute J * M^-1 for angular component Matrix3x3.MultiplyByTransposed(ref angularA, ref angularJacobianA, out angularA); //Compute (J * M^-1) * JT for angular component } else { //Treat pinned bones as if they have infinite inertia. linearA = new Matrix3x3(); angularA = new Matrix3x3(); } if (!ConnectionB.Pinned) { Matrix3x3.CreateScale(ConnectionB.inverseMass, out linearW); Matrix3x3.Multiply(ref linearJacobianB, ref linearW, out linearB); //Compute J * M^-1 for linear component Matrix3x3.MultiplyByTransposed(ref linearB, ref linearJacobianB, out linearB); //Compute (J * M^-1) * JT for linear component Matrix3x3.Multiply(ref angularJacobianB, ref ConnectionB.inertiaTensorInverse, out angularB); //Compute J * M^-1 for angular component Matrix3x3.MultiplyByTransposed(ref angularB, ref angularJacobianB, out angularB); //Compute (J * M^-1) * JT for angular component } else { //Treat pinned bones as if they have infinite inertia. linearB = new Matrix3x3(); angularB = new Matrix3x3(); } //A nice side effect of the block diagonal nature of M^-1 is that the above separated components are now combined into the complete denominator matrix by addition! Matrix3x3.Add(ref linearA, ref angularA, out effectiveMass); Matrix3x3.Add(ref effectiveMass, ref linearB, out effectiveMass); Matrix3x3.Add(ref effectiveMass, ref angularB, out effectiveMass); //Incorporate the constraint softness into the effective mass denominator. This pushes the matrix away from singularity. //Softness will also be incorporated into the velocity solve iterations to complete the implementation. if (effectiveMass.M11 != 0) { effectiveMass.M11 += softness; } if (effectiveMass.M22 != 0) { effectiveMass.M22 += softness; } if (effectiveMass.M33 != 0) { effectiveMass.M33 += softness; } //Invert! Takes us from J * M^-1 * JT to 1 / (J * M^-1 * JT). Matrix3x3.AdaptiveInvert(ref effectiveMass, out effectiveMass); }
/// <summary> /// Initializes the constraint for the current frame. /// </summary> /// <param name="dt">Time between frames.</param> public override void Update(float dt) { basis.rotationMatrix4 = connectionA.orientationMatrix4; basis.ComputeWorldSpaceAxes(); float inverseDt = 1 / dt; if (settings.mode == MotorMode.Servomechanism) //Only need to do the bulk of this work if it's a servo. { //The error is computed using this equation: //GoalRelativeOrientation * ConnectionA.Orientation * Error = ConnectionB.Orientation //GoalRelativeOrientation is the original rotation from A to B in A's local space. //Multiplying by A's orientation gives us where B *should* be. //Of course, B won't be exactly where it should be after initialization. //The Error component holds the difference between what is and what should be. //Error = (GoalRelativeOrientation * ConnectionA.Orientation)^-1 * ConnectionB.Orientation //ConnectionA.Orientation is replaced in the above by the world space basis orientation. Quaternion worldBasis = Quaternion.CreateFromRotationMatrix(basis.WorldTransform); Quaternion bTarget; Quaternion.Concatenate(ref settings.servo.goal, ref worldBasis, out bTarget); Quaternion bTargetConjugate; Quaternion.Conjugate(ref bTarget, out bTargetConjugate); Quaternion error; Quaternion.Concatenate(ref bTargetConjugate, ref connectionB.orientation, out error); float errorReduction; settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, inverseDt, out errorReduction, out usedSoftness); //Turn this into an axis-angle representation. Quaternion.GetAxisAngleFromQuaternion(ref error, out axis, out angle); //Scale the axis by the desired velocity if the angle is sufficiently large (epsilon). if (angle > Toolbox.BigEpsilon) { float velocity = -(MathHelper.Min(settings.servo.baseCorrectiveSpeed, angle * inverseDt) + angle * errorReduction); biasVelocity.X = axis.X * velocity; biasVelocity.Y = axis.Y * velocity; biasVelocity.Z = axis.Z * velocity; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared; if (length > settings.servo.maxCorrectiveVelocitySquared) { float multiplier = settings.servo.maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; biasVelocity.Z *= multiplier; } } else { biasVelocity.X = 0; biasVelocity.Y = 0; biasVelocity.Z = 0; } } else { usedSoftness = settings.velocityMotor.softness * inverseDt; angle = 0; //Zero out the error; Matrix3x3 transform = basis.WorldTransform; Matrix3x3.Transform(ref settings.velocityMotor.goalVelocity, ref transform, out biasVelocity); } //Compute effective mass Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out effectiveMassMatrix4); effectiveMassMatrix4.M11 += usedSoftness; effectiveMassMatrix4.M22 += usedSoftness; effectiveMassMatrix4.M33 += usedSoftness; Matrix3x3.Invert(ref effectiveMassMatrix4, out effectiveMassMatrix4); //Update the maximum force ComputeMaxForces(settings.maximumForce, dt); }
/// <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); }