public void SolveIteration()
{
Vector3Width4 linearVelocityA = new Vector3Width4(ref VelocitiesA0.LinearVelocity, ref VelocitiesA1.LinearVelocity, ref VelocitiesA2.LinearVelocity, ref VelocitiesA3.LinearVelocity);
Vector3Width4 angularVelocityA = new Vector3Width4(ref VelocitiesA0.AngularVelocity, ref VelocitiesA1.AngularVelocity, ref VelocitiesA2.AngularVelocity, ref VelocitiesA3.AngularVelocity);
Vector3Width4 linearVelocityB = new Vector3Width4(ref VelocitiesB0.LinearVelocity, ref VelocitiesB1.LinearVelocity, ref VelocitiesB2.LinearVelocity, ref VelocitiesB3.LinearVelocity);
Vector3Width4 angularVelocityB = new Vector3Width4(ref VelocitiesB0.AngularVelocity, ref VelocitiesB1.AngularVelocity, ref VelocitiesB2.AngularVelocity, ref VelocitiesB3.AngularVelocity);
for (int i = 0; i < ConstraintCount; ++i)
{
Constraints[i].SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
}
//for (int i = 0; i < 4; ++i)
//{
// a.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
//}
//a.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
//b.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
//c.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
//d.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
Vector3Width4.Transpose(ref linearVelocityA, out VelocitiesA0.LinearVelocity, out VelocitiesA1.LinearVelocity, out VelocitiesA2.LinearVelocity, out VelocitiesA3.LinearVelocity);
Vector3Width4.Transpose(ref linearVelocityB, out VelocitiesB0.LinearVelocity, out VelocitiesB1.LinearVelocity, out VelocitiesB2.LinearVelocity, out VelocitiesB3.LinearVelocity);
Vector3Width4.Transpose(ref angularVelocityA, out VelocitiesA0.AngularVelocity, out VelocitiesA1.AngularVelocity, out VelocitiesA2.AngularVelocity, out VelocitiesA3.AngularVelocity);
Vector3Width4.Transpose(ref angularVelocityB, out VelocitiesB0.AngularVelocity, out VelocitiesB1.AngularVelocity, out VelocitiesB2.AngularVelocity, out VelocitiesB3.AngularVelocity);
}
internal static unsafe void Transpose(ref Vector3Width4 source, Vector3 *result)
{
result[0] = new Vector3(source.X.X, source.Y.X, source.Z.X);
result[1] = new Vector3(source.X.Y, source.Y.Y, source.Z.Y);
result[2] = new Vector3(source.X.Z, source.Y.Z, source.Z.Z);
result[3] = new Vector3(source.X.W, source.Y.W, source.Z.W);
}
void ApplyImpulse(float lambda, ref Vector3Width4 velocities)
{
Vector3Width4 change;
Vector3Width4.Multiply(ref JacobiansIT, lambda, out change);
Vector3Width4.Subtract(ref velocities, ref change, out velocities);
}
public void SolveIteration()
{
Vector3Width4 linearVelocityA = new Vector3Width4(ref VelocitiesA0.LinearVelocity, ref VelocitiesA1.LinearVelocity, ref VelocitiesA2.LinearVelocity, ref VelocitiesA3.LinearVelocity);
Vector3Width4 angularVelocityA = new Vector3Width4(ref VelocitiesA0.AngularVelocity, ref VelocitiesA1.AngularVelocity, ref VelocitiesA2.AngularVelocity, ref VelocitiesA3.AngularVelocity);
Vector3Width4 linearVelocityB = new Vector3Width4(ref VelocitiesB0.LinearVelocity, ref VelocitiesB1.LinearVelocity, ref VelocitiesB2.LinearVelocity, ref VelocitiesB3.LinearVelocity);
Vector3Width4 angularVelocityB = new Vector3Width4(ref VelocitiesB0.AngularVelocity, ref VelocitiesB1.AngularVelocity, ref VelocitiesB2.AngularVelocity, ref VelocitiesB3.AngularVelocity);
Vector4 linearA, angularA, linearB, angularB;
Vector3Width4.Dot(ref LinearJacobianA, ref linearVelocityA, out linearA);
Vector3Width4.Dot(ref AngularJacobianA, ref angularVelocityA, out angularA);
Vector3Width4.Dot(ref LinearJacobianB, ref linearVelocityB, out linearB);
Vector3Width4.Dot(ref AngularJacobianB, ref angularVelocityB, out angularB);
var lambda = EffectiveMass * (linearA + angularA + linearB + angularB + PenetrationBias - AccumulatedImpulse * Softness);
var previous = AccumulatedImpulse;
AccumulatedImpulse = Vector4.Max(Vector4.Zero, AccumulatedImpulse + lambda);
lambda = AccumulatedImpulse - previous;
ApplyImpulse(ref lambda, ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
Vector3Width4.Transpose(ref linearVelocityA, out VelocitiesA0.LinearVelocity, out VelocitiesA1.LinearVelocity, out VelocitiesA2.LinearVelocity, out VelocitiesA3.LinearVelocity);
Vector3Width4.Transpose(ref linearVelocityB, out VelocitiesB0.LinearVelocity, out VelocitiesB1.LinearVelocity, out VelocitiesB2.LinearVelocity, out VelocitiesB3.LinearVelocity);
Vector3Width4.Transpose(ref angularVelocityA, out VelocitiesA0.AngularVelocity, out VelocitiesA1.AngularVelocity, out VelocitiesA2.AngularVelocity, out VelocitiesA3.AngularVelocity);
Vector3Width4.Transpose(ref angularVelocityB, out VelocitiesB0.AngularVelocity, out VelocitiesB1.AngularVelocity, out VelocitiesB2.AngularVelocity, out VelocitiesB3.AngularVelocity);
}
internal static unsafe void Transpose(ref Vector3Width4 source, out Vector3 a, out Vector3 b, out Vector3 c, out Vector3 d)
{
a = new Vector3(source.X.X, source.Y.X, source.Z.X);
b = new Vector3(source.X.Y, source.Y.Y, source.Z.Y);
c = new Vector3(source.X.Z, source.Y.Z, source.Z.Z);
d = new Vector3(source.X.W, source.Y.W, source.Z.W);
}
public void Prestep(float inverseDt)
{
//Given that we're collecting position, inverse mass, and inertia all at once, it makes no sense to store position separately from inversemass and inertia.
//Since you should not expect the 4 involved bodies to be in memory *together*, the best you can do is to ensure that the set of values are together.
//Otherwise you're multiplying cache misses for no reason!
var inverseMassA = new Vector4(BodyA0.InverseMass, BodyA1.InverseMass, BodyA2.InverseMass, BodyA3.InverseMass);
var inverseMassB = new Vector4(BodyB0.InverseMass, BodyB1.InverseMass, BodyB2.InverseMass, BodyB3.InverseMass);
var inverseInertiaTensorA = new Matrix3x3Width4(ref BodyA0.InertiaTensorInverse, ref BodyA1.InertiaTensorInverse, ref BodyA2.InertiaTensorInverse, ref BodyA3.InertiaTensorInverse);
var inverseInertiaTensorB = new Matrix3x3Width4(ref BodyB0.InertiaTensorInverse, ref BodyB1.InertiaTensorInverse, ref BodyB2.InertiaTensorInverse, ref BodyB3.InertiaTensorInverse);
Vector3Width4 positionA = new Vector3Width4(ref BodyA0.Position, ref BodyA1.Position, ref BodyA2.Position, ref BodyA3.Position);
Vector3Width4 positionB = new Vector3Width4(ref BodyA0.Position, ref BodyB1.Position, ref BodyB2.Position, ref BodyB3.Position);
for (int i = 0; i < ConstraintCount; ++i)
{
Constraints[i].Prestep(inverseDt, ref inverseMassA, ref inverseMassB, ref inverseInertiaTensorA, ref inverseInertiaTensorB, ref positionA, ref positionB);
}
//for (int i = 0; i < 4; ++i)
//{
// a.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
//}
//a.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
//b.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
//c.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
//d.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
}
public void SolveIteration()
{
Vector3Width4 linearVelocityA = new Vector3Width4(ref VelocitiesA0.LinearVelocity, ref VelocitiesA1.LinearVelocity, ref VelocitiesA2.LinearVelocity, ref VelocitiesA3.LinearVelocity);
Vector3Width4 angularVelocityA = new Vector3Width4(ref VelocitiesA0.AngularVelocity, ref VelocitiesA1.AngularVelocity, ref VelocitiesA2.AngularVelocity, ref VelocitiesA3.AngularVelocity);
Vector3Width4 linearVelocityB = new Vector3Width4(ref VelocitiesB0.LinearVelocity, ref VelocitiesB1.LinearVelocity, ref VelocitiesB2.LinearVelocity, ref VelocitiesB3.LinearVelocity);
Vector3Width4 angularVelocityB = new Vector3Width4(ref VelocitiesB0.AngularVelocity, ref VelocitiesB1.AngularVelocity, ref VelocitiesB2.AngularVelocity, ref VelocitiesB3.AngularVelocity);
for (int i = 0; i < ConstraintCount; ++i)
{
Constraints[i].SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
}
//for (int i = 0; i < 4; ++i)
//{
// a.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
//}
//a.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
//b.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
//c.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
//d.SolveIteration(ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
Vector3Width4.Transpose(ref linearVelocityA, out VelocitiesA0.LinearVelocity, out VelocitiesA1.LinearVelocity, out VelocitiesA2.LinearVelocity, out VelocitiesA3.LinearVelocity);
Vector3Width4.Transpose(ref linearVelocityB, out VelocitiesB0.LinearVelocity, out VelocitiesB1.LinearVelocity, out VelocitiesB2.LinearVelocity, out VelocitiesB3.LinearVelocity);
Vector3Width4.Transpose(ref angularVelocityA, out VelocitiesA0.AngularVelocity, out VelocitiesA1.AngularVelocity, out VelocitiesA2.AngularVelocity, out VelocitiesA3.AngularVelocity);
Vector3Width4.Transpose(ref angularVelocityB, out VelocitiesB0.AngularVelocity, out VelocitiesB1.AngularVelocity, out VelocitiesB2.AngularVelocity, out VelocitiesB3.AngularVelocity);
}
public void Prestep(float inverseDt)
{
//Given that we're collecting position, inverse mass, and inertia all at once, it makes no sense to store position separately from inversemass and inertia.
//Since you should not expect the 4 involved bodies to be in memory *together*, the best you can do is to ensure that the set of values are together.
//Otherwise you're multiplying cache misses for no reason!
var inverseMassA = new Vector4(BodyA0.InverseMass, BodyA1.InverseMass, BodyA2.InverseMass, BodyA3.InverseMass);
var inverseMassB = new Vector4(BodyB0.InverseMass, BodyB1.InverseMass, BodyB2.InverseMass, BodyB3.InverseMass);
var inverseInertiaTensorA = new Matrix3x3Width4(ref BodyA0.InertiaTensorInverse, ref BodyA1.InertiaTensorInverse, ref BodyA2.InertiaTensorInverse, ref BodyA3.InertiaTensorInverse);
var inverseInertiaTensorB = new Matrix3x3Width4(ref BodyB0.InertiaTensorInverse, ref BodyB1.InertiaTensorInverse, ref BodyB2.InertiaTensorInverse, ref BodyB3.InertiaTensorInverse);
Vector3Width4 positionA = new Vector3Width4(ref BodyA0.Position, ref BodyA1.Position, ref BodyA2.Position, ref BodyA3.Position);
Vector3Width4 positionB = new Vector3Width4(ref BodyA0.Position, ref BodyB1.Position, ref BodyB2.Position, ref BodyB3.Position);
for (int i = 0; i < ConstraintCount; ++i)
{
Constraints[i].Prestep(inverseDt, ref inverseMassA, ref inverseMassB, ref inverseInertiaTensorA, ref inverseInertiaTensorB, ref positionA, ref positionB);
}
//for (int i = 0; i < 4; ++i)
//{
// a.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
//}
//a.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
//b.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
//c.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
//d.Prestep(inverseDt, ref InverseMassA, ref InverseMassB, ref InverseInertiaTensorA, ref InverseInertiaTensorB, ref positionA, ref positionB);
}
public void Prestep(float inverseDt)
{
//C = dot(Pa - Pb, N) > 0
//Jacobians:
//LinearA: N
//AngularA: cross(OffsetPa, N)
//LinearB: -N
//AngularB: -cross(OffsetPb, N)
//var positionA = new Vector3Width4();
//var positionB = new Vector3Width4();
//Given that we're collecting position, inverse mass, and inertia all at once, it makes no sense to store position separately from inversemass and inertia.
//Since you should not expect the 4 involved bodies to be in memory *together*, the best you can do is to ensure that the set of values are together.
//Otherwise you're multiplying cache misses for no reason!
var InverseMassA = new Vector4(BodyA0.InverseMass, BodyA1.InverseMass, BodyA2.InverseMass, BodyA3.InverseMass);
var InverseMassB = new Vector4(BodyB0.InverseMass, BodyB1.InverseMass, BodyB2.InverseMass, BodyB3.InverseMass);
var InverseInertiaTensorA = new Matrix3x3Width4(ref BodyA0.InertiaTensorInverse, ref BodyA1.InertiaTensorInverse, ref BodyA2.InertiaTensorInverse, ref BodyA3.InertiaTensorInverse);
var InverseInertiaTensorB = new Matrix3x3Width4(ref BodyB0.InertiaTensorInverse, ref BodyB1.InertiaTensorInverse, ref BodyB2.InertiaTensorInverse, ref BodyB3.InertiaTensorInverse);
Vector3Width4 positionA = new Vector3Width4(ref BodyA0.Position, ref BodyA1.Position, ref BodyA2.Position, ref BodyA3.Position);
Vector3Width4 positionB = new Vector3Width4(ref BodyA0.Position, ref BodyB1.Position, ref BodyB2.Position, ref BodyB3.Position);
LinearJacobianA = ContactNormal;
Vector3Width4.Negate(ref ContactNormal, out LinearJacobianB);
Vector3Width4 offsetA, offsetB;
Vector3Width4.Subtract(ref ContactPosition, ref positionA, out offsetA);
Vector3Width4.Subtract(ref ContactPosition, ref positionB, out offsetB);
Vector3Width4.Cross(ref offsetA, ref ContactNormal, out AngularJacobianA);
Vector3Width4.Cross(ref ContactNormal, ref offsetB, out AngularJacobianB);// note negation->parameter reverse
//Allow velocity that closes a gap, and apply penetration correction against positive depth.
//Bounciness not yet included.
PenetrationBias = ContactPenetration * inverseDt;
PenetrationBias = -Vector4.Min(Vector4.Min(PenetrationBias, PenetrationBias * 0.2f), new Vector4(0.2f));
//The inertia tensor is in world space, so no jacobian transformation is required.
Vector3Width4.Multiply(ref LinearJacobianA, ref InverseMassA, out LinearJacobianITA);
Vector3Width4.Multiply(ref LinearJacobianB, ref InverseMassB, out LinearJacobianITB);
Matrix3x3Width4.Transform(ref AngularJacobianA, ref InverseInertiaTensorA, out AngularJacobianITA);
Matrix3x3Width4.Transform(ref AngularJacobianB, ref InverseInertiaTensorB, out AngularJacobianITB);
Vector4 angularContributionA, angularContributionB;
Vector3Width4.Dot(ref AngularJacobianITA, ref AngularJacobianITA, out angularContributionA);
Vector3Width4.Dot(ref AngularJacobianITB, ref AngularJacobianITB, out angularContributionB);
var inverseEffectiveMass = InverseMassA + InverseMassB + angularContributionA + angularContributionB;
Vector4 CollisionSoftness = new Vector4(5);
Softness = CollisionSoftness * inverseEffectiveMass * inverseDt;
EffectiveMass = Vector4.One / (Softness + inverseEffectiveMass);
}
public void Prestep(float inverseDt,
ref Vector4 inverseMassA, ref Vector4 inverseMassB,
ref Matrix3x3Width4 inverseInertiaTensorA, ref Matrix3x3Width4 inverseInertiaTensorB,
ref Vector3Width4 positionA, ref Vector3Width4 positionB)
{
//C = dot(Pa - Pb, N) > 0
//Jacobians:
//LinearA: N
//AngularA: cross(OffsetPa, N)
//LinearB: -N
//AngularB: -cross(OffsetPb, N)
//var positionA = new Vector3Width4();
//var positionB = new Vector3Width4();
LinearJacobianA = ContactNormal;
Vector3Width4.Negate(ref ContactNormal, out LinearJacobianB);
Vector3Width4 offsetA, offsetB;
Vector3Width4.Subtract(ref ContactPosition, ref positionA, out offsetA);
Vector3Width4.Subtract(ref ContactPosition, ref positionB, out offsetB);
Vector3Width4.Cross(ref offsetA, ref ContactNormal, out AngularJacobianA);
Vector3Width4.Cross(ref ContactNormal, ref offsetB, out AngularJacobianB);// note negation->parameter reverse
//Allow velocity that closes a gap, and apply penetration correction against positive depth.
//Bounciness not yet included.
PenetrationBias = ContactPenetration * inverseDt;
PenetrationBias = -Vector4.Min(Vector4.Min(PenetrationBias, PenetrationBias * 0.2f), new Vector4(0.2f));
//The inertia tensor is in world space, so no jacobian transformation is required.
Vector3Width4.Multiply(ref LinearJacobianA, ref inverseMassA, out LinearJacobianITA);
Vector3Width4.Multiply(ref LinearJacobianB, ref inverseMassB, out LinearJacobianITB);
Matrix3x3Width4.Transform(ref AngularJacobianA, ref inverseInertiaTensorA, out AngularJacobianITA);
Matrix3x3Width4.Transform(ref AngularJacobianB, ref inverseInertiaTensorB, out AngularJacobianITB);
Vector4 angularContributionA, angularContributionB;
Vector3Width4.Dot(ref AngularJacobianITA, ref AngularJacobianITA, out angularContributionA);
Vector3Width4.Dot(ref AngularJacobianITB, ref AngularJacobianITB, out angularContributionB);
var inverseEffectiveMass = inverseMassA + inverseMassB + angularContributionA + angularContributionB;
Vector4 CollisionSoftness = new Vector4(5);
Softness = CollisionSoftness * inverseEffectiveMass * inverseDt;
EffectiveMass = Vector4.One / (Softness + inverseEffectiveMass);
}
void ApplyImpulse(ref Vector4 lambda)
{
Vector3Width4 linearChangeA, linearChangeB;
Vector3Width4.Multiply(ref LinearJacobianITA, ref lambda, out linearChangeA);
Vector3Width4.Multiply(ref LinearJacobianITB, ref lambda, out linearChangeB);
Vector3 linearChangeA0, linearChangeA1, linearChangeA2, linearChangeA3;
Vector3 linearChangeB0, linearChangeB1, linearChangeB2, linearChangeB3;
Vector3Width4.Transpose(ref linearChangeA, out linearChangeA0, out linearChangeA1, out linearChangeA2, out linearChangeA3);
Vector3Width4.Transpose(ref linearChangeB, out linearChangeB0, out linearChangeB1, out linearChangeB2, out linearChangeB3);
//World inertia available, so no need for extra transforms.
Vector3Width4 angularChangeA, angularChangeB;
Vector3Width4.Multiply(ref AngularJacobianITA, ref lambda, out angularChangeA);
Vector3Width4.Multiply(ref AngularJacobianITB, ref lambda, out angularChangeB);
Vector3 angularChangeA0, angularChangeA1, angularChangeA2, angularChangeA3;
Vector3 angularChangeB0, angularChangeB1, angularChangeB2, angularChangeB3;
Vector3Width4.Transpose(ref angularChangeA, out angularChangeA0, out angularChangeA1, out angularChangeA2, out angularChangeA3);
Vector3Width4.Transpose(ref angularChangeB, out angularChangeB0, out angularChangeB1, out angularChangeB2, out angularChangeB3);
VelocitiesA0.LinearVelocity -= linearChangeA0;
VelocitiesA0.AngularVelocity -= angularChangeA0;
VelocitiesA1.LinearVelocity -= linearChangeA1;
VelocitiesA1.AngularVelocity -= angularChangeA1;
VelocitiesA2.LinearVelocity -= linearChangeA2;
VelocitiesA2.AngularVelocity -= angularChangeA2;
VelocitiesA3.LinearVelocity -= linearChangeA3;
VelocitiesA3.AngularVelocity -= angularChangeA3;
VelocitiesB0.LinearVelocity -= linearChangeB0;
VelocitiesB0.AngularVelocity -= angularChangeB0;
VelocitiesB1.LinearVelocity -= linearChangeB1;
VelocitiesB1.AngularVelocity -= angularChangeB1;
VelocitiesB2.LinearVelocity -= linearChangeB2;
VelocitiesB2.AngularVelocity -= angularChangeB2;
VelocitiesB3.LinearVelocity -= linearChangeB3;
VelocitiesB3.AngularVelocity -= angularChangeB3;
}
public void SolveIteration()
{
Vector3Width4 velocities = new Vector3Width4(ref ConnectionA.LinearVelocity, ref ConnectionA.AngularVelocity, ref ConnectionB.LinearVelocity, ref ConnectionB.AngularVelocity);
Vector4 velocityContributions;
Vector3Width4.Dot(ref velocities, ref Jacobians, out velocityContributions);
var lambda = EffectiveMass * (
velocityContributions.X + velocityContributions.Y + velocityContributions.Z + velocityContributions.W +
PenetrationBias - AccumulatedImpulse * Softness);
var previous = AccumulatedImpulse;
AccumulatedImpulse = Math.Max(0, AccumulatedImpulse + lambda);
lambda = AccumulatedImpulse - previous;
ApplyImpulse(lambda, ref velocities);
Vector3Width4.Transpose(ref velocities, out ConnectionA.LinearVelocity, out ConnectionA.AngularVelocity, out ConnectionB.LinearVelocity, out ConnectionB.AngularVelocity);
}
public void SolveIteration()
{
Vector3Width4 velocities = new Vector3Width4(ref ConnectionA.LinearVelocity, ref ConnectionA.AngularVelocity, ref ConnectionB.LinearVelocity, ref ConnectionB.AngularVelocity);
Vector4 velocityContributions;
Vector3Width4.Dot(ref velocities, ref Jacobians, out velocityContributions);
var lambda = EffectiveMass * (
velocityContributions.X + velocityContributions.Y + velocityContributions.Z + velocityContributions.W +
PenetrationBias - AccumulatedImpulse * Softness);
var previous = AccumulatedImpulse;
AccumulatedImpulse = Math.Max(0, AccumulatedImpulse + lambda);
lambda = AccumulatedImpulse - previous;
ApplyImpulse(lambda, ref velocities);
Vector3Width4.Transpose(ref velocities, out ConnectionA.LinearVelocity, out ConnectionA.AngularVelocity, out ConnectionB.LinearVelocity, out ConnectionB.AngularVelocity);
}
void ApplyImpulse(ref Vector4 lambda, ref Vector3Width4 linearVelocityA, ref Vector3Width4 angularVelocityA, ref Vector3Width4 linearVelocityB, ref Vector3Width4 angularVelocityB)
{
Vector3Width4 linearChangeA, linearChangeB;
Vector3Width4.Multiply(ref LinearJacobianITA, ref lambda, out linearChangeA);
Vector3Width4.Multiply(ref LinearJacobianITB, ref lambda, out linearChangeB);
Vector3Width4.Add(ref linearVelocityA, ref linearChangeA, out linearVelocityA);
Vector3Width4.Add(ref linearVelocityB, ref linearChangeB, out linearVelocityB);
//World inertia available, so no need for extra transforms.
Vector3Width4 angularChangeA, angularChangeB;
Vector3Width4.Multiply(ref AngularJacobianITA, ref lambda, out angularChangeA);
Vector3Width4.Multiply(ref AngularJacobianITB, ref lambda, out angularChangeB);
Vector3Width4.Add(ref angularVelocityA, ref angularChangeA, out angularVelocityA);
Vector3Width4.Add(ref angularVelocityB, ref angularChangeB, out angularVelocityB);
}
public unsafe static void Test()
{
var identityMatrix = new Matrix3x3 {
X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 0, 1)
};
var a = new RigidBody
{
Position = new Vector3(0, 0, 0),
Orientation = identityMatrix,
InertiaTensorInverse = identityMatrix,
InverseMass = 1,
LinearVelocity = new Vector3(0, 0, 0)
};
var b = new RigidBody
{
Position = new Vector3(0, 1, 0),
Orientation = identityMatrix,
InertiaTensorInverse = identityMatrix,
InverseMass = 1,
LinearVelocity = new Vector3(0, 0, 0)
};
RigidBody *aBodies = stackalloc RigidBody[4];
RigidBody *bBodies = stackalloc RigidBody[4];
for (int i = 0; i < 4; ++i)
{
aBodies[i] = a;
bBodies[i] = b;
}
Vector3 up = Vector3.UnitY;
var contactPosition = new Vector3Width4();
var contactNormal = new Vector3Width4(ref up, ref up, ref up, ref up);
var contactPenetration = new Vector4();
var constraint = new VectorizedManifoldConstraint
{
BodyA0 = new BodyState {
InertiaTensorInverse = new Matrix3x3 {
X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0)
}, InverseMass = 1, Position = new Vector3()
},
BodyA1 = new BodyState {
InertiaTensorInverse = new Matrix3x3 {
X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0)
}, InverseMass = 1, Position = new Vector3()
},
BodyA2 = new BodyState {
InertiaTensorInverse = new Matrix3x3 {
X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0)
}, InverseMass = 1, Position = new Vector3()
},
BodyA3 = new BodyState {
InertiaTensorInverse = new Matrix3x3 {
X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0)
}, InverseMass = 1, Position = new Vector3()
},
BodyB0 = new BodyState {
InertiaTensorInverse = new Matrix3x3 {
X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0)
}, InverseMass = 1, Position = new Vector3(0, 1, 0)
},
BodyB1 = new BodyState {
InertiaTensorInverse = new Matrix3x3 {
X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0)
}, InverseMass = 1, Position = new Vector3(0, 1, 0)
},
BodyB2 = new BodyState {
InertiaTensorInverse = new Matrix3x3 {
X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0)
}, InverseMass = 1, Position = new Vector3(0, 1, 0)
},
BodyB3 = new BodyState {
InertiaTensorInverse = new Matrix3x3 {
X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0)
}, InverseMass = 1, Position = new Vector3(0, 1, 0)
},
VelocitiesB0 = new Velocities {
LinearVelocity = new Vector3(0, -1, 0)
},
VelocitiesB1 = new Velocities {
LinearVelocity = new Vector3(0, -1, 0)
},
VelocitiesB2 = new Velocities {
LinearVelocity = new Vector3(0, -1, 0)
},
VelocitiesB3 = new Velocities {
LinearVelocity = new Vector3(0, -1, 0)
},
//a = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration },
//b = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration },
//c = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration },
//d = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration }
};
constraint.ConstraintCount = 4;
var pointer = stackalloc VectorizedManifoldPenetrationConstraint[constraint.ConstraintCount];
constraint.Constraints = pointer;
//constraint.Constraints = new VectorizedManifoldPenetrationConstraint[constraintCount];
for (int i = 0; i < constraint.ConstraintCount; ++i)
{
constraint.Constraints[i] = new VectorizedManifoldPenetrationConstraint {
ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration
};
}
float dt = 1 / 60f;
float inverseDt = 1 / dt;
constraint.Prestep(inverseDt);
constraint.WarmStart();
constraint.SolveIteration();
var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < VectorizedConstraintTest.TestCount / constraint.ConstraintCount; ++i)
{
constraint.Prestep(inverseDt);
constraint.WarmStart();
for (int iterationIndex = 0; iterationIndex < VectorizedConstraintTest.IterationCount; ++iterationIndex)
{
constraint.SolveIteration();
}
}
var endtime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
Console.WriteLine($"Vectorized Manifold: {endtime - startTime}");
}
public static void Multiply(ref Vector3Width4 a, float b, out Vector3Width4 result)
{
result.X = a.X * b;
result.Y = a.Y * b;
result.Z = a.Z * b;
}
public static void Cross(ref Vector3Width4 a, ref Vector3Width4 b, out Vector3Width4 result)
{
result.X = a.Y * b.Z - a.Z * b.Y;
result.Y = a.Z * b.X - a.X * b.Z;
result.Z = a.X * b.Y - a.Y * b.X;
}
public void WarmStart(ref Vector3Width4 linearVelocityA, ref Vector3Width4 angularVelocityA, ref Vector3Width4 linearVelocityB, ref Vector3Width4 angularVelocityB)
{
ApplyImpulse(ref AccumulatedImpulse, ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
}
public static void Add(ref Vector3Width4 a, ref Vector3Width4 b, out Vector3Width4 result)
{
result.X = a.X + b.X;
result.Y = a.Y + b.Y;
result.Z = a.Z + b.Z;
}
public void WarmStart(ref Vector3Width4 linearVelocityA, ref Vector3Width4 angularVelocityA, ref Vector3Width4 linearVelocityB, ref Vector3Width4 angularVelocityB)
{
ApplyImpulse(ref AccumulatedImpulse, ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
}
public void Prestep(float inverseDt,
ref Vector4 inverseMassA, ref Vector4 inverseMassB,
ref Matrix3x3Width4 inverseInertiaTensorA, ref Matrix3x3Width4 inverseInertiaTensorB,
ref Vector3Width4 positionA, ref Vector3Width4 positionB)
{
//C = dot(Pa - Pb, N) > 0
//Jacobians:
//LinearA: N
//AngularA: cross(OffsetPa, N)
//LinearB: -N
//AngularB: -cross(OffsetPb, N)
//var positionA = new Vector3Width4();
//var positionB = new Vector3Width4();
LinearJacobianA = ContactNormal;
Vector3Width4.Negate(ref ContactNormal, out LinearJacobianB);
Vector3Width4 offsetA, offsetB;
Vector3Width4.Subtract(ref ContactPosition, ref positionA, out offsetA);
Vector3Width4.Subtract(ref ContactPosition, ref positionB, out offsetB);
Vector3Width4.Cross(ref offsetA, ref ContactNormal, out AngularJacobianA);
Vector3Width4.Cross(ref ContactNormal, ref offsetB, out AngularJacobianB);// note negation->parameter reverse
//Allow velocity that closes a gap, and apply penetration correction against positive depth.
//Bounciness not yet included.
PenetrationBias = ContactPenetration * inverseDt;
PenetrationBias = -Vector4.Min(Vector4.Min(PenetrationBias, PenetrationBias * 0.2f), new Vector4(0.2f));
//The inertia tensor is in world space, so no jacobian transformation is required.
Vector3Width4.Multiply(ref LinearJacobianA, ref inverseMassA, out LinearJacobianITA);
Vector3Width4.Multiply(ref LinearJacobianB, ref inverseMassB, out LinearJacobianITB);
Matrix3x3Width4.Transform(ref AngularJacobianA, ref inverseInertiaTensorA, out AngularJacobianITA);
Matrix3x3Width4.Transform(ref AngularJacobianB, ref inverseInertiaTensorB, out AngularJacobianITB);
Vector4 angularContributionA, angularContributionB;
Vector3Width4.Dot(ref AngularJacobianITA, ref AngularJacobianITA, out angularContributionA);
Vector3Width4.Dot(ref AngularJacobianITB, ref AngularJacobianITB, out angularContributionB);
var inverseEffectiveMass = inverseMassA + inverseMassB + angularContributionA + angularContributionB;
Vector4 CollisionSoftness = new Vector4(5);
Softness = CollisionSoftness * inverseEffectiveMass * inverseDt;
EffectiveMass = Vector4.One / (Softness + inverseEffectiveMass);
}
public void Prestep(float inverseDt)
{
//C = dot(Pa - Pb, N) > 0
//Jacobians:
//LinearA: N
//AngularA: cross(OffsetPa, N)
//LinearB: -N
//AngularB: -cross(OffsetPb, N)
//var positionA = new Vector3Width4();
//var positionB = new Vector3Width4();
//Given that we're collecting position, inverse mass, and inertia all at once, it makes no sense to store position separately from inversemass and inertia.
//Since you should not expect the 4 involved bodies to be in memory *together*, the best you can do is to ensure that the set of values are together.
//Otherwise you're multiplying cache misses for no reason!
var InverseMassA = new Vector4(BodyA0.InverseMass, BodyA1.InverseMass, BodyA2.InverseMass, BodyA3.InverseMass);
var InverseMassB = new Vector4(BodyB0.InverseMass, BodyB1.InverseMass, BodyB2.InverseMass, BodyB3.InverseMass);
var InverseInertiaTensorA = new Matrix3x3Width4(ref BodyA0.InertiaTensorInverse, ref BodyA1.InertiaTensorInverse, ref BodyA2.InertiaTensorInverse, ref BodyA3.InertiaTensorInverse);
var InverseInertiaTensorB = new Matrix3x3Width4(ref BodyB0.InertiaTensorInverse, ref BodyB1.InertiaTensorInverse, ref BodyB2.InertiaTensorInverse, ref BodyB3.InertiaTensorInverse);
Vector3Width4 positionA = new Vector3Width4(ref BodyA0.Position, ref BodyA1.Position, ref BodyA2.Position, ref BodyA3.Position);
Vector3Width4 positionB = new Vector3Width4(ref BodyA0.Position, ref BodyB1.Position, ref BodyB2.Position, ref BodyB3.Position);
LinearJacobianA = ContactNormal;
Vector3Width4.Negate(ref ContactNormal, out LinearJacobianB);
Vector3Width4 offsetA, offsetB;
Vector3Width4.Subtract(ref ContactPosition, ref positionA, out offsetA);
Vector3Width4.Subtract(ref ContactPosition, ref positionB, out offsetB);
Vector3Width4.Cross(ref offsetA, ref ContactNormal, out AngularJacobianA);
Vector3Width4.Cross(ref ContactNormal, ref offsetB, out AngularJacobianB);// note negation->parameter reverse
//Allow velocity that closes a gap, and apply penetration correction against positive depth.
//Bounciness not yet included.
PenetrationBias = ContactPenetration * inverseDt;
PenetrationBias = -Vector4.Min(Vector4.Min(PenetrationBias, PenetrationBias * 0.2f), new Vector4(0.2f));
//The inertia tensor is in world space, so no jacobian transformation is required.
Vector3Width4.Multiply(ref LinearJacobianA, ref InverseMassA, out LinearJacobianITA);
Vector3Width4.Multiply(ref LinearJacobianB, ref InverseMassB, out LinearJacobianITB);
Matrix3x3Width4.Transform(ref AngularJacobianA, ref InverseInertiaTensorA, out AngularJacobianITA);
Matrix3x3Width4.Transform(ref AngularJacobianB, ref InverseInertiaTensorB, out AngularJacobianITB);
Vector4 angularContributionA, angularContributionB;
Vector3Width4.Dot(ref AngularJacobianITA, ref AngularJacobianITA, out angularContributionA);
Vector3Width4.Dot(ref AngularJacobianITB, ref AngularJacobianITB, out angularContributionB);
var inverseEffectiveMass = InverseMassA + InverseMassB + angularContributionA + angularContributionB;
Vector4 CollisionSoftness = new Vector4(5);
Softness = CollisionSoftness * inverseEffectiveMass * inverseDt;
EffectiveMass = Vector4.One / (Softness + inverseEffectiveMass);
}
internal static unsafe void Transpose(ref Vector3Width4 source, Vector3* result)
{
result[0] = new Vector3(source.X.X, source.Y.X, source.Z.X);
result[1] = new Vector3(source.X.Y, source.Y.Y, source.Z.Y);
result[2] = new Vector3(source.X.Z, source.Y.Z, source.Z.Z);
result[3] = new Vector3(source.X.W, source.Y.W, source.Z.W);
}
public static void Subtract(ref Vector3Width4 a, ref Vector3Width4 b, out Vector3Width4 result)
{
result.X = a.X - b.X;
result.Y = a.Y - b.Y;
result.Z = a.Z - b.Z;
}
public static void Subtract(ref Vector3Width4 a, ref Vector3Width4 b, out Vector3Width4 result)
{
result.X = a.X - b.X;
result.Y = a.Y - b.Y;
result.Z = a.Z - b.Z;
}
public static void Multiply(ref Vector3Width4 a, float b, out Vector3Width4 result)
{
result.X = a.X * b;
result.Y = a.Y * b;
result.Z = a.Z * b;
}
public void SolveIteration(ref Vector3Width4 linearVelocityA, ref Vector3Width4 angularVelocityA, ref Vector3Width4 linearVelocityB, ref Vector3Width4 angularVelocityB)
{
Vector4 linearA, angularA, linearB, angularB;
Vector3Width4.Dot(ref LinearJacobianA, ref linearVelocityA, out linearA);
Vector3Width4.Dot(ref AngularJacobianA, ref angularVelocityA, out angularA);
Vector3Width4.Dot(ref LinearJacobianB, ref linearVelocityB, out linearB);
Vector3Width4.Dot(ref AngularJacobianB, ref angularVelocityB, out angularB);
var lambda = EffectiveMass * (linearA + angularA + linearB + angularB + PenetrationBias - AccumulatedImpulse * Softness);
var previous = AccumulatedImpulse;
AccumulatedImpulse = Vector4.Max(Vector4.Zero, AccumulatedImpulse + lambda);
lambda = AccumulatedImpulse - previous;
ApplyImpulse(ref lambda, ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);
}
public static void Transform(ref Vector3Width4 v, ref Matrix3x3Width4 m, out Vector3Width4 result)
{
result.X = v.X * m.M11 + v.Y * m.M21 + v.Z * m.M31;
result.Y = v.X * m.M12 + v.Y * m.M22 + v.Z * m.M32;
result.Z = v.X * m.M13 + v.Y * m.M23 + v.Z * m.M33;
}
internal static void Dot(ref Vector3Width4 a, ref Vector3Width4 b, out Vector4 result)
{
result = a.X * b.X + a.Y * b.Y + a.Z * b.Z;
}
internal static unsafe void Transpose(ref Vector3Width4 source, out Vector3 a, out Vector3 b, out Vector3 c, out Vector3 d)
{
a = new Vector3(source.X.X, source.Y.X, source.Z.X);
b = new Vector3(source.X.Y, source.Y.Y, source.Z.Y);
c = new Vector3(source.X.Z, source.Y.Z, source.Z.Z);
d = new Vector3(source.X.W, source.Y.W, source.Z.W);
}
public static void Negate(ref Vector3Width4 v, out Vector3Width4 result)
{
result.X = -v.X;
result.Y = -v.Y;
result.Z = -v.Z;
}
public static void Cross(ref Vector3Width4 a, ref Vector3Width4 b, out Vector3Width4 result)
{
result.X = a.Y * b.Z - a.Z * b.Y;
result.Y = a.Z * b.X - a.X * b.Z;
result.Z = a.X * b.Y - a.Y * b.X;
}
void ApplyImpulse(ref Vector4 lambda, ref Vector3Width4 linearVelocityA, ref Vector3Width4 angularVelocityA, ref Vector3Width4 linearVelocityB, ref Vector3Width4 angularVelocityB)
{
Vector3Width4 linearChangeA, linearChangeB;
Vector3Width4.Multiply(ref LinearJacobianITA, ref lambda, out linearChangeA);
Vector3Width4.Multiply(ref LinearJacobianITB, ref lambda, out linearChangeB);
Vector3Width4.Add(ref linearVelocityA, ref linearChangeA, out linearVelocityA);
Vector3Width4.Add(ref linearVelocityB, ref linearChangeB, out linearVelocityB);
//World inertia available, so no need for extra transforms.
Vector3Width4 angularChangeA, angularChangeB;
Vector3Width4.Multiply(ref AngularJacobianITA, ref lambda, out angularChangeA);
Vector3Width4.Multiply(ref AngularJacobianITB, ref lambda, out angularChangeB);
Vector3Width4.Add(ref angularVelocityA, ref angularChangeA, out angularVelocityA);
Vector3Width4.Add(ref angularVelocityB, ref angularChangeB, out angularVelocityB);
}
public static void Negate(ref Vector3Width4 v, out Vector3Width4 result)
{
result.X = -v.X;
result.Y = -v.Y;
result.Z = -v.Z;
}
public static void Transform(ref Vector3Width4 v, ref Matrix3x3Width4 m, out Vector3Width4 result)
{
result.X = v.X * m.M11 + v.Y * m.M21 + v.Z * m.M31;
result.Y = v.X * m.M12 + v.Y * m.M22 + v.Z * m.M32;
result.Z = v.X * m.M13 + v.Y * m.M23 + v.Z * m.M33;
}
internal static void Dot(ref Vector3Width4 a, ref Vector3Width4 b, out Vector4 result)
{
result = a.X * b.X + a.Y * b.Y + a.Z * b.Z;
}
void ApplyImpulse(float lambda, ref Vector3Width4 velocities)
{
Vector3Width4 change;
Vector3Width4.Multiply(ref JacobiansIT, lambda, out change);
Vector3Width4.Subtract(ref velocities, ref change, out velocities);
}
public static unsafe void Test()
{
var identityMatrix = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 0, 1) };
var a = new RigidBody
{
Position = new Vector3(0, 0, 0),
Orientation = identityMatrix,
InertiaTensorInverse = identityMatrix,
InverseMass = 1,
LinearVelocity = new Vector3(0, 0, 0)
};
var b = new RigidBody
{
Position = new Vector3(0, 1, 0),
Orientation = identityMatrix,
InertiaTensorInverse = identityMatrix,
InverseMass = 1,
LinearVelocity = new Vector3(0, 0, 0)
};
RigidBody* aBodies = stackalloc RigidBody[4];
RigidBody* bBodies = stackalloc RigidBody[4];
for (int i = 0; i < 4; ++i)
{
aBodies[i] = a;
bBodies[i] = b;
}
Vector3 up = Vector3.UnitY;
var contactPosition = new Vector3Width4();
var contactNormal = new Vector3Width4(ref up, ref up, ref up, ref up);
var contactPenetration = new Vector4();
var constraint = new VectorizedManifoldConstraint
{
BodyA0 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3() },
BodyA1 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3() },
BodyA2 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3() },
BodyA3 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3() },
BodyB0 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3(0, 1, 0) },
BodyB1 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3(0, 1, 0) },
BodyB2 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3(0, 1, 0) },
BodyB3 = new BodyState { InertiaTensorInverse = new Matrix3x3 { X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 1, 0) }, InverseMass = 1, Position = new Vector3(0, 1, 0) },
VelocitiesB0 = new Velocities { LinearVelocity = new Vector3(0, -1, 0) },
VelocitiesB1 = new Velocities { LinearVelocity = new Vector3(0, -1, 0) },
VelocitiesB2 = new Velocities { LinearVelocity = new Vector3(0, -1, 0) },
VelocitiesB3 = new Velocities { LinearVelocity = new Vector3(0, -1, 0) },
//a = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration },
//b = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration },
//c = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration },
//d = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration }
};
constraint.ConstraintCount = 4;
var pointer = stackalloc VectorizedManifoldPenetrationConstraint[constraint.ConstraintCount];
constraint.Constraints = pointer;
//constraint.Constraints = new VectorizedManifoldPenetrationConstraint[constraintCount];
for (int i = 0; i < constraint.ConstraintCount; ++i)
{
constraint.Constraints[i] = new VectorizedManifoldPenetrationConstraint { ContactPosition = contactPosition, ContactNormal = contactNormal, ContactPenetration = contactPenetration };
}
float dt = 1 / 60f;
float inverseDt = 1 / dt;
constraint.Prestep(inverseDt);
constraint.WarmStart();
constraint.SolveIteration();
var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
for (int i = 0; i < VectorizedConstraintTest.TestCount / constraint.ConstraintCount; ++i)
{
constraint.Prestep(inverseDt);
constraint.WarmStart();
for (int iterationIndex = 0; iterationIndex < VectorizedConstraintTest.IterationCount; ++iterationIndex)
{
constraint.SolveIteration();
}
}
var endtime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
Console.WriteLine($"Vectorized Manifold: {endtime - startTime}");
}
public static void Add(ref Vector3Width4 a, ref Vector3Width4 b, out Vector3Width4 result)
{
result.X = a.X + b.X;
result.Y = a.Y + b.Y;
result.Z = a.Z + b.Z;
}