public void TestAddFlexibleInPlace()
{
Random rnd = new Random(2);
Vector3 p1 = RandomUtil.Vector3(rnd); float m1 = RandomUtil.PositiveFloat(rnd);
Vector3 p2 = RandomUtil.Vector3(rnd); float m2 = RandomUtil.PositiveFloat(rnd);
Vector3 p3 = RandomUtil.Vector3(rnd); float m3 = RandomUtil.PositiveFloat(rnd);
MassMoment momentToAdd = new MassMoment();
momentToAdd.AddInplace(m1, p1);
momentToAdd.AddInplace(m2, p2);
momentToAdd.AddInplace(m3, p3);
float flexiblity = 0.6f;
Vector3 centerOfRotation = RandomUtil.Vector3(rnd);
MassMoment massMoment = new MassMoment();
massMoment.AddFlexibleInPlace(momentToAdd, flexiblity, centerOfRotation);
MassMoment expectedMoment = new MassMoment();
expectedMoment.AddInplace((1 - flexiblity) * m1, p1);
expectedMoment.AddInplace((1 - flexiblity) * m2, p2);
expectedMoment.AddInplace((1 - flexiblity) * m3, p3);
expectedMoment.AddInplace(flexiblity * m1, centerOfRotation);
expectedMoment.AddInplace(flexiblity * m2, centerOfRotation);
expectedMoment.AddInplace(flexiblity * m3, centerOfRotation);
PhysicsAssert.AreEqual(massMoment, expectedMoment, Acc);
}
public void AddInplace(MassMoment massMoment)
{
mass += massMoment.mass;
massPosition += massMoment.massPosition;
inertiaXX += massMoment.inertiaXX;
inertiaYY += massMoment.inertiaYY;
inertiaZZ += massMoment.inertiaZZ;
inertiaXY += massMoment.inertiaXY;
inertiaXZ += massMoment.inertiaXZ;
inertiaYZ += massMoment.inertiaYZ;
}
public static void AreEqual(MassMoment expected, MassMoment actual, float delta)
{
Assert.AreEqual(expected.Mass, actual.Mass, delta, "Mass");
MathAssert.AreEqual(expected.MassPosition, actual.MassPosition, delta);
Assert.AreEqual(expected.InertiaXX, actual.InertiaXX, delta, "InertiaXX");
Assert.AreEqual(expected.InertiaYY, actual.InertiaYY, delta, "InertiaYY");
Assert.AreEqual(expected.InertiaZZ, actual.InertiaZZ, delta, "InertiaZZ");
Assert.AreEqual(expected.InertiaXY, actual.InertiaXY, delta, "InertiaXY");
Assert.AreEqual(expected.InertiaXZ, actual.InertiaXZ, delta, "InertiaXZ");
Assert.AreEqual(expected.InertiaYZ, actual.InertiaYZ, delta, "InertiaYZ");
}
/**
* Add a child body on a flexible hinge.
* The result is a interpolation between adding a rigid body and adding a point mass at the hinge point.
*/
public void AddFlexibleInPlace(MassMoment massMoment, float flexibility, Vector3 centerOfRotation)
{
float rigidity = 1 - flexibility;
mass += massMoment.mass;
massPosition += rigidity * massMoment.massPosition + flexibility * massMoment.mass * centerOfRotation;
inertiaXX += rigidity * massMoment.inertiaXX + flexibility * massMoment.mass * (Sqr(centerOfRotation.Y) + Sqr(centerOfRotation.Z));
inertiaYY += rigidity * massMoment.inertiaYY + flexibility * massMoment.mass * (Sqr(centerOfRotation.X) + Sqr(centerOfRotation.Z));
inertiaZZ += rigidity * massMoment.inertiaZZ + flexibility * massMoment.mass * (Sqr(centerOfRotation.X) + Sqr(centerOfRotation.Y));
inertiaXY += rigidity * massMoment.inertiaXY - flexibility * massMoment.mass * centerOfRotation.X * centerOfRotation.Y;
inertiaXZ += rigidity * massMoment.inertiaXZ - flexibility * massMoment.mass * centerOfRotation.X * centerOfRotation.Z;
inertiaYZ += rigidity * massMoment.inertiaYZ - flexibility * massMoment.mass * centerOfRotation.Y * centerOfRotation.Z;
}
public void TestCenterOfMass()
{
Random rnd = new Random(0);
Vector3 p1 = RandomUtil.Vector3(rnd); float m1 = RandomUtil.PositiveFloat(rnd);
Vector3 p2 = RandomUtil.Vector3(rnd); float m2 = RandomUtil.PositiveFloat(rnd);
Vector3 p3 = RandomUtil.Vector3(rnd); float m3 = RandomUtil.PositiveFloat(rnd);
var accum = new MassMoment();
accum.AddInplace(m1, p1);
accum.AddInplace(m2, p2);
accum.AddInplace(m3, p3);
Vector3 expectedCenterOfMass = (m1 * p1 + m2 * p2 + m3 * p3) / (m1 + m2 + m3);
MathAssert.AreEqual(expectedCenterOfMass, accum.GetCenterOfMass(), Acc);
}
public void TestRotate()
{
Random rnd = new Random(1);
Vector3 p1 = RandomUtil.Vector3(rnd); float m1 = RandomUtil.PositiveFloat(rnd);
Vector3 p2 = RandomUtil.Vector3(rnd); float m2 = RandomUtil.PositiveFloat(rnd);
Vector3 p3 = RandomUtil.Vector3(rnd); float m3 = RandomUtil.PositiveFloat(rnd);
Quaternion rotation = RandomUtil.UnitQuaternion(rnd);
var massMoment = new MassMoment();
var rotatedMassMoment = new MassMoment();
massMoment.AddInplace(m1, p1); rotatedMassMoment.AddInplace(m1, Vector3.Transform(p1, rotation));
massMoment.AddInplace(m2, p2); rotatedMassMoment.AddInplace(m2, Vector3.Transform(p2, rotation));
massMoment.AddInplace(m3, p3); rotatedMassMoment.AddInplace(m3, Vector3.Transform(p3, rotation));
PhysicsAssert.AreEqual(massMoment.Rotate(rotation), rotatedMassMoment, Acc);
}
public void TestTranslate()
{
Random rnd = new Random(1);
Vector3 p1 = RandomUtil.Vector3(rnd); float m1 = RandomUtil.PositiveFloat(rnd);
Vector3 p2 = RandomUtil.Vector3(rnd); float m2 = RandomUtil.PositiveFloat(rnd);
Vector3 p3 = RandomUtil.Vector3(rnd); float m3 = RandomUtil.PositiveFloat(rnd);
Vector3 translation = RandomUtil.Vector3(rnd);
var massMoment = new MassMoment();
var translatedMassMoment = new MassMoment();
massMoment.AddInplace(m1, p1); translatedMassMoment.AddInplace(m1, p1 + translation);
massMoment.AddInplace(m2, p2); translatedMassMoment.AddInplace(m2, p2 + translation);
massMoment.AddInplace(m3, p3); translatedMassMoment.AddInplace(m3, p3 + translation);
PhysicsAssert.AreEqual(massMoment.Translate(translation), translatedMassMoment, Acc);
}
private MassMoment[] GetMassMoments(RigidTransform[] totalTransforms, RigidBone[] boneChain)
{
MassMoment[] accumulators = new MassMoment[boneSystem.Bones.Length];
bool[] areOnChain = new bool[boneSystem.Bones.Length];
foreach (var bone in boneChain)
{
areOnChain[bone.Index] = true;
}
foreach (var bone in boneSystem.Bones.Reverse())
{
var unposedBoneCenteredMassMoment = boneAttributes[bone.Index].MassMoment;
var unposedMassMoment = unposedBoneCenteredMassMoment.Translate(bone.CenterPoint);
var totalTransform = totalTransforms[bone.Index];
var massMoment = unposedMassMoment.Rotate(totalTransform.Rotation).Translate(totalTransform.Translation);
var centerOfRation = totalTransform.Transform(bone.CenterPoint);
accumulators[bone.Index].AddInplace(massMoment);
var parent = bone.Parent;
if (parent != null)
{
float counterRotationRatio;
if (areOnChain[bone.Index])
{
counterRotationRatio = 0;
}
else
{
counterRotationRatio = boneAttributes[bone.Index].MassMoment.Mass / boneAttributes[0].MassIncludingDescendants;
}
accumulators[parent.Index].AddFlexibleInPlace(accumulators[bone.Index], counterRotationRatio, centerOfRation);
}
}
return(accumulators);
}
private MassMoment[] CalculateBoneMassMoments()
{
Debug.Assert(skinBinding.BoneWeights.Count == geometry.VertexCount);
MassMoment[] massMoments = new MassMoment[boneSystem.Bones.Count];
foreach (var quad in geometry.Faces)
{
if (quad.IsDegeneratedIntoTriangle)
{
throw new NotImplementedException("BoneAttributesCalculator only supports Quad faces");
}
for (int cornerIdx = 0; cornerIdx < Quad.SideCount; ++cornerIdx)
{
int vertexIdx = quad.GetCorner(cornerIdx);
foreach (var boneWeight in skinBinding.BoneWeights.GetElements(vertexIdx))
{
var bone = skinBinding.Bones[boneWeight.Index];
Vector3 p1 = geometry.VertexPositions[quad.GetCorner(cornerIdx - 1)];
Vector3 p2 = geometry.VertexPositions[vertexIdx];
Vector3 p3 = geometry.VertexPositions[quad.GetCorner(cornerIdx + 1)];
var boneCenter = bone.CenterPoint.GetValue(channelSystem.DefaultOutputs);
float volume = SignedTetrahedralVolume(boneCenter, p1, p2, p3) / CubicCentimetersPerLiter;
float mass = volume / 2; //assume a density of 1 kg per liter, half because each point is covered by two tetrahedra
Vector3 position = (boneCenter + p1 + p2 + p3) / 4;
Vector3 relativePosition = position - boneCenter;
massMoments[bone.Index].AddInplace(mass, relativePosition);
}
}
}
return(massMoments);
}
public void TestMomentOfInertia()
{
Random rnd = new Random(1);
Vector3 p1 = RandomUtil.Vector3(rnd); float m1 = RandomUtil.PositiveFloat(rnd);
Vector3 p2 = RandomUtil.Vector3(rnd); float m2 = RandomUtil.PositiveFloat(rnd);
Vector3 p3 = RandomUtil.Vector3(rnd); float m3 = RandomUtil.PositiveFloat(rnd);
var accum = new MassMoment();
accum.AddInplace(m1, p1);
accum.AddInplace(m2, p2);
accum.AddInplace(m3, p3);
Vector3 axisOfRotation = RandomUtil.UnitVector3(rnd);
Vector3 centerOfRotation = RandomUtil.Vector3(rnd);
float expectedMomentOfInertia =
PointMassMomentOfInertia(m1, p1, axisOfRotation, centerOfRotation) +
PointMassMomentOfInertia(m2, p2, axisOfRotation, centerOfRotation) +
PointMassMomentOfInertia(m3, p3, axisOfRotation, centerOfRotation);
Assert.AreEqual(expectedMomentOfInertia, accum.GetMomentOfInertia(axisOfRotation, centerOfRotation), Acc);
}
public void TestAddingMassMoments()
{
Random rnd = new Random(2);
Vector3 p1 = RandomUtil.Vector3(rnd); float m1 = RandomUtil.PositiveFloat(rnd);
Vector3 p2 = RandomUtil.Vector3(rnd); float m2 = RandomUtil.PositiveFloat(rnd);
Vector3 p3 = RandomUtil.Vector3(rnd); float m3 = RandomUtil.PositiveFloat(rnd);
var accumA = new MassMoment();
accumA.AddInplace(m1, p1);
accumA.AddInplace(m2, p2);
accumA.AddInplace(m3, p3);
var accumB = new MassMoment();
accumB.AddInplace(m1, p1);
var accumB2 = new MassMoment();
accumB2.AddInplace(m2, p2);
accumB2.AddInplace(m3, p3);
accumB.AddInplace(accumB2);
PhysicsAssert.AreEqual(accumA, accumB, Acc);
}
public BoneAttributes(bool isIkable, MassMoment massMoment, float massIncludingDescendants)
{
IsIkable = isIkable;
MassMoment = massMoment;
MassIncludingDescendants = massIncludingDescendants;
}
