public void CompositeShapeWithRotatedChildren()
{
var s = new CompositeShape();
s.Children.Add(new GeometricObject(new BoxShape(1, 2, 3), new Vector3F(1.1f, 0.3f, 0.8f), new Pose(new Vector3F(100, 10, 0), RandomHelper.Random.NextQuaternionF())));
s.Children.Add(new GeometricObject(new ConeShape(1, 2), new Vector3F(1.1f, 0.3f, 0.8f), new Pose(new Vector3F(-10, -10, 0), RandomHelper.Random.NextQuaternionF())));
float m0;
Vector3F com0;
Matrix33F i0;
MassHelper.GetMass(s, new Vector3F(2), 1, true, 0.001f, 10, out m0, out com0, out i0);
var m = s.GetMesh(0.001f, 6);
m.Transform(Matrix44F.CreateScale(2));
float m1;
Vector3F com1;
Matrix33F i1;
MassHelper.GetMass(m, out m1, out com1, out i1);
const float e = 0.01f;
Assert.IsTrue(Numeric.AreEqual(m0, m1, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com1, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0, i1, e * (1 + i0.Trace)));
}
public void GeneralShape()
{
var s = new CylinderShape(1, 3);
float m0;
Vector3F com0;
Matrix33F i0;
MassHelper.GetMass(s, new Vector3F(1, -2, -3), 0.7f, true, 0.001f, 4, out m0, out com0, out i0);
var m = s.GetMesh(0.001f, 10);
var s2 = new TriangleMeshShape(m);
float m1;
Vector3F com1;
Matrix33F i1;
MassHelper.GetMass(s2, new Vector3F(1, -2, -3), 0.7f, true, 0.001f, 4, out m1, out com1, out i1);
const float e = 0.01f;
Assert.IsTrue(Numeric.AreEqual(m0, m1, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com1, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0, i1, e * (1 + i0.Trace)));
// Try with target mass.
float m2;
Vector3F com2;
Matrix33F i2;
MassHelper.GetMass(s2, new Vector3F(1, -2, -3), 23, false, 0.001f, 4, out m2, out com2, out i2);
Assert.IsTrue(Numeric.AreEqual(23, m2, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com2, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0 * 23 / m0, i2, e * (1 + i0.Trace)));
}
public void ScaledConvexMass()
{
var s = new ScaledConvexShape(new CapsuleShape(1, 3), new Vector3F(0.9f, -0.8f, 1.2f));
float m0;
Vector3F com0;
Matrix33F i0;
MassHelper.GetMass(s, new Vector3F(1, -2, -3), 1, true, 0.001f, 10, out m0, out com0, out i0);
var m = s.GetMesh(0.001f, 6);
m.Transform(Matrix44F.CreateScale(1, -2, -3));
float m1;
Vector3F com1;
Matrix33F i1;
MassHelper.GetMass(m, out m1, out com1, out i1);
const float e = 0.01f;
Assert.IsTrue(Numeric.AreEqual(m0, m1, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com1, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0, i1, e * (1 + i0.Trace)));
// Try other density.
float m2;
Vector3F com2;
Matrix33F i2;
MassHelper.GetMass(s, new Vector3F(1, -2, -3), 0.7f, true, 0.001f, 10, out m2, out com2, out i2);
Assert.IsTrue(Numeric.AreEqual(m0 * 0.7f, m2, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com2, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0 * 0.7f, i2, e * (1 + i0.Trace)));
}
public void MultiplyMatrix()
{
Matrix33F m = new Matrix33F(12, 23, 45,
67, 89, 90,
43, 65, 87);
Assert.AreEqual(Matrix33F.Zero, Matrix33F.Multiply(m, Matrix33F.Zero));
Assert.AreEqual(Matrix33F.Zero, Matrix33F.Multiply(Matrix33F.Zero, m));
Assert.AreEqual(m, Matrix33F.Multiply(m, Matrix33F.Identity));
Assert.AreEqual(m, Matrix33F.Multiply(Matrix33F.Identity, m));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.Identity, Matrix33F.Multiply(m, m.Inverse)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.Identity, Matrix33F.Multiply(m.Inverse, m)));
Matrix33F m1 = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
Matrix33F m2 = new Matrix33F(12, 23, 45,
67, 89, 90,
43, 65, 87);
Matrix33F result = Matrix33F.Multiply(m1, m2);
for (int column = 0; column < 3; column++)
{
for (int row = 0; row < 3; row++)
{
Assert.AreEqual(Vector3F.Dot(m1.GetRow(row), m2.GetColumn(column)), result[row, column]);
}
}
}
public void InverseWithNearSingularMatrix()
{
Matrix33F m = new Matrix33F(0.0001f, 0, 0,
0, 0.0001f, 0,
0, 0, 0.0001f);
Vector3F v = Vector3F.One;
Vector3F w = m * v;
Assert.IsTrue(Vector3F.AreNumericallyEqual(v, m.Inverse * w));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.Identity, m * m.Inverse));
}
public void CreateRotationZ()
{
float angle = (float)MathHelper.ToRadians(30);
Matrix33F m = Matrix33F.CreateRotationZ(angle);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F((float)Math.Cos(angle), (float)Math.Sin(angle), 0), m * Vector3F.UnitX));
QuaternionF q = QuaternionF.CreateRotation(Vector3F.UnitZ, angle);
Assert.IsTrue(Vector3F.AreNumericallyEqual(q.Rotate(Vector3F.One), m * Vector3F.One));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.CreateRotation(Vector3F.UnitZ, angle), m));
}
public void Inverse()
{
Assert.AreEqual(Matrix33F.Identity, Matrix33F.Identity.Inverse);
Matrix33F m = new Matrix33F(1, 2, 3,
2, 5, 8,
7, 6, -1);
Vector3F v = Vector3F.One;
Vector3F w = m * v;
Assert.IsTrue(Vector3F.AreNumericallyEqual(v, m.Inverse * w));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.Identity, m * m.Inverse));
}
public void ToPose()
{
var srt = new SrtTransform(new Vector3F(4, 5, 6), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(1, 2, 3));
var pose = srt.ToPose();
Assert.AreEqual(pose.Position, srt.Translation);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(pose.Orientation, srt.Rotation.ToRotationMatrix33()));
srt = new SrtTransform(new Vector3F(4, 5, 6), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(1, 2, 3));
pose = (Pose)srt;
Assert.AreEqual(pose.Position, srt.Translation);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(pose.Orientation, srt.Rotation.ToRotationMatrix33()));
}
public void AreEqualWithEpsilon()
{
float epsilon = 0.001f;
Matrix33F m0 = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
Matrix33F m1 = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
m1 += new Matrix33F(0.002f);
Matrix33F m2 = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
m2 += new Matrix33F(0.0001f);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(m0, m0, epsilon));
Assert.IsFalse(Matrix33F.AreNumericallyEqual(m0, m1, epsilon));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(m0, m2, epsilon));
}
public void FromPose()
{
var pose = new Pose(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 3, 4).Normalized);
var srt = SrtTransform.FromPose(pose);
Assert.AreEqual(Vector3F.One, srt.Scale);
Assert.AreEqual(pose.Position, srt.Translation);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(pose.Orientation, srt.Rotation.ToRotationMatrix33()));
pose = new Pose(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 3, 4).Normalized);
srt = pose;
Assert.AreEqual(Vector3F.One, srt.Scale);
Assert.AreEqual(pose.Position, srt.Translation);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(pose.Orientation, srt.Rotation.ToRotationMatrix33()));
}
public void Interpolate()
{
Pose p1 = new Pose(new Vector3F(1, 2, 3), QuaternionF.CreateRotationY(0.3f));
Pose p2 = new Pose(new Vector3F(-4, 5, -6), QuaternionF.CreateRotationZ(-0.1f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(p1.Position, Pose.Interpolate(p1, p2, 0).Position));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(p1.Orientation, Pose.Interpolate(p1, p2, 0).Orientation));
Assert.IsTrue(Vector3F.AreNumericallyEqual(p2.Position, Pose.Interpolate(p1, p2, 1).Position));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(p2.Orientation, Pose.Interpolate(p1, p2, 1).Orientation));
Assert.IsTrue(Vector3F.AreNumericallyEqual(InterpolationHelper.Lerp(p1.Position, p2.Position, 0.3f), Pose.Interpolate(p1, p2, 0.3f).Position));
Assert.IsTrue(
QuaternionF.AreNumericallyEqual(
InterpolationHelper.Lerp(QuaternionF.CreateRotation(p1.Orientation), QuaternionF.CreateRotation(p2.Orientation), 0.3f),
QuaternionF.CreateRotation(Pose.Interpolate(p1, p2, 0.3f).Orientation)));
}
public void ConeMass()
{
var s = new ConeShape(1, 2);
float m0;
Vector3F com0;
Matrix33F i0;
MassHelper.GetMass(s, new Vector3F(1, -2, -3), 1, true, 0.001f, 10, out m0, out com0, out i0);
var m = s.GetMesh(0.001f, 10);
m.Transform(Matrix44F.CreateScale(1, -2, -3));
float m1;
Vector3F com1;
Matrix33F i1;
MassHelper.GetMass(m, out m1, out com1, out i1);
const float e = 0.01f;
Assert.IsTrue(Numeric.AreEqual(m0, m1, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com1, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0, i1, e * (1 + i0.Trace)));
// Try other density.
float m2;
Vector3F com2;
Matrix33F i2;
MassHelper.GetMass(s, new Vector3F(1, -2, -3), 0.7f, true, 0.001f, 10, out m2, out com2, out i2);
Assert.IsTrue(Numeric.AreEqual(m0 * 0.7f, m2, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com2, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0 * 0.7f, i2, e * (1 + i0.Trace)));
// Try with target mass.
float m3;
Vector3F com3;
Matrix33F i3;
MassHelper.GetMass(s, new Vector3F(1, -2, -3), 23, false, 0.001f, 10, out m3, out com3, out i3);
Assert.IsTrue(Numeric.AreEqual(23, m3, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com3, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0 * 23 / m0, i3, e * (1 + i0.Trace)));
}
public void Test1()
{
Pose p = Pose.Identity;
Assert.AreEqual(Matrix44F.Identity, p.ToMatrix44F());
Assert.AreEqual(Matrix33F.Identity, p.Orientation);
Assert.AreEqual(Vector3F.Zero, p.Position);
p.Position = new Vector3F(1, 2, 3);
p.Orientation = Matrix33F.CreateRotation(new Vector3F(3, -4, 9), 0.49f);
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldDirection(Vector3F.UnitX), 0), p * new Vector4F(1, 0, 0, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldDirection(Vector3F.UnitY), 0), p * new Vector4F(0, 1, 0, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldDirection(Vector3F.UnitZ), 0), p * new Vector4F(0, 0, 1, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldPosition(Vector3F.UnitX), 1), p * new Vector4F(1, 0, 0, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldPosition(Vector3F.UnitY), 1), p * new Vector4F(0, 1, 0, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldPosition(Vector3F.UnitZ), 1), p * new Vector4F(0, 0, 1, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalDirection(Vector3F.UnitX), 0), p.Inverse * new Vector4F(1, 0, 0, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalDirection(Vector3F.UnitY), 0), p.Inverse * new Vector4F(0, 1, 0, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalDirection(Vector3F.UnitZ), 0), p.Inverse * new Vector4F(0, 0, 1, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalPosition(Vector3F.UnitX), 1), p.Inverse * new Vector4F(1, 0, 0, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalPosition(Vector3F.UnitY), 1), p.Inverse * new Vector4F(0, 1, 0, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalPosition(Vector3F.UnitZ), 1), p.Inverse * new Vector4F(0, 0, 1, 1)));
Pose p2 = Pose.FromMatrix(new Matrix44F(p.Orientation, Vector3F.Zero));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(p.Orientation, p2.Orientation));
Assert.IsTrue(Vector3F.AreNumericallyEqual(p2.Position, Vector3F.Zero));
Matrix44F m = p2;
m.SetColumn(3, new Vector4F(p.Position, 1));
p2 = Pose.FromMatrix(m);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(p.Orientation, p2.Orientation));
Assert.AreEqual(p.Position, p2.Position);
//Assert.IsTrue(Vector3F.AreNumericallyEqual(p.Position, p2.Position));
// Test other constructors.
Assert.AreEqual(Vector3F.Zero, new Pose(QuaternionF.CreateRotationX(0.3f)).Position);
Assert.AreEqual(Matrix33F.CreateRotationX(0.3f), new Pose(Matrix33F.CreateRotationX(0.3f)).Orientation);
Assert.AreEqual(new Vector3F(1, 2, 3), new Pose(new Vector3F(1, 2, 3)).Position);
Assert.AreEqual(Matrix33F.Identity, new Pose(new Vector3F(1, 2, 3)).Orientation);
}
public void AreEqual()
{
float originalEpsilon = Numeric.EpsilonF;
Numeric.EpsilonF = 1e-8f;
Matrix33F m0 = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
Matrix33F m1 = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
m1 += new Matrix33F(0.000001f);
Matrix33F m2 = new Matrix33F(columnMajor, MatrixOrder.ColumnMajor);
m2 += new Matrix33F(0.00000001f);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(m0, m0));
Assert.IsFalse(Matrix33F.AreNumericallyEqual(m0, m1));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(m0, m2));
Numeric.EpsilonF = originalEpsilon;
}
public void CompositeShapeWithRigidBodies()
{
// The first composite shape does not use rigid bodies.
var s = new CompositeShape();
s.Children.Add(new GeometricObject(new BoxShape(1, 2, 3), new Vector3F(1.1f, 0.3f, 0.8f), new Pose(new Vector3F(100, 10, 0), RandomHelper.Random.NextQuaternionF())));
s.Children.Add(new GeometricObject(new ConeShape(1, 2), new Vector3F(1.1f, 0.3f, 0.8f), new Pose(new Vector3F(-10, -10, 0), RandomHelper.Random.NextQuaternionF())));
float m0;
Vector3F com0;
Matrix33F i0;
MassHelper.GetMass(s, new Vector3F(1), 0.7f, true, 0.001f, 10, out m0, out com0, out i0);
// The second composite shape uses rigid bodies as children.
var r0 = new RigidBody(s.Children[0].Shape);
r0.Pose = s.Children[0].Pose;
r0.Scale = s.Children[0].Scale;
r0.MassFrame = MassFrame.FromShapeAndDensity(r0.Shape, r0.Scale, 0.7f, 0.001f, 10);
var r1 = new RigidBody(s.Children[1].Shape);
r1.Pose = s.Children[1].Pose;
r1.Scale = s.Children[1].Scale;
r1.MassFrame = MassFrame.FromShapeAndDensity(r1.Shape, r1.Scale, 0.7f, 0.001f, 10);
var s1 = new CompositeShape();
s1.Children.Add(r0);
s1.Children.Add(r1);
float m1;
Vector3F com1;
Matrix33F i1;
MassHelper.GetMass(s1, new Vector3F(1), 100, true, 0.001f, 10, out m1, out com1, out i1);
const float e = 0.01f;
Assert.IsTrue(Numeric.AreEqual(m0, m1, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com1, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0, i1, e * (1 + i0.Trace)));
}
public void ApproximateAabbMass()
{
var s = new ConvexHullOfPoints(new[] { new Vector3F(1, 1, 1), new Vector3F(2, 4, 6) });
float m0;
Vector3F com0;
Matrix33F i0;
MassHelper.GetMass(s, new Vector3F(1.2f, 2.1f, 0.6f), 0.7f, true, 0.001f, 0, out m0, out com0, out i0);
var s2 = new TransformedShape(new GeometricObject(new BoxShape(1, 3, 5), new Pose(new Vector3F(1.5f, 2.5f, 3.5f))));
float m1;
Vector3F com1;
Matrix33F i1;
MassHelper.GetMass(s2, new Vector3F(1.2f, 2.1f, 0.6f), 0.7f, true, 0.001f, 0, out m1, out com1, out i1);
const float e = 0.0001f;
Assert.IsTrue(Numeric.AreEqual(m0, m1, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com1, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0, i1, e * (1 + i0.Trace)));
}
public void Orthogonalize()
{
var m = Matrix33F.CreateRotationX(0.1f) * Matrix33F.CreateRotationY(20) * Matrix33F.CreateRotationZ(1000);
// Introduce error.
m.M01 += 0.1f;
m.M22 += 0.1f;
Assert.IsFalse(m.IsOrthogonal);
Assert.IsFalse(m.IsRotation);
m.Orthogonalize();
Assert.IsTrue(m.IsOrthogonal);
Assert.IsTrue(m.IsRotation);
// Orthogonalizing and orthogonal matrix does not change the matrix.
var n = m;
n.Orthogonalize();
Assert.IsTrue(Matrix33F.AreNumericallyEqual(m, n));
}
public void DiagonalizeTest()
{
for (int i = 0; i < 1000; i++)
{
var inertia = RandomHelper.Random.NextMatrix33F(0, 100);
// Make symmetric
inertia.M10 = inertia.M01;
inertia.M12 = inertia.M21;
inertia.M20 = inertia.M02;
Assert.IsTrue(inertia.IsSymmetric);
Vector3F inertiaDiagonale;
Matrix33F rotation;
MassHelper.DiagonalizeInertia(inertia, out inertiaDiagonale, out rotation);
Assert.IsTrue(rotation.IsRotation);
var inertia2 = rotation * Matrix33F.CreateScale(inertiaDiagonale) * rotation.Transposed;
Assert.IsTrue(Matrix33F.AreNumericallyEqual(inertia, inertia2, 0.001f)); // Epsilon = 10^-4 is already too small :-(
}
}
public void CompositeShapeWithNonUniformScaling()
{
var s = new CompositeShape();
s.Children.Add(new GeometricObject(new BoxShape(1, 2, 3), new Vector3F(1.1f, 0.3f, 0.8f), new Pose(new Vector3F(100, 10, 0))));
s.Children.Add(new GeometricObject(new SphereShape(1), new Vector3F(1.1f, 0.3f, 0.8f), new Pose(new Vector3F(-10, -10, 0))));
float m0;
Vector3F com0;
Matrix33F i0;
MassHelper.GetMass(s, new Vector3F(2, 2.1f, 2.8f), 0.7f, true, 0.001f, 10, out m0, out com0, out i0);
var m = s.GetMesh(0.001f, 6);
m.Transform(Matrix44F.CreateScale(2, 2.1f, 2.8f));
float m1;
Vector3F com1;
Matrix33F i1;
MassHelper.GetMass(m, out m1, out com1, out i1);
const float e = 0.01f;
Assert.IsTrue(Numeric.AreEqual(m0, 0.7f * m1, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com1, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0, 0.7f * i1, e * (1 + i0.Trace)));
// Try with target mass.
float m3;
Vector3F com3;
Matrix33F i3;
MassHelper.GetMass(s, new Vector3F(2, 2.1f, 2.8f), 23, false, 0.001f, 10, out m3, out com3, out i3);
Assert.IsTrue(Numeric.AreEqual(23, m3, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com3, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0 * 23 / m0, i3, e * (1 + i0.Trace)));
}
public void TransformedShapeMassWithNonuniformScaling()
{
var s = new TransformedShape(new GeometricObject(new BoxShape(3, 2, 1), new Vector3F(0.7f, 0.8f, 0.9f), new Pose(new Vector3F(-1, 7, 4))));
float m0;
Vector3F com0;
Matrix33F i0;
MassHelper.GetMass(s, new Vector3F(2, 2.1f, 2.8f), 1, true, 0.001f, 10, out m0, out com0, out i0);
var m = s.GetMesh(0.001f, 6);
m.Transform(Matrix44F.CreateScale(2, 2.1f, 2.8f));
float m1;
Vector3F com1;
Matrix33F i1;
MassHelper.GetMass(m, out m1, out com1, out i1);
const float e = 0.01f;
Assert.IsTrue(Numeric.AreEqual(m0, m1, e * (1 + m0)));
Assert.IsTrue(Vector3F.AreNumericallyEqual(com0, com1, e * (1 + com0.Length)));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(i0, i1, e * (1 + i0.Trace)));
}