/// <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>
/// Constructs a new affine transform.
///</summary>
///<param name="scaling">Scaling to apply in the linear transform.</param>
///<param name="orientation">Orientation to apply in the linear transform.</param>
///<param name="translation">Translation to apply.</param>
public AffineTransform(ref Vector3 scaling, ref Quaternion orientation, ref Vector3 translation)
{
//Create an SRT transform.
Matrix3x3.CreateScale(ref scaling, out LinearTransform);
Matrix3x3 rotation;
Matrix3x3.CreateFromQuaternion(ref orientation, out rotation);
Matrix3x3.Multiply(ref LinearTransform, ref rotation, out LinearTransform);
Translation = translation;
}
// TODO There are no tests of this method.
/// <summary>
/// Transforms point in screen space to point in 2D world space as seen by camera.
/// </summary>
/// <param name="cameraEntity">Entity with camera component attached.</param>
/// <param name="screenPoint">Point in screen space.</param>
/// <returns>Point in 2D world space corresponding to given point in screen space as seen by camera.</returns>
public static Vector2 ScreenPointTo2DWorldPoint(this Entity cameraEntity, Vector2 screenPoint)
{
if (!cameraEntity.HasComponent <CameraComponent>())
{
throw new ArgumentException("Entity is not a camera.");
}
var cameraComponent = cameraEntity.GetComponent <CameraComponent>();
var cameraTransform = cameraEntity.GetComponent <Transform2DComponent>();
var viewRectangleScale = GetViewRectangleScale(cameraEntity);
var transformationMatrix = cameraTransform.ToMatrix() * Matrix3x3.CreateScale(new Vector2(viewRectangleScale.X, -viewRectangleScale.Y)) *
Matrix3x3.CreateTranslation(new Vector2(-cameraComponent.ScreenWidth / 2.0, -cameraComponent.ScreenHeight / 2.0));
return((transformationMatrix * screenPoint.Homogeneous).ToVector2());
}
/// <summary>
/// Creates view matrix that converts coordinates from 2D space to the screen space as seen by camera.
/// </summary>
/// <param name="cameraEntity">Entity with camera component attached.</param>
/// <returns>View matrix that converts coordinates from 2D space to the screen space as seen by camera.</returns>
public static Matrix3x3 Create2DWorldToScreenMatrix(this Entity cameraEntity)
{
if (!cameraEntity.HasComponent <CameraComponent>())
{
throw new ArgumentException("Entity is not a camera.");
}
var cameraTransform = cameraEntity.GetComponent <Transform2DComponent>();
var cameraScale = cameraTransform.Scale;
var viewRectangleScale = GetViewRectangleScale(cameraEntity);
var finalCameraScale = new Vector2(cameraScale.X * viewRectangleScale.X, cameraScale.Y * viewRectangleScale.Y);
return(Matrix3x3.CreateScale(new Vector2(1 / finalCameraScale.X, 1 / finalCameraScale.Y)) *
Matrix3x3.CreateRotation(-cameraTransform.Rotation) *
Matrix3x3.CreateTranslation(-cameraTransform.Translation) * Matrix3x3.Identity);
}
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 CreateScale(double sx, double sy, double m11, double m12, double m13, double m21, double m22,
double m23, double m31, double m32, double m33)
{
// Arrange
var scaleVector = new Vector2(sx, sy);
// Act
var scaleMatrix = Matrix3x3.CreateScale(scaleVector);
// Assert
Assert.That(scaleMatrix.M11, Is.EqualTo(m11));
Assert.That(scaleMatrix.M12, Is.EqualTo(m12));
Assert.That(scaleMatrix.M13, Is.EqualTo(m13));
Assert.That(scaleMatrix.M21, Is.EqualTo(m21));
Assert.That(scaleMatrix.M22, Is.EqualTo(m22));
Assert.That(scaleMatrix.M23, Is.EqualTo(m23));
Assert.That(scaleMatrix.M31, Is.EqualTo(m31));
Assert.That(scaleMatrix.M32, Is.EqualTo(m32));
Assert.That(scaleMatrix.M33, Is.EqualTo(m33));
}
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>
/// Calculates necessary information for velocity solving.
/// Called by preStep(float dt)
/// </summary>
/// <param name="dt">Time in seconds since the last update.</param>
public override void Update(float dt)
{
Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA);
Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB);
float errorReductionParameter;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReductionParameter, out softness);
//Mass System.Numerics.Matrix4x4
Matrix3x3 k;
Matrix3x3 linearComponent;
Matrix3x3.CreateCrossProduct(ref worldOffsetA, out rACrossProduct);
Matrix3x3.CreateCrossProduct(ref worldOffsetB, out rBCrossProduct);
if (connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3.CreateScale(connectionA.inverseMass + connectionB.inverseMass, out linearComponent);
Matrix3x3 angularComponentA, angularComponentB;
Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA);
Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB);
Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k);
Matrix3x3.Subtract(ref k, ref angularComponentB, out k);
}
else if (connectionA.isDynamic && !connectionB.isDynamic)
{
Matrix3x3.CreateScale(connectionA.inverseMass, out linearComponent);
Matrix3x3 angularComponentA;
Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA);
Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k);
}
else if (!connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3.CreateScale(connectionB.inverseMass, out linearComponent);
Matrix3x3 angularComponentB;
Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB);
Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentB, out k);
}
else
{
throw new InvalidOperationException("Cannot constrain two kinematic bodies.");
}
k.M11 += softness;
k.M22 += softness;
k.M33 += softness;
Matrix3x3.Invert(ref k, out massMatrix);
Vector3Ex.Add(ref connectionB.position, ref worldOffsetB, out error);
Vector3Ex.Subtract(ref error, ref connectionA.position, out error);
Vector3Ex.Subtract(ref error, ref worldOffsetA, out error);
Vector3Ex.Multiply(ref error, -errorReductionParameter, out biasVelocity);
//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;
}
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public AddRemoveStressDemo(DemosGame game)
: base(game)
{
Space.Remove(vehicle.Vehicle);
var compoundShape = new CompoundShape(new List <CompoundShapeEntry>
{
new CompoundShapeEntry(new BoxShape(1, 1, 1), new Vector3(0, 1, 0), 1),
new CompoundShapeEntry(new BoxShape(2, 1, 2), new Vector3(), 1),
new CompoundShapeEntry(new BoxShape(1, 1, 1), new Vector3(0, -1, 0), 1)
});
for (int i = 0; i < 300; ++i)
{
var toAdd = new Entity(compoundShape, 10);
addedEntities.Add(toAdd);
}
var boxShape = new BoxShape(1, 1, 1);
for (int i = 0; i < 300; ++i)
{
var toAdd = new Entity(boxShape, 10);
addedEntities.Add(toAdd);
}
Vector3[] vertices;
int[] indices;
ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load <Model>("cube"), out vertices, out indices);
var mobileMeshShape = new MobileMeshShape(vertices, indices, new AffineTransform(Matrix3x3.CreateScale(1, 2, 1), new Vector3()), MobileMeshSolidity.Counterclockwise);
for (int i = 0; i < 300; ++i)
{
var toAdd = new Entity(mobileMeshShape, 10);
addedEntities.Add(toAdd);
}
for (int i = 0; i < addedEntities.Count; ++i)
{
var entity = addedEntities[i];
entity.Gravity = new Vector3();
entity.Position = GetRandomPosition(random);
entity.LinearVelocity = 3 * Vector3.Normalize(entity.Position);
Space.Add(entity);
}
var playgroundModel = game.Content.Load <Model>("playground");
ModelDataExtractor.GetVerticesAndIndicesFromModel(playgroundModel, out vertices, out indices);
var staticMesh = new StaticMesh(vertices, indices, new AffineTransform(Matrix3x3.CreateFromAxisAngle(Vector3.Up, MathHelper.Pi), new Vector3(0, -30, 0)));
staticMesh.Sidedness = TriangleSidedness.Counterclockwise;
Space.Add(staticMesh);
game.ModelDrawer.Add(staticMesh);
game.Camera.Position = new Vector3(0, 6, 15);
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Transform point into world space.
Matrix3x3.Transform(ref localPoint, ref entity.orientationMatrix, out r);
Vector3.Add(ref r, ref entity.position, out worldPoint);
float updateRate = 1 / dt;
if (settings.mode == MotorMode.Servomechanism)
{
Vector3.Subtract(ref settings.servo.goal, ref worldPoint, out error);
float separationDistance = error.Length();
if (separationDistance > Toolbox.BigEpsilon)
{
float errorReduction;
settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness);
//The rate of correction can be based on a constant correction velocity as well as a 'spring like' correction velocity.
//The constant correction velocity could overshoot the destination, so clamp it.
float correctionSpeed = MathHelper.Min(settings.servo.baseCorrectiveSpeed, separationDistance * updateRate) +
separationDistance * errorReduction;
Vector3.Multiply(ref error, correctionSpeed / separationDistance, out biasVelocity);
//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
{
//Wouldn't want to use a bias from an earlier frame.
biasVelocity = new Vector3();
}
}
else
{
usedSoftness = settings.velocityMotor.softness * updateRate;
biasVelocity = settings.velocityMotor.goalVelocity;
error = Vector3.Zero;
}
//Compute the maximum force that can be applied this frame.
ComputeMaxForces(settings.maximumForce, dt);
//COMPUTE EFFECTIVE MASS MATRIX
//Transforms a change in velocity to a change in momentum when multiplied.
Matrix3x3 linearComponent;
Matrix3x3.CreateScale(entity.inverseMass, out linearComponent);
Matrix3x3 rACrossProduct;
Matrix3x3.CreateCrossProduct(ref r, out rACrossProduct);
Matrix3x3 angularComponentA;
Matrix3x3.Multiply(ref rACrossProduct, ref entity.inertiaTensorInverse, out angularComponentA);
Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out effectiveMassMatrix);
effectiveMassMatrix.M11 += usedSoftness;
effectiveMassMatrix.M22 += usedSoftness;
effectiveMassMatrix.M33 += usedSoftness;
Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
}
void ComputeShapeInformation(TransformableMeshData data, out ShapeDistributionInformation shapeInformation)
{
//Compute the surface vertices of the shape.
surfaceVertices.Clear();
try
{
ConvexHullHelper.GetConvexHull(data.vertices, surfaceVertices);
for (int i = 0; i < surfaceVertices.Count; i++)
{
AffineTransform.Transform(ref surfaceVertices.Elements[i], ref data.worldTransform, out surfaceVertices.Elements[i]);
}
}
catch
{
surfaceVertices.Clear();
//If the convex hull failed, then the point set has no volume. A mobile mesh is allowed to have zero volume, however.
//In this case, compute the bounding box of all points.
BoundingBox box = new BoundingBox();
for (int i = 0; i < data.vertices.Length; i++)
{
Vector3 v;
data.GetVertexPosition(i, out v);
if (v.X > box.Max.X)
{
box.Max.X = v.X;
}
if (v.X < box.Min.X)
{
box.Min.X = v.X;
}
if (v.Y > box.Max.Y)
{
box.Max.Y = v.Y;
}
if (v.Y < box.Min.Y)
{
box.Min.Y = v.Y;
}
if (v.Z > box.Max.Z)
{
box.Max.Z = v.Z;
}
if (v.Z < box.Min.Z)
{
box.Min.Z = v.Z;
}
}
//Add the corners. This will overestimate the size of the surface a bit.
surfaceVertices.Add(box.Min);
surfaceVertices.Add(box.Max);
surfaceVertices.Add(new Vector3(box.Min.X, box.Min.Y, box.Max.Z));
surfaceVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Min.Z));
surfaceVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Min.Z));
surfaceVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Max.Z));
surfaceVertices.Add(new Vector3(box.Max.X, box.Max.Y, box.Min.Z));
surfaceVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Max.Z));
}
shapeInformation.Center = new Vector3();
if (solidity == MobileMeshSolidity.Solid)
{
//The following inertia tensor calculation assumes a closed mesh.
shapeInformation.Volume = 0;
for (int i = 0; i < data.indices.Length; i += 3)
{
Vector3 v2, v3, v4;
data.GetTriangle(i, out v2, out v3, out v4);
//Determinant is 6 * volume. It's signed, though; this is because the mesh isn't necessarily convex nor centered on the origin.
float tetrahedronVolume = v2.X * (v3.Y * v4.Z - v3.Z * v4.Y) -
v3.X * (v2.Y * v4.Z - v2.Z * v4.Y) +
v4.X * (v2.Y * v3.Z - v2.Z * v3.Y);
shapeInformation.Volume += tetrahedronVolume;
shapeInformation.Center += tetrahedronVolume * (v2 + v3 + v4);
}
shapeInformation.Center /= shapeInformation.Volume * 4;
shapeInformation.Volume /= 6;
shapeInformation.Volume = Math.Abs(shapeInformation.Volume);
data.worldTransform.Translation -= shapeInformation.Center;
//Source: Explicit Exact Formulas for the 3-D Tetrahedron Inertia Tensor in Terms of its Vertex Coordinates
//http://www.scipub.org/fulltext/jms2/jms2118-11.pdf
//x1, x2, x3, x4 are origin, triangle1, triangle2, triangle3
//Looking to find inertia tensor matrix of the form
// [ a -b' -c' ]
// [ -b' b -a' ]
// [ -c' -a' c ]
float a = 0, b = 0, c = 0, ao = 0, bo = 0, co = 0;
float totalWeight = 0;
for (int i = 0; i < data.indices.Length; i += 3)
{
Vector3 v2, v3, v4;
data.GetTriangle(i, out v2, out v3, out v4);
//Determinant is 6 * volume. It's signed, though; this is because the mesh isn't necessarily convex nor centered on the origin.
float tetrahedronVolume = v2.X * (v3.Y * v4.Z - v3.Z * v4.Y) -
v3.X * (v2.Y * v4.Z - v2.Z * v4.Y) +
v4.X * (v2.Y * v3.Z - v2.Z * v3.Y);
totalWeight += tetrahedronVolume;
a += tetrahedronVolume * (v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y +
v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z);
b += tetrahedronVolume * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X +
v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z);
c += tetrahedronVolume * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X +
v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y);
ao += tetrahedronVolume * (2 * v2.Y * v2.Z + v3.Y * v2.Z + v4.Y * v2.Z + v2.Y * v3.Z + 2 * v3.Y * v3.Z + v4.Y * v3.Z + v2.Y * v4.Z + v3.Y * v4.Z + 2 * v4.Y * v4.Z);
bo += tetrahedronVolume * (2 * v2.X * v2.Z + v3.X * v2.Z + v4.X * v2.Z + v2.X * v3.Z + 2 * v3.X * v3.Z + v4.X * v3.Z + v2.X * v4.Z + v3.X * v4.Z + 2 * v4.X * v4.Z);
co += tetrahedronVolume * (2 * v2.X * v2.Y + v3.X * v2.Y + v4.X * v2.Y + v2.X * v3.Y + 2 * v3.X * v3.Y + v4.X * v3.Y + v2.X * v4.Y + v3.X * v4.Y + 2 * v4.X * v4.Y);
}
float density = 1 / totalWeight;
float diagonalFactor = density / 10;
float offFactor = -density / 20;
a *= diagonalFactor;
b *= diagonalFactor;
c *= diagonalFactor;
ao *= offFactor;
bo *= offFactor;
co *= offFactor;
shapeInformation.VolumeDistribution = new Matrix3x3(a, bo, co,
bo, b, ao,
co, ao, c);
}
else
{
shapeInformation.Center = new Vector3();
float totalWeight = 0;
for (int i = 0; i < data.indices.Length; i += 3)
{ //Configure the inertia tensor to be local.
Vector3 vA, vB, vC;
data.GetTriangle(i, out vA, out vB, out vC);
Vector3 vAvB;
Vector3 vAvC;
Vector3.Subtract(ref vB, ref vA, out vAvB);
Vector3.Subtract(ref vC, ref vA, out vAvC);
Vector3 cross;
Vector3.Cross(ref vAvB, ref vAvC, out cross);
float weight = cross.Length();
totalWeight += weight;
shapeInformation.Center += weight * (vA + vB + vC) / 3;
}
shapeInformation.Center /= totalWeight;
shapeInformation.Volume = 0;
data.worldTransform.Translation -= shapeInformation.Center;
shapeInformation.VolumeDistribution = new Matrix3x3();
for (int i = 0; i < data.indices.Length; i += 3)
{ //Configure the inertia tensor to be local.
Vector3 vA, vB, vC;
data.GetTriangle(i, out vA, out vB, out vC);
Vector3 vAvB;
Vector3 vAvC;
Vector3.Subtract(ref vB, ref vA, out vAvB);
Vector3.Subtract(ref vC, ref vA, out vAvC);
Vector3 cross;
Vector3.Cross(ref vAvB, ref vAvC, out cross);
float weight = cross.Length();
totalWeight += weight;
Matrix3x3 innerProduct;
Matrix3x3.CreateScale(vA.LengthSquared(), out innerProduct);
Matrix3x3 outerProduct;
Matrix3x3.CreateOuterProduct(ref vA, ref vA, out outerProduct);
Matrix3x3 contribution;
Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out contribution);
Matrix3x3.Multiply(ref contribution, weight, out contribution);
Matrix3x3.Add(ref shapeInformation.VolumeDistribution, ref contribution, out shapeInformation.VolumeDistribution);
Matrix3x3.CreateScale(vB.LengthSquared(), out innerProduct);
Matrix3x3.CreateOuterProduct(ref vB, ref vB, out outerProduct);
Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out outerProduct);
Matrix3x3.Multiply(ref contribution, weight, out contribution);
Matrix3x3.Add(ref shapeInformation.VolumeDistribution, ref contribution, out shapeInformation.VolumeDistribution);
Matrix3x3.CreateScale(vC.LengthSquared(), out innerProduct);
Matrix3x3.CreateOuterProduct(ref vC, ref vC, out outerProduct);
Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out contribution);
Matrix3x3.Multiply(ref contribution, weight, out contribution);
Matrix3x3.Add(ref shapeInformation.VolumeDistribution, ref contribution, out shapeInformation.VolumeDistribution);
}
Matrix3x3.Multiply(ref shapeInformation.VolumeDistribution, 1 / (6 * totalWeight), out shapeInformation.VolumeDistribution);
}
////Configure the inertia tensor to be local.
//Vector3 finalOffset = shapeInformation.Center;
//Matrix3X3 finalInnerProduct;
//Matrix3X3.CreateScale(finalOffset.LengthSquared(), out finalInnerProduct);
//Matrix3X3 finalOuterProduct;
//Matrix3X3.CreateOuterProduct(ref finalOffset, ref finalOffset, out finalOuterProduct);
//Matrix3X3 finalContribution;
//Matrix3X3.Subtract(ref finalInnerProduct, ref finalOuterProduct, out finalContribution);
//Matrix3X3.Subtract(ref shapeInformation.VolumeDistribution, ref finalContribution, out shapeInformation.VolumeDistribution);
}
/// <summary> /// Creates 2D transformation matrix that represents this transform component. /// </summary> /// <returns>2D transformation matrix representing this transform component.</returns> public Matrix3x3 ToMatrix() => Matrix3x3.CreateTranslation(Translation) * Matrix3x3.CreateRotation(Rotation) * Matrix3x3.CreateScale(Scale) * Matrix3x3.Identity;
private static void Run()
{
#region Near Equality Methods
var x = 1.0;
var y = 2.0;
bool isItTrueThat = x.IsPracticallySame(y);
isItTrueThat = x.IsPracticallySame(y);
Vector2 v2_1 = new Vector2(1.0, 2.0);
Vector2 v2_2 = new Vector2(1.00000000001, 2.000000000002);
isItTrueThat = v2_1.IsPracticallySame(v2_2);
Vector3 v3_1 = new Vector3(1.0, 2.0, 3.0);
Vector3 v3_2 = new Vector3(1.00000000001, 2.000000000002, 3.0);
isItTrueThat = v3_1.IsPracticallySame(v3_2);
isItTrueThat = x.IsNegligible();
isItTrueThat = v2_1.IsNegligible();
isItTrueThat = v3_1.IsNegligible();
isItTrueThat = x.IsGreaterThanNonNegligible(y);
isItTrueThat = x.IsLessThanNonNegligible(y);
#endregion
#region All Vector2 Methods
v2_1 = new Vector2();
Vector2 nullVector2 = Vector2.Null;
Vector2 zeroVector2 = Vector2.Zero;
Vector2 oneVector2 = Vector2.One;
Vector2 unitVector2X = Vector2.UnitX;
Vector2 unitVector2Y = Vector2.UnitY;
Vector2 copyVector = v2_1.Copy();
double[] coordinates = v2_1.Position;
x = v2_1.X;
x = v2_1[0];
y = v2_1.Y;
y = v2_1[1];
double length = v2_1.Length();
double lengthSquared = v2_1.LengthSquared();
double distance = v2_1.Distance(zeroVector2);
double distanceSquared = v2_1.DistanceSquared(zeroVector2);
Vector2 normal = v2_1.Normalize();
Vector2 reflect = v2_1.Reflect(unitVector2Y);
Vector2 clamp = v2_1.Clamp(zeroVector2, oneVector2);
Vector2 lerp = v2_1.Lerp(oneVector2, 0.5);
v2_2 = v2_1 + v2_1;
v2_2 = v2_1 - v2_1;
v2_2 = v2_1 * v2_1; //not dot or cross - basically a
//component to component product vector whos terms sum to dot product
v2_2 = v2_1 - v2_1;
v2_2 = v2_1 / v2_1;
v2_2 = v2_1 / new double();
v2_2 = -v2_1;
isItTrueThat = v2_1.IsNull();
isItTrueThat = v2_1.IsNegligible();
v2_1.CopyTo(coordinates);
isItTrueThat = v2_1 == v2_2;
isItTrueThat = v2_1 != v2_2;
double dot = v2_1.Dot(v2_2);
double cross = v2_1.Cross(v2_2);
Vector2 minVector = Vector2.Min(v2_1, v2_2);
Vector2 maxVector = Vector2.Max(v2_1, v2_2);
Vector2 absVector = Vector2.Abs(v2_1);
Vector2 sqrtVector = Vector2.SquareRoot(v2_1);
Matrix3x3 m3x3 = new Matrix3x3();
v2_1 = v2_1.Transform(m3x3);
v2_1 = v2_1.TransformNoTranslate(m3x3);
Matrix4x4 m4x4 = new Matrix4x4();
v2_1 = v2_1.Transform(m4x4);
v2_1 = v2_1.TransformNoTranslate(m4x4);
v2_1 = v2_1.Transform(new Quaternion());
#endregion
#region All Matrix3x3 Methods
isItTrueThat = m3x3.IsProjectiveTransform;
double value = m3x3.M11;
value = m3x3.M12;
value = m3x3.M13;
value = m3x3.M21;
value = m3x3.M22;
value = m3x3.M23;
value = m3x3.M31;
value = m3x3.M32;
value = m3x3.M33;
m3x3 = Matrix3x3.Identity;
m3x3 = Matrix3x3.Null;
isItTrueThat = m3x3.IsIdentity();
isItTrueThat = m3x3.IsNull();
Vector2 t = m3x3.Translation;
m3x3 = Matrix3x3.CreateTranslation(t);
m3x3 = Matrix3x3.CreateTranslation(x, y);
m3x3 = Matrix3x3.CreateScale(1.0);
m3x3 = Matrix3x3.CreateScale(x, y);
m3x3 = Matrix3x3.CreateScale(Vector2.One);
m3x3 = Matrix3x3.CreateScale(x, y, v2_2); //vResult is the center of scaling
m3x3 = Matrix3x3.CreateSkew(2.0, 2.0);
m3x3 = Matrix3x3.CreateSkew(2.0, 2.0, v2_2); //vResult is the center of skewing
m3x3 = Matrix3x3.CreateRotation(1.0); //in radians
m3x3 = Matrix3x3.CreateRotation(1.0, v2_2); //vResult is the center of rotate
m3x3 = m3x3.Transpose();
isItTrueThat = Matrix3x3.Invert(m3x3, out Matrix3x3 invM3x3);
var d = m3x3.GetDeterminant();
m3x3 = Matrix3x3.Lerp(m3x3, m3x3, 0.5);
m3x3 = -m3x3;
var m3x3Another = 4.0 * m3x3;
m3x3 = m3x3 + m3x3;
m3x3 = m3x3 - m3x3;
m3x3 = m3x3 * m3x3;
isItTrueThat = m3x3 == m3x3Another;
isItTrueThat = m3x3 != m3x3Another;
#endregion
#region All Vector3 Methods
v3_1 = new Vector3();
v3_1 = new Vector3(v2_1, 0.0);
Vector3 nullVector3 = Vector3.Null;
Vector3 zeroVector3 = Vector3.Zero;
Vector3 oneVector3 = Vector3.One;
Vector3 unitVector3X = Vector3.UnitX;
Vector3 unitVector3Y = Vector3.UnitY;
Vector3 unitVector3Z = Vector3.UnitZ;
unitVector3X = Vector3.UnitVector(CartesianDirections.XNegative);
unitVector3X = Vector3.UnitVector(0);
Vector3 copyVector3 = v3_1.Copy();
coordinates = v3_1.Position;
x = v3_1.X;
x = v3_1[0];
y = v3_1.Y;
y = v3_1[1];
double z = v3_1.Z;
y = v3_1[2];
length = v3_1.Length();
lengthSquared = v3_1.LengthSquared();
distance = v3_1.Distance(zeroVector3);
distanceSquared = v3_1.DistanceSquared(zeroVector3);
Vector3 normal3 = v3_1.Normalize();
Vector3 reflect3 = v3_1.Reflect(unitVector3Y);
Vector3 clamp3 = v3_1.Clamp(zeroVector3, oneVector3);
Vector3 lerp3 = v3_1.Lerp(oneVector3, 0.5);
v3_2 = v3_1 + v3_1;
v3_2 = v3_1 - v3_1;
v3_2 = v3_1 * v3_1; //not dot or cross - basically a
//component to component product vector whos terms sum to dot product
v3_2 = v3_1 - v3_1;
v3_2 = v3_1 / v3_1;
v3_2 = v3_1 / new double();
v3_2 = -v3_1;
isItTrueThat = v3_1.IsNull();
isItTrueThat = v3_1.IsNegligible();
v3_1.CopyTo(coordinates);
isItTrueThat = v3_1 == v3_2;
isItTrueThat = v3_1 != v3_2;
double dot3 = v3_1.Dot(v3_2);
Vector3 cross3 = v3_1.Cross(v3_2);
Vector3 minVector3 = Vector3.Min(v3_1, v3_2);
Vector3 maxVector3 = Vector3.Max(v3_1, v3_2);
Vector3 absVector3 = Vector3.Abs(v3_1);
Vector3 sqrtVector3 = Vector3.SquareRoot(v3_1);
m3x3 = new Matrix3x3();
v3_1 = v3_1.Multiply(m3x3);
m4x4 = new Matrix4x4();
v3_1 = v3_1.Transform(m4x4);
v3_1 = v3_1.TransformNoTranslate(m4x4);
v3_1 = v3_1.Transform(new Quaternion());
#endregion
#region All Matrix4x4 Methods
isItTrueThat = m4x4.IsProjectiveTransform;
value = m4x4.M11;
value = m4x4.M12;
value = m4x4.M13;
value = m4x4.M14;
value = m4x4.M21;
value = m4x4.M22;
value = m4x4.M23;
value = m4x4.M24;
value = m4x4.M31;
value = m4x4.M32;
value = m4x4.M33;
value = m4x4.M34;
value = m4x4.M41;
value = m4x4.M42;
value = m4x4.M43;
value = m4x4.M44;
m4x4 = Matrix4x4.Identity;
m4x4 = Matrix4x4.Null;
m4x4 = new Matrix4x4(m3x3);
isItTrueThat = m4x4.IsIdentity();
isItTrueThat = m4x4.IsNull();
Vector3 t3 = m4x4.TranslationAsVector;
m4x4 = Matrix4x4.CreateBillboard(v3_1, v3_1, v3_1, v3_1);
m4x4 = Matrix4x4.CreateConstrainedBillboard(v3_1, v3_1, v3_1, v3_1, v3_1);
m4x4 = Matrix4x4.CreateTranslation(t3);
m4x4 = Matrix4x4.CreateTranslation(x, y, z);
m4x4 = Matrix4x4.CreateScale(1.0);
m4x4 = Matrix4x4.CreateScale(x, y, z);
m4x4 = Matrix4x4.CreateScale(v3_1);
m4x4 = Matrix4x4.CreateScale(v3_1, v3_2); //vOther is the center of scaling
m4x4 = Matrix4x4.CreateScale(x, y, z, v3_2); //vOther is the center of scaling
m4x4 = Matrix4x4.CreateRotationX(1.0); //in radians
m4x4 = Matrix4x4.CreateRotationX(1.0, v3_2); //vOther is the center of rotate
m4x4 = Matrix4x4.CreateRotationY(1.0); //in radians
m4x4 = Matrix4x4.CreateRotationY(1.0, v3_2); //vOther is the center of rotate
m4x4 = Matrix4x4.CreateRotationZ(1.0); //in radians
m4x4 = Matrix4x4.CreateRotationZ(1.0, v3_2); //vOther is the center of rotate
m4x4 = Matrix4x4.CreateFromAxisAngle(v3_2, 1.0); //vOther is the center of rotate
m4x4 = Matrix4x4.CreatePerspectiveFieldOfView(1.0, 2.0, 3.0, 4.0);
m4x4 = Matrix4x4.CreatePerspective(1.0, 1.0, 1.0, 1.0);
m4x4 = Matrix4x4.CreatePerspectiveOffCenter(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
m4x4 = Matrix4x4.CreateOrthographic(1.0, 2.0, 3.0, 4.0);
m4x4 = Matrix4x4.CreateOrthographicOffCenter(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
m4x4 = Matrix4x4.CreateLookAt(v3_1, v3_1, v3_2);
m4x4 = Matrix4x4.CreateWorld(v3_1, v3_1, v3_2);
m4x4 = Matrix4x4.CreateFromYawPitchRoll(1.0, 2.0, 3.0);
m4x4 = Matrix4x4.CreateFromQuaternion(new Quaternion());
m4x4 = Matrix4x4.CreateShadow(v3_1, new Plane(d, v3_2));
m4x4 = Matrix4x4.CreateReflection(new Plane(d, v3_2));
m4x4 = m4x4.Transpose();
isItTrueThat = Matrix4x4.Invert(m4x4, out Matrix4x4 invm4x4);
d = m4x4.GetDeterminant();
isItTrueThat = m4x4.Decompose(out var scale, out var rotQ, out var trans3);
m4x4 = m4x4.Transform(rotQ);
m4x4 = Matrix4x4.Lerp(m4x4, m4x4, 0.5);
m4x4 = -m4x4;
var m4x4Another = 4.0 * m4x4;
m4x4 = m4x4 + m4x4;
m4x4 = m4x4 - m4x4;
m4x4 = m4x4 * m4x4;
isItTrueThat = m4x4 == m4x4Another;
isItTrueThat = m4x4 != m4x4Another;
#endregion
#region All Quaternion Methods
var quat = new Quaternion();
x = quat.X;
y = quat.Y;
z = quat.Z;
var w = quat.W;
var quatOther = new Quaternion(v3_1, w);
quat = new Quaternion(x, y, z, w);
quat = Quaternion.Identity;
isItTrueThat = quat.IsIdentity();
quat = Quaternion.Null;
isItTrueThat = quat.IsNull();
length = quat.Length();
length = quat.LengthSquared();
quat = quat.Normalize();
quat = quat.Conjugate();
quat = quat.Inverse();
quat = Quaternion.CreateFromAxisAngle(v3_1, d);
quat = Quaternion.CreateFromYawPitchRoll(1.0, 2.0, 3.0);
quat = Quaternion.CreateFromRotationMatrix(m4x4);
dot = quat.Dot(quat);
quat = Quaternion.Lerp(quat, quat, 0.5);
quat = Quaternion.Slerp(quat, quat, 0.5);
quat = -quat;
quat = quat + quat;
quat = quat - quat;
quat = quat * quat;
quat = 4.0 * quat;
quat = quat / quat;
isItTrueThat = quat == quatOther;
isItTrueThat = quat != quatOther;
#endregion
#region All Plane Methods
var plane = new Plane();
plane = new Plane(d, v3_1);
var planeOther = new Plane(d, new Vector3(x, y, z));
v3_1 = plane.Normal;
d = plane.DistanceToOrigin;
plane = Plane.CreateFromVertices(v3_1, unitVector3X, v3_2);
plane.Normalize();
plane.Transform(m4x4);
plane.Transform(quat);
dot = plane.DotCoordinate(v3_1);
dot = plane.DotNormal(v3_1);
isItTrueThat = plane == planeOther;
isItTrueThat = plane != planeOther;
#endregion
#region IEnumerable<double> Statistics
IEnumerable <double> numbers = new[] { 1.1, 2.2, 3.3 };
var mean = numbers.Mean();
var median = numbers.Median();
var nrmse = numbers.NormalizedRootMeanSquareError();
var nthMedian = numbers.NthOrderStatistic(3);
var varMean = numbers.VarianceFromMean(mean);
var varMedian = numbers.VarianceFromMedian(median);
#endregion
}