internal void setupContactConstraint( btSolverConstraint solverConstraint,
btSolverBody bodyA, btSolverBody bodyB,
btManifoldPoint cp, btContactSolverInfo infoGlobal,
out double relaxation,
ref btVector3 rel_pos1, ref btVector3 rel_pos2 )
{
// ref btVector3 pos1 = cp.getPositionWorldOnA();
// ref btVector3 pos2 = cp.getPositionWorldOnB();
btRigidBody rb0 = bodyA.m_originalBody;
btRigidBody rb1 = bodyB.m_originalBody;
// btVector3 rel_pos1 = pos1 - colObj0.getWorldTransform().getOrigin();
// btVector3 rel_pos2 = pos2 - colObj1.getWorldTransform().getOrigin();
//rel_pos1 = pos1 - bodyA.getWorldTransform().getOrigin();
//rel_pos2 = pos2 - bodyB.getWorldTransform().getOrigin();
relaxation = 1;
btVector3 torqueAxis0; rel_pos1.cross( ref cp.m_normalWorldOnB, out torqueAxis0 );
btVector3 tmp;
//solverConstraint.m_angularComponentA = rb0 != null ? rb0.m_invInertiaTensorWorld * torqueAxis0 * rb0.getAngularFactor() : btVector3.Zero;
if( rb0 != null )
{
rb0.m_invInertiaTensorWorld.Apply( ref torqueAxis0, out tmp );
tmp.Mult( ref rb0.m_angularFactor, out solverConstraint.m_angularComponentA );
}
else
solverConstraint.m_angularComponentA = btVector3.Zero;
btVector3 torqueAxis1; rel_pos2.cross( ref cp.m_normalWorldOnB, out torqueAxis1 );
torqueAxis1.Invert( out torqueAxis1 );
//solverConstraint.m_angularComponentB = rb1 != null ? rb1.m_invInertiaTensorWorld * -torqueAxis1 * rb1.getAngularFactor() : btVector3.Zero;
if( rb1 != null )
{
rb1.m_invInertiaTensorWorld.Apply( ref torqueAxis1, out tmp );
tmp.Mult( ref rb1.m_angularFactor, out solverConstraint.m_angularComponentB );
}
else
solverConstraint.m_angularComponentB = btVector3.Zero;
{
#if COMPUTE_IMPULSE_DENOM
double denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
double denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else
btVector3 vec;
double denom0 = 0;
double denom1 = 0;
if( rb0 != null )
{
( solverConstraint.m_angularComponentA ).cross( ref rel_pos1, out vec );
denom0 = rb0.getInvMass() + cp.m_normalWorldOnB.dot( vec );
}
if( rb1 != null )
{
solverConstraint.m_angularComponentB.Invert( out tmp );
tmp.cross( ref rel_pos2, out vec );
denom1 = rb1.getInvMass() + cp.m_normalWorldOnB.dot( vec );
}
#endif //COMPUTE_IMPULSE_DENOM
double denom = relaxation / ( denom0 + denom1 );
btScalar.Dbg( "m_jacDiagABInv 3 set to " + denom.ToString( "g17" ) );
solverConstraint.m_jacDiagABInv = denom;
}
if( rb0 != null )
{
solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
btScalar.Dbg( "Torque Axis to relpos1 " + torqueAxis0 );
}
else
{
solverConstraint.m_contactNormal1 = btVector3.Zero;
solverConstraint.m_relpos1CrossNormal = btVector3.Zero;
}
if( rb1 != null )
{
cp.m_normalWorldOnB.Invert( out solverConstraint.m_contactNormal2 );
solverConstraint.m_relpos2CrossNormal = torqueAxis1;
btScalar.Dbg( "Torque Axis to relpos2 " + torqueAxis1 );
}
else
{
solverConstraint.m_contactNormal2 = btVector3.Zero;
solverConstraint.m_relpos2CrossNormal = btVector3.Zero;
}
double restitution = 0;
double penetration = cp.getDistance() + infoGlobal.m_linearSlop;
{
btVector3 vel1, vel2;
vel1 = rb0 != null ? rb0.getVelocityInLocalPoint( ref rel_pos1 ) : btVector3.Zero;
vel2 = rb1 != null ? rb1.getVelocityInLocalPoint( ref rel_pos2 ) : btVector3.Zero;
// btVector3 vel2 = rb1 ? rb1.getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
btVector3 vel; vel1.Sub( ref vel2, out vel );
double rel_vel = cp.m_normalWorldOnB.dot( ref vel );
solverConstraint.m_friction = cp.m_combinedFriction;
restitution = restitutionCurve( rel_vel, cp.m_combinedRestitution );
if( restitution <= btScalar.BT_ZERO )
{
restitution = 0;
};
}
///warm starting (or zero if disabled)
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 )
{
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if( rb0 != null )
{
solverConstraint.m_contactNormal1.Mult2( ref bodyA.m_invMass, ref rb0.m_linearFactor, out tmp );
bodyA.applyImpulse( ref tmp, ref solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse );
}
if( rb1 != null )
{
solverConstraint.m_contactNormal2.Mult2( ref rb1.m_linearFactor, ref bodyB.m_invMass, out tmp );
tmp.Invert( out tmp );
btVector3 tmp2; solverConstraint.m_angularComponentB.Invert( out tmp2 );
bodyB.applyImpulse( ref tmp, ref tmp2, -(double)solverConstraint.m_appliedImpulse );
}
}
else
{
solverConstraint.m_appliedImpulse = 0;
}
solverConstraint.m_appliedPushImpulse = 0;
{
btVector3 externalForceImpulseA = bodyA.m_originalBody != null ? bodyA.m_externalForceImpulse : btVector3.Zero;
btVector3 externalTorqueImpulseA = bodyA.m_originalBody != null ? bodyA.m_externalTorqueImpulse : btVector3.Zero;
btVector3 externalForceImpulseB = bodyB.m_originalBody != null ? bodyB.m_externalForceImpulse : btVector3.Zero;
btVector3 externalTorqueImpulseB = bodyB.m_originalBody != null ? bodyB.m_externalTorqueImpulse : btVector3.Zero;
btScalar.Dbg( "external torque impulses " + externalTorqueImpulseA + externalTorqueImpulseB );
double vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref bodyA.m_linearVelocity, ref externalForceImpulseA )
+ solverConstraint.m_relpos1CrossNormal.dotAdded( ref bodyA.m_angularVelocity, ref externalTorqueImpulseA );
double vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref bodyB.m_linearVelocity, ref externalForceImpulseB )
+ solverConstraint.m_relpos2CrossNormal.dotAdded( ref bodyB.m_angularVelocity, ref externalTorqueImpulseB );
double rel_vel = vel1Dotn + vel2Dotn;
double positionalError = 0;
double velocityError = restitution - rel_vel;// * damping;
double erp = infoGlobal.m_erp2;
if( !infoGlobal.m_splitImpulse || ( penetration > infoGlobal.m_splitImpulsePenetrationThreshold ) )
{
erp = infoGlobal.m_erp;
}
if( penetration > 0 )
{
positionalError = 0;
velocityError -= penetration / infoGlobal.m_timeStep;
}
else
{
positionalError = -penetration * erp / infoGlobal.m_timeStep;
}
double penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
if( !infoGlobal.m_splitImpulse || ( penetration > infoGlobal.m_splitImpulsePenetrationThreshold ) )
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA.m_externalForce*bodyA.m_invMass-bodyB.m_externalForce/bodyB.m_invMass)*solverConstraint.m_jacDiagABInv;
btScalar.Dbg( "Constraint 3 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_rhsPenetration = 0;
}
else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
btScalar.Dbg( "Constraint 4 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
solverConstraint.m_cfm = 0;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
}
internal void setFrictionConstraintImpulse( btSolverConstraint solverConstraint,
btSolverBody bodyA, btSolverBody bodyB,
btManifoldPoint cp, btContactSolverInfo infoGlobal )
{
btRigidBody rb0 = bodyA.m_originalBody;
btRigidBody rb1 = bodyB.m_originalBody;
{
btSolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 )
{
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
btScalar.Dbg( "New Applied source is " + cp.m_appliedImpulseLateral1.ToString( "g17" ) );
if( rb0 != null )
{
btVector3 tmp; frictionConstraint1.m_contactNormal1.Mult2( ref rb0.m_linearFactor, rb0.getInvMass(), out tmp );
bodyA.applyImpulse( ref tmp, ref frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse );
}
if( rb1 != null )
{
btVector3 tmp; frictionConstraint1.m_contactNormal2.Mult2( ref rb1.m_linearFactor, -rb1.getInvMass(), out tmp );
btVector3 tmp2; frictionConstraint1.m_angularComponentB.Invert( out tmp2 );
bodyB.applyImpulse( ref tmp, ref tmp2, -(double)frictionConstraint1.m_appliedImpulse );
}
}
else
{
frictionConstraint1.m_appliedImpulse = 0;
}
}
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
{
btSolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 )
{
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
if( rb0 != null )
{
btVector3 tmp; frictionConstraint2.m_contactNormal1.Mult( rb0.getInvMass(), out tmp );
bodyA.applyImpulse( ref tmp, ref frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse );
}
if( rb1 != null )
{
btVector3 tmp; frictionConstraint2.m_contactNormal2.Mult( -rb1.getInvMass(), out tmp );
btVector3 tmp2; frictionConstraint2.m_angularComponentB.Invert( out tmp2 );
bodyB.applyImpulse( ref tmp, ref tmp2, -(double)frictionConstraint2.m_appliedImpulse );
}
}
else
{
frictionConstraint2.m_appliedImpulse = 0;
}
}
}
internal void setupRollingFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis1
, btSolverBody solverBodyA, btSolverBody solverBodyB,
btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2,
btCollisionObject colObj0, btCollisionObject colObj1, double relaxation,
double desiredVelocity = 0, double cfmSlip = 0.0 )
{
btVector3 normalAxis = btVector3.Zero;
solverConstraint.m_contactNormal1 = normalAxis;
normalAxis.Invert( out solverConstraint.m_contactNormal2 );
//btSolverBody solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
//btSolverBody solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
btRigidBody body0 = solverBodyA.m_originalBody;
btRigidBody body1 = solverBodyB.m_originalBody;
solverConstraint.m_solverBodyA = solverBodyA;
solverConstraint.m_solverBodyB = solverBodyB;
solverConstraint.m_friction = cp.m_combinedRollingFriction;
solverConstraint.m_originalContactPoint = null;
solverConstraint.m_appliedImpulse = 0;
solverConstraint.m_appliedPushImpulse = 0;
btVector3 iMJaA;
btVector3 iMJaB;
{
btVector3 ftorqueAxis1; normalAxis1.Invert( out ftorqueAxis1 );
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
if( body0 != null )
{
body0.m_invInertiaTensorWorld.Apply( ref ftorqueAxis1, out iMJaA );
iMJaA.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA );
}
else
iMJaA = btVector3.Zero;
//solverConstraint.m_angularComponentA = body0 != null ? body0.getInvInertiaTensorWorld() * ftorqueAxis1 * body0.getAngularFactor() : btVector3.Zero;
}
{
btVector3 ftorqueAxis1 = normalAxis1;
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
if( body1 != null )
{
body1.m_invInertiaTensorWorld.Apply( ref ftorqueAxis1, out iMJaB );
iMJaB.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB );
}
else
iMJaB = btVector3.Zero;
//solverConstraint.m_angularComponentB = body1 != null ? body1.getInvInertiaTensorWorld() * ftorqueAxis1 * body1.getAngularFactor() : btVector3.Zero;
}
{
//btVector3 iMJaA = body0 != null ? body0.getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3.Zero;
//btVector3 iMJaB = body1 != null ? body1.getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3.Zero;
double sum = 0;
sum += iMJaA.dot( ref solverConstraint.m_relpos1CrossNormal );
sum += iMJaB.dot( ref solverConstraint.m_relpos2CrossNormal );
btScalar.Dbg( "m_jacDiagABInv 2 set to " + ( btScalar.BT_ONE / sum ).ToString( "g17" ) );
solverConstraint.m_jacDiagABInv = btScalar.BT_ONE / sum;
}
{
double rel_vel;
double vel1Dotn;
double vel2Dotn;
//double vel1Dotn = solverConstraint.m_contactNormal1.dot( body0 != null ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3.Zero )
// + solverConstraint.m_relpos1CrossNormal.dot( body0 != null ? solverBodyA.m_angularVelocity : btVector3.Zero );
//double vel2Dotn = solverConstraint.m_contactNormal2.dot( body1 != null ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3.Zero )
// + solverConstraint.m_relpos2CrossNormal.dot( body1 != null ? solverBodyB.m_angularVelocity : btVector3.Zero );
if( body0 != null )
vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyA.m_linearVelocity, ref solverBodyA.m_externalForceImpulse )
+ solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyA.m_angularVelocity );
else
vel1Dotn = 0;
if( body1 != null )
vel2Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse )
+ solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyB.m_angularVelocity );
else
vel2Dotn = 0;
rel_vel = vel1Dotn + vel2Dotn;
// double positionalError = 0;
double velocityError = desiredVelocity - rel_vel;
double velocityImpulse = velocityError * ( solverConstraint.m_jacDiagABInv );
solverConstraint.m_rhs = velocityImpulse;
btScalar.Dbg( "Constraint 2 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
}
}
internal btSolverConstraint addRollingFrictionConstraint( ref btVector3 normalAxis
, btSolverBody solverBodyA, btSolverBody solverBodyB
, int frictionIndex, btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0 )
{
btSolverConstraint solverConstraint;
m_tmpSolverContactRollingFrictionConstraintPool.Add( solverConstraint = BulletGlobals.SolverConstraintPool.Get() );
solverConstraint.m_frictionIndex = frictionIndex;
setupRollingFrictionConstraint( solverConstraint, ref normalAxis, solverBodyA, solverBodyB, cp, ref rel_pos1, ref rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip );
return solverConstraint;
}
internal void setupFrictionConstraint( btSolverConstraint solverConstraint, ref btVector3 normalAxis
//, int solverBodyIdA, int solverBodyIdB
, btSolverBody solverBodyA, btSolverBody solverBodyB
, btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0.0 )
{
//btSolverBody solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
//btSolverBody solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
btRigidBody body0 = solverBodyA.m_originalBody;
btRigidBody body1 = solverBodyB.m_originalBody;
solverConstraint.m_solverBodyA = solverBodyA;
solverConstraint.m_solverBodyB = solverBodyB;
solverConstraint.m_friction = cp.m_combinedFriction;
solverConstraint.m_originalContactPoint = null;
solverConstraint.m_appliedImpulse = 0;
solverConstraint.m_appliedPushImpulse = 0;
if( body0 != null )
{
solverConstraint.m_contactNormal1 = normalAxis;
rel_pos1.cross( ref solverConstraint.m_contactNormal1, out solverConstraint.m_relpos1CrossNormal );
btVector3 tmp;
body0.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp );
tmp.Mult( ref body0.m_angularFactor, out solverConstraint.m_angularComponentA );
}
else
{
solverConstraint.m_contactNormal1.setZero();
solverConstraint.m_relpos1CrossNormal.setZero();
solverConstraint.m_angularComponentA.setZero();
}
if( body1 != null )
{
normalAxis.Invert( out solverConstraint.m_contactNormal2 );
rel_pos2.cross( ref solverConstraint.m_contactNormal2, out solverConstraint.m_relpos2CrossNormal );
btVector3 tmp;
body1.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp );
tmp.Mult( ref body1.m_angularFactor, out solverConstraint.m_angularComponentB );
}
else
{
solverConstraint.m_contactNormal2 = btVector3.Zero;
solverConstraint.m_relpos2CrossNormal = btVector3.Zero;
solverConstraint.m_angularComponentB = btVector3.Zero;
}
{
btVector3 vec;
double denom0 = 0;
double denom1 = 0;
if( body0 != null )
{
solverConstraint.m_angularComponentA.cross( ref rel_pos1, out vec );
denom0 = body0.getInvMass() + normalAxis.dot( ref vec );
}
if( body1 != null )
{
btVector3 tmp;
solverConstraint.m_angularComponentB.Invert( out tmp );
tmp.cross( ref rel_pos2, out vec );
denom1 = body1.getInvMass() + normalAxis.dot( ref vec );
}
double denom = relaxation / ( denom0 + denom1 );
btScalar.Dbg( "m_jacDiagABInv 1 set to " + denom.ToString( "g17" ) );
solverConstraint.m_jacDiagABInv = denom;
}
{
double rel_vel;
double vel1Dotn;
double vel2Dotn;
//double vel1Dotn = solverConstraint.m_contactNormal1.dot( body0 != null ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3.Zero )
// + solverConstraint.m_relpos1CrossNormal.dot( body0 != null ? solverBodyA.m_angularVelocity : btVector3.Zero );
if( body0 != null )
vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref solverBodyA.m_linearVelocity, ref solverBodyA.m_externalForceImpulse )
+ solverConstraint.m_relpos1CrossNormal.dot( ref solverBodyA.m_angularVelocity );
else
vel1Dotn = 0;
//double vel2Dotn = solverConstraint.m_contactNormal2.dot( body1 != null ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3.Zero )
// + solverConstraint.m_relpos2CrossNormal.dot( body1 != null ? solverBodyB.m_angularVelocity : btVector3.Zero );
if( body1 != null )
vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref solverBodyB.m_linearVelocity, ref solverBodyB.m_externalForceImpulse )
+ solverConstraint.m_relpos2CrossNormal.dot( ref solverBodyB.m_angularVelocity );
else
vel2Dotn = 0;
rel_vel = vel1Dotn + vel2Dotn;
// double positionalError = 0;
double velocityError = desiredVelocity - rel_vel;
double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = velocityImpulse;
btScalar.Dbg( "Constraint 1 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_rhsPenetration = 0;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
}
}
internal btSolverConstraint addFrictionConstraint( ref btVector3 normalAxis
//, int solverBodyIdA, int solverBodyIdB
, btSolverBody solverBodyA, btSolverBody solverBodyB
, int frictionIndex, btManifoldPoint cp, ref btVector3 rel_pos1, ref btVector3 rel_pos2, btCollisionObject colObj0, btCollisionObject colObj1, double relaxation, double desiredVelocity = 0, double cfmSlip = 0 )
{
btSolverConstraint solverConstraint;
m_tmpSolverContactFrictionConstraintPool.Add( solverConstraint = BulletGlobals.SolverConstraintPool.Get() );
solverConstraint.m_frictionIndex = frictionIndex;
setupFrictionConstraint( solverConstraint, ref normalAxis, solverBodyA, solverBodyB, cp, ref rel_pos1, ref rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip );
btScalar.Dbg( "Setup Impulse2 = " + solverConstraint.m_appliedImpulse.ToString( "g17" ) );
return solverConstraint;
}
void clearUserCache( btManifoldPoint pt )
{
object oldPtr = pt.m_userPersistentData;
if( oldPtr != null )
{
#if DEBUG_PERSISTENCY
int i;
int occurance = 0;
for( i = 0; i < m_cachedPoints; i++ )
{
if( m_pointCache[i].m_userPersistentData == oldPtr )
{
occurance++;
if( occurance > 1 )
Console.WriteLine( "error in clearUserCache\n" );
}
}
Debug.Assert( occurance <= 0 );
#endif //DEBUG_PERSISTENCY
if( pt.m_userPersistentData != null && gContactDestroyedCallback != null )
{
gContactDestroyedCallback( pt.m_userPersistentData );
pt.m_userPersistentData = null;
}
#if DEBUG_PERSISTENCY
DebugPersistency();
#endif
}
}
internal abstract double addSingleResult( btManifoldPoint cp, btCollisionObjectWrapper colObj0Wrap, int partId0, int index0, btCollisionObjectWrapper colObj1Wrap, int partId1, int index1 ) ;
bool validContactDistance( btManifoldPoint pt )
{
return pt.m_distance1 <= m_contactBreakingThreshold;
}
internal void replaceContactPoint( btManifoldPoint newPoint, int insertIndex )
{
Debug.Assert( validContactDistance( newPoint ) );
#if MAINTAIN_PERSISTENCY
// bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized;
btManifoldPoint old_point = m_pointCache[insertIndex];
Debug.Assert( old_point.m_lifeTime >= 0 );
m_pointCache[insertIndex] = newPoint;
newPoint.m_userPersistentData = old_point.m_userPersistentData;
newPoint.m_appliedImpulse = old_point.m_appliedImpulse;
newPoint.m_appliedImpulseLateral1 = old_point.m_appliedImpulseLateral1;
newPoint.m_appliedImpulseLateral2 = old_point.m_appliedImpulseLateral2;
newPoint.m_lifeTime = old_point.m_lifeTime;
BulletGlobals.ManifoldPointPool.Free( old_point );
#else
clearUserCache( m_pointCache[insertIndex] );
m_pointCache[insertIndex] = newPoint;
#endif
}
internal int addManifoldPoint( btManifoldPoint newPoint, bool isPredictive = false )
{
if( !isPredictive )
{
Debug.Assert( validContactDistance( newPoint ) );
}
int insertIndex = m_cachedPoints;
if( m_cachedPoints == MANIFOLD_CACHE_SIZE )
{
if( MANIFOLD_CACHE_SIZE >= 4 )
//sort cache so best points come first, based on area
insertIndex = sortCachedPoints( newPoint );
else
insertIndex = 0;
clearUserCache( m_pointCache[insertIndex] );
}
else
{
m_cachedPoints++;
}
if( insertIndex < 0 )
insertIndex = 0;
Debug.Assert( m_pointCache[insertIndex] == null ||
m_pointCache[insertIndex].m_userPersistentData == null );
m_pointCache[insertIndex] = newPoint;
return insertIndex;
}
internal int getCacheEntry( ref btManifoldPoint newPoint )
{
double shortestDist = m_contactBreakingThresholdSquared;
int size = m_cachedPoints;
int nearestPoint = -1;
for( int i = 0; i < size; i++ )
{
btManifoldPoint mp = m_pointCache[i];
btVector3 diffA; mp.m_localPointA.Sub( ref newPoint.m_localPointA, out diffA );
double distToManiPoint = diffA.dot( ref diffA );
if( distToManiPoint < shortestDist )
{
shortestDist = distToManiPoint;
nearestPoint = i;
}
}
return nearestPoint;
}
/// sort cached points so most isolated points come first
int sortCachedPoints( btManifoldPoint pt )
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
int maxPenetrationIndex = -1;
#if KEEP_DEEPEST_POINT
double maxPenetration = pt.getDistance();
for( int i = 0; i < 4; i++ )
{
if( m_pointCache[i].getDistance() < maxPenetration )
{
maxPenetrationIndex = i;
maxPenetration = m_pointCache[i].getDistance();
}
}
#endif //KEEP_DEEPEST_POINT
double res0 = ( 0 )
, res1 = ( (double)( 0 ) ), res2 = ( (double)( 0 ) )
, res3 = ( (double)( 0 ) );
if( gContactCalcArea3Points )
{
if( maxPenetrationIndex != 0 )
{
btVector3 a0; pt.m_localPointA.Sub( ref m_pointCache[1].m_localPointA, out a0 );
btVector3 b0; m_pointCache[3].m_localPointA.Sub( ref m_pointCache[2].m_localPointA, out b0 );
btVector3 cross; a0.cross( ref b0, out cross );
res0 = cross.length2();
}
if( maxPenetrationIndex != 1 )
{
btVector3 a1; pt.m_localPointA.Sub( ref m_pointCache[0].m_localPointA, out a1 );
btVector3 b1; m_pointCache[3].m_localPointA.Sub( ref m_pointCache[2].m_localPointA, out b1 );
btVector3 cross; a1.cross( ref b1, out cross );
res1 = cross.length2();
}
if( maxPenetrationIndex != 2 )
{
btVector3 a2; pt.m_localPointA.Sub( ref m_pointCache[0].m_localPointA, out a2 );
btVector3 b2; m_pointCache[3].m_localPointA.Sub( ref m_pointCache[1].m_localPointA, out b2 );
btVector3 cross; a2.cross( ref b2, out cross );
res2 = cross.length2();
}
if( maxPenetrationIndex != 3 )
{
btVector3 a3; pt.m_localPointA.Sub( ref m_pointCache[0].m_localPointA, out a3 );
btVector3 b3; m_pointCache[2].m_localPointA.Sub( ref m_pointCache[1].m_localPointA, out b3 );
btVector3 cross; a3.cross( ref b3, out cross );
res3 = cross.length2();
}
}
else
{
if( maxPenetrationIndex != 0 )
{
res0 = calcArea4Points( ref pt.m_localPointA, ref m_pointCache[1].m_localPointA, ref m_pointCache[2].m_localPointA, ref m_pointCache[3].m_localPointA );
}
if( maxPenetrationIndex != 1 )
{
res1 = calcArea4Points( ref pt.m_localPointA, ref m_pointCache[0].m_localPointA, ref m_pointCache[2].m_localPointA, ref m_pointCache[3].m_localPointA );
}
if( maxPenetrationIndex != 2 )
{
res2 = calcArea4Points( ref pt.m_localPointA, ref m_pointCache[0].m_localPointA, ref m_pointCache[1].m_localPointA, ref m_pointCache[3].m_localPointA );
}
if( maxPenetrationIndex != 3 )
{
res3 = calcArea4Points( ref pt.m_localPointA, ref m_pointCache[0].m_localPointA, ref m_pointCache[1].m_localPointA, ref m_pointCache[2].m_localPointA );
}
}
btVector3 maxvec = new btVector3( res0, res1, res2, res3 );
return maxvec.closestAxis4();
}
