|
private Dot ( |
||
| a | ||
| b | ||
| result | Vector4 | |
| Результат | void | |
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);
}
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);
}
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);
}
