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