/// <summary>
/// Multiplies the two matrices.
/// </summary>
/// <param name="a">First matrix to multiply.</param>
/// <param name="b">Second matrix to multiply.</param>
/// <param name="result">Product of the multiplication.</param>
public static void Multiply(ref Matrix2X3 a, ref Matrix3X2 b, out Matrix2X2 result)
{
result.M11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31;
result.M12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32;
result.M21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31;
result.M22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32;
}
/// <summary>
/// Transforms the vector by the matrix.
/// </summary>
/// <param name="v">Vector2 to transform. Considered to be a column vector for purposes of multiplication.</param>
/// <param name="matrix">Matrix to use as the transformation.</param>
/// <param name="result">Column vector product of the transformation.</param>
public static void Transform(ref Vector3 v, ref Matrix2X3 matrix, out Vector2 result)
{
#if !WINDOWS
result = new Vector2();
#endif
result.X = matrix.M11 * v.X + matrix.M12 * v.Y + matrix.M13 * v.Z;
result.Y = matrix.M21 * v.X + matrix.M22 * v.Y + matrix.M23 * v.Z;
}
/// <summary>
/// Transforms the vector by the matrix.
/// </summary>
/// <param name="v">Vector2 to transform. Considered to be a row vector for purposes of multiplication.</param>
/// <param name="matrix">Matrix to use as the transformation.</param>
/// <param name="result">Row vector product of the transformation.</param>
public static void Transform(ref Vector2 v, ref Matrix2X3 matrix, out Vector3 result)
{
#if !WINDOWS
result = new Vector3();
#endif
result.X = v.X * matrix.M11 + v.Y * matrix.M21;
result.Y = v.X * matrix.M12 + v.Y * matrix.M22;
result.Z = v.X * matrix.M13 + v.Y * matrix.M23;
}
internal void Setup(ConvexContactManifoldConstraint contactManifoldConstraint)
{
this.contactManifoldConstraint = contactManifoldConstraint;
isActive = true;
linearA = new Matrix2X3();
entityA = contactManifoldConstraint.EntityA;
entityB = contactManifoldConstraint.EntityB;
}
/// <summary>
/// Computes the transposed matrix of a matrix.
/// </summary>
/// <param name="matrix">Matrix to transpose.</param>
/// <param name="result">Transposed matrix.</param>
public static void Transpose(ref Matrix3X2 matrix, out Matrix2X3 result)
{
result.M11 = matrix.M11;
result.M12 = matrix.M21;
result.M13 = matrix.M31;
result.M21 = matrix.M12;
result.M22 = matrix.M22;
result.M23 = matrix.M32;
}
internal void Setup(ConvexContactManifoldConstraint contactManifoldConstraint)
{
this.contactManifoldConstraint = contactManifoldConstraint;
isActive = true;
linearA = new Matrix2X3();
entityA = contactManifoldConstraint.EntityA;
entityB = contactManifoldConstraint.EntityB;
entityADynamic = entityA != null && entityA.isDynamic;
entityBDynamic = entityB != null && entityB.isDynamic;
}
/// <summary>
/// Multiplies the two matrices.
/// </summary>
/// <param name="a">First matrix to multiply.</param>
/// <param name="b">Second matrix to multiply.</param>
/// <param name="result">Product of the multiplication.</param>
public static void Multiply(ref Matrix2X3 a, ref Matrix3X3 b, out Matrix2X3 result)
{
float resultM11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31;
float resultM12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32;
float resultM13 = a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33;
float resultM21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31;
float resultM22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32;
float resultM23 = a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33;
result.M11 = resultM11;
result.M12 = resultM12;
result.M13 = resultM13;
result.M21 = resultM21;
result.M22 = resultM22;
result.M23 = resultM23;
}
/// <summary>
/// Adds the two matrices together on a per-element basis.
/// </summary>
/// <param name="a">First matrix to add.</param>
/// <param name="b">Second matrix to add.</param>
/// <param name="result">Sum of the two matrices.</param>
public static void Add(ref Matrix2X3 a, ref Matrix2X3 b, out Matrix2X3 result)
{
float m11 = a.M11 + b.M11;
float m12 = a.M12 + b.M12;
float m13 = a.M13 + b.M13;
float m21 = a.M21 + b.M21;
float m22 = a.M22 + b.M22;
float m23 = a.M23 + b.M23;
result.M11 = m11;
result.M12 = m12;
result.M13 = m13;
result.M21 = m21;
result.M22 = m22;
result.M23 = m23;
}
/// <summary>
/// Subtracts the two matrices from each other on a per-element basis.
/// </summary>
/// <param name="a">First matrix to subtract.</param>
/// <param name="b">Second matrix to subtract.</param>
/// <param name="result">Difference of the two matrices.</param>
public static void Subtract(ref Matrix2X3 a, ref Matrix2X3 b, out Matrix2X3 result)
{
float m11 = a.M11 - b.M11;
float m12 = a.M12 - b.M12;
float m13 = a.M13 - b.M13;
float m21 = a.M21 - b.M21;
float m22 = a.M22 - b.M22;
float m23 = a.M23 - b.M23;
result.M11 = m11;
result.M12 = m12;
result.M13 = m13;
result.M21 = m21;
result.M22 = m22;
result.M23 = m23;
}
/// <summary>
/// Negates every element in the matrix.
/// </summary>
/// <param name="matrix">Matrix to negate.</param>
/// <param name="result">Negated matrix.</param>
public static void Negate(ref Matrix2X3 matrix, out Matrix2X3 result)
{
float m11 = -matrix.M11;
float m12 = -matrix.M12;
float m13 = -matrix.M13;
float m21 = -matrix.M21;
float m22 = -matrix.M22;
float m23 = -matrix.M23;
result.M11 = m11;
result.M12 = m12;
result.M13 = m13;
result.M21 = m21;
result.M22 = m22;
result.M23 = m23;
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
contactCount = contactManifoldConstraint.penetrationConstraints.count;
switch (contactCount)
{
case 1:
manifoldCenter = contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position;
break;
case 2:
Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
out manifoldCenter);
manifoldCenter.X *= .5f;
manifoldCenter.Y *= .5f;
manifoldCenter.Z *= .5f;
break;
case 3:
Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
out manifoldCenter);
Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position,
ref manifoldCenter,
out manifoldCenter);
manifoldCenter.X *= .333333333f;
manifoldCenter.Y *= .333333333f;
manifoldCenter.Z *= .333333333f;
break;
case 4:
//This isn't actually the center of the manifold. Is it good enough? Sure seems like it.
Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
out manifoldCenter);
Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position,
ref manifoldCenter,
out manifoldCenter);
Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[3].contact.Position,
ref manifoldCenter,
out manifoldCenter);
manifoldCenter.X *= .25f;
manifoldCenter.Y *= .25f;
manifoldCenter.Z *= .25f;
break;
default:
manifoldCenter = Toolbox.NoVector;
break;
}
//Compute the three dimensional relative velocity at the point.
Vector3 velocityA, velocityB;
if (entityA != null)
{
Vector3.Subtract(ref manifoldCenter, ref entityA.position, out ra);
Vector3.Cross(ref entityA.angularVelocity, ref ra, out velocityA);
Vector3.Add(ref velocityA, ref entityA.linearVelocity, out velocityA);
}
else
{
velocityA = new Vector3();
}
if (entityB != null)
{
Vector3.Subtract(ref manifoldCenter, ref entityB.position, out rb);
Vector3.Cross(ref entityB.angularVelocity, ref rb, out velocityB);
Vector3.Add(ref velocityB, ref entityB.linearVelocity, out velocityB);
}
else
{
velocityB = new Vector3();
}
Vector3.Subtract(ref velocityA, ref velocityB, out relativeVelocity);
//Get rid of the normal velocity.
Vector3 normal = contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Normal;
float normalVelocityScalar = normal.X * relativeVelocity.X + normal.Y * relativeVelocity.Y + normal.Z * relativeVelocity.Z;
relativeVelocity.X -= normalVelocityScalar * normal.X;
relativeVelocity.Y -= normalVelocityScalar * normal.Y;
relativeVelocity.Z -= normalVelocityScalar * normal.Z;
//Create the jacobian entry and decide the friction coefficient.
float length = relativeVelocity.LengthSquared();
if (length > Toolbox.Epsilon)
{
length = (float)Math.Sqrt(length);
linearA.M11 = relativeVelocity.X / length;
linearA.M12 = relativeVelocity.Y / length;
linearA.M13 = relativeVelocity.Z / length;
friction = length > CollisionResponseSettings.StaticFrictionVelocityThreshold ?
contactManifoldConstraint.materialInteraction.KineticFriction :
contactManifoldConstraint.materialInteraction.StaticFriction;
}
else
{
friction = contactManifoldConstraint.materialInteraction.StaticFriction;
//If there was no velocity, try using the previous frame's jacobian... if it exists.
//Reusing an old one is okay since jacobians are cleared when a contact is initialized.
if (!(linearA.M11 != 0 || linearA.M12 != 0 || linearA.M13 != 0))
{
//Otherwise, just redo it all.
//Create arbitrary axes.
Vector3 axis1;
Vector3.Cross(ref normal, ref Toolbox.RightVector, out axis1);
length = axis1.LengthSquared();
if (length > Toolbox.Epsilon)
{
length = (float)Math.Sqrt(length);
linearA.M11 = axis1.X / length;
linearA.M12 = axis1.Y / length;
linearA.M13 = axis1.Z / length;
}
else
{
Vector3.Cross(ref normal, ref Toolbox.UpVector, out axis1);
axis1.Normalize();
linearA.M11 = axis1.X;
linearA.M12 = axis1.Y;
linearA.M13 = axis1.Z;
}
}
}
//Second axis is first axis x normal
linearA.M21 = (linearA.M12 * normal.Z) - (linearA.M13 * normal.Y);
linearA.M22 = (linearA.M13 * normal.X) - (linearA.M11 * normal.Z);
linearA.M23 = (linearA.M11 * normal.Y) - (linearA.M12 * normal.X);
//Compute angular jacobians
if (entityA != null)
{
//angularA 1 = ra x linear axis 1
angularA.M11 = (ra.Y * linearA.M13) - (ra.Z * linearA.M12);
angularA.M12 = (ra.Z * linearA.M11) - (ra.X * linearA.M13);
angularA.M13 = (ra.X * linearA.M12) - (ra.Y * linearA.M11);
//angularA 2 = ra x linear axis 2
angularA.M21 = (ra.Y * linearA.M23) - (ra.Z * linearA.M22);
angularA.M22 = (ra.Z * linearA.M21) - (ra.X * linearA.M23);
angularA.M23 = (ra.X * linearA.M22) - (ra.Y * linearA.M21);
}
//angularB 1 = linear axis 1 x rb
if (entityB != null)
{
angularB.M11 = (linearA.M12 * rb.Z) - (linearA.M13 * rb.Y);
angularB.M12 = (linearA.M13 * rb.X) - (linearA.M11 * rb.Z);
angularB.M13 = (linearA.M11 * rb.Y) - (linearA.M12 * rb.X);
//angularB 2 = linear axis 2 x rb
angularB.M21 = (linearA.M22 * rb.Z) - (linearA.M23 * rb.Y);
angularB.M22 = (linearA.M23 * rb.X) - (linearA.M21 * rb.Z);
angularB.M23 = (linearA.M21 * rb.Y) - (linearA.M22 * rb.X);
}
//Compute inverse effective mass matrix
Matrix2X2 entryA, entryB;
//these are the transformed coordinates
Matrix2X3 transform;
Matrix3X2 transpose;
if (entityADynamic)
{
Matrix2X3.Multiply(ref angularA, ref entityA.inertiaTensorInverse, out transform);
Matrix2X3.Transpose(ref angularA, out transpose);
Matrix2X2.Multiply(ref transform, ref transpose, out entryA);
entryA.M11 += entityA.inverseMass;
entryA.M22 += entityA.inverseMass;
}
else
{
entryA = new Matrix2X2();
}
if (entityBDynamic)
{
Matrix2X3.Multiply(ref angularB, ref entityB.inertiaTensorInverse, out transform);
Matrix2X3.Transpose(ref angularB, out transpose);
Matrix2X2.Multiply(ref transform, ref transpose, out entryB);
entryB.M11 += entityB.inverseMass;
entryB.M22 += entityB.inverseMass;
}
else
{
entryB = new Matrix2X2();
}
velocityToImpulse.M11 = -entryA.M11 - entryB.M11;
velocityToImpulse.M12 = -entryA.M12 - entryB.M12;
velocityToImpulse.M21 = -entryA.M21 - entryB.M21;
velocityToImpulse.M22 = -entryA.M22 - entryB.M22;
Matrix2X2.Invert(ref velocityToImpulse, out velocityToImpulse);
}
