|
|
|
|
@ -196,6 +196,7 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context )
|
|
|
|
|
cp->baseSeparation = mp->separation - b3Dot( b3Sub( cp->rB, cp->rA ), normal );
|
|
|
|
|
cp->normalImpulse = warmStartScale * mp->normalImpulse;
|
|
|
|
|
cp->totalNormalImpulse = 0.0f;
|
|
|
|
|
cp->stepNormalImpulse = 0.0f;
|
|
|
|
|
|
|
|
|
|
b3Vec3 rA = cp->rA;
|
|
|
|
|
b3Vec3 rB = cp->rB;
|
|
|
|
|
@ -456,6 +457,7 @@ void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool use
|
|
|
|
|
deltaImpulse = newImpulse - cp->normalImpulse;
|
|
|
|
|
cp->normalImpulse = newImpulse;
|
|
|
|
|
cp->totalNormalImpulse += newImpulse;
|
|
|
|
|
if ( useBias == false ) cp->stepNormalImpulse += newImpulse;
|
|
|
|
|
|
|
|
|
|
totalNormalImpulse += newImpulse;
|
|
|
|
|
totalTwistLimit += cp->leverArm * cp->normalImpulse;
|
|
|
|
|
@ -650,6 +652,7 @@ void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context )
|
|
|
|
|
impulse = newImpulse - cp->normalImpulse;
|
|
|
|
|
cp->normalImpulse = newImpulse;
|
|
|
|
|
cp->totalNormalImpulse += impulse;
|
|
|
|
|
cp->stepNormalImpulse += impulse;
|
|
|
|
|
|
|
|
|
|
// apply contact impulse
|
|
|
|
|
b3Vec3 P = b3MulSV( impulse, normal );
|
|
|
|
|
@ -754,6 +757,7 @@ void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int work
|
|
|
|
|
b3ManifoldPoint* mp = manifold->points + pointIndex;
|
|
|
|
|
mp->normalImpulse = cp->normalImpulse;
|
|
|
|
|
mp->totalNormalImpulse = cp->totalNormalImpulse;
|
|
|
|
|
mp->stepNormalImpulse = cp->stepNormalImpulse;
|
|
|
|
|
mp->normalVelocity = cp->relativeVelocity;
|
|
|
|
|
|
|
|
|
|
if ( checkHitEvents && flagged == false &&
|
|
|
|
|
@ -1330,6 +1334,7 @@ typedef struct b3ContactConstraintPointWide
|
|
|
|
|
b3FloatW baseSeparations;
|
|
|
|
|
b3FloatW normalImpulses;
|
|
|
|
|
b3FloatW totalNormalImpulses;
|
|
|
|
|
b3FloatW stepNormalImpulses;
|
|
|
|
|
b3FloatW normalMasses;
|
|
|
|
|
b3FloatW leverArms;
|
|
|
|
|
b3FloatW relativeVelocities;
|
|
|
|
|
@ -1805,6 +1810,7 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context )
|
|
|
|
|
|
|
|
|
|
( (float*)&cp->normalImpulses )[lane] = warmStartScale * mp->normalImpulse;
|
|
|
|
|
( (float*)&cp->totalNormalImpulses )[lane] = 0.0f;
|
|
|
|
|
( (float*)&cp->stepNormalImpulses )[lane] = 0.0f;
|
|
|
|
|
|
|
|
|
|
b3Vec3 rnA = b3Cross( rA, normal );
|
|
|
|
|
b3Vec3 rnB = b3Cross( rB, normal );
|
|
|
|
|
@ -1892,6 +1898,7 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context )
|
|
|
|
|
( (float*)&cp->baseSeparations )[lane] = 0.0f;
|
|
|
|
|
( (float*)&cp->normalImpulses )[lane] = 0.0f;
|
|
|
|
|
( (float*)&cp->totalNormalImpulses )[lane] = 0.0f;
|
|
|
|
|
( (float*)&cp->stepNormalImpulses )[lane] = 0.0f;
|
|
|
|
|
( (float*)&cp->normalMasses )[lane] = 0.0f;
|
|
|
|
|
( (float*)&cp->relativeVelocities )[lane] = 0.0f;
|
|
|
|
|
( (float*)&cp->leverArms )[lane] = 0.0f;
|
|
|
|
|
@ -2051,6 +2058,7 @@ void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool u
|
|
|
|
|
b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses );
|
|
|
|
|
cp->normalImpulses = newImpulse;
|
|
|
|
|
cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, newImpulse );
|
|
|
|
|
if ( useBias == false ) cp->stepNormalImpulses = b3AddW( cp->stepNormalImpulses, newImpulse );
|
|
|
|
|
|
|
|
|
|
totalNormalImpulse = b3AddW( totalNormalImpulse, newImpulse );
|
|
|
|
|
totalTwistLimit = b3AddW( totalTwistLimit, b3MulW( cp->leverArms, newImpulse ) );
|
|
|
|
|
@ -2232,6 +2240,7 @@ void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context )
|
|
|
|
|
b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses );
|
|
|
|
|
cp->normalImpulses = newImpulse;
|
|
|
|
|
cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, deltaImpulse );
|
|
|
|
|
cp->stepNormalImpulses = b3AddW( cp->stepNormalImpulses, deltaImpulse );
|
|
|
|
|
|
|
|
|
|
// Apply contact impulse
|
|
|
|
|
b3Vec3W P = b3MulSVW( deltaImpulse, c->normal );
|
|
|
|
|
@ -2335,6 +2344,7 @@ void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int wo
|
|
|
|
|
b3ManifoldPoint* mp = m->points + pointIndex;
|
|
|
|
|
mp->normalImpulse = normalImpulse[lane];
|
|
|
|
|
mp->totalNormalImpulse = totalNormalImpulse[lane];
|
|
|
|
|
mp->stepNormalImpulse = ((const float*)&cp->stepNormalImpulses)[lane];
|
|
|
|
|
mp->normalVelocity = normalVelocity[lane];
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|