public void convertContact( btPersistentManifold manifold, btContactSolverInfo infoGlobal )
{
btCollisionObject colObj0, colObj1;
colObj0 = manifold.m_body0;
colObj1 = manifold.m_body1;
//int solverBodyIdA = ;
//int solverBodyIdB = ;
// btRigidBody bodyA = btRigidBody::upcast(colObj0);
// btRigidBody bodyB = btRigidBody::upcast(colObj1);
btSolverBody solverBodyA = getOrInitSolverBody( colObj0, infoGlobal.m_timeStep );
btSolverBody solverBodyB = getOrInitSolverBody( colObj1, infoGlobal.m_timeStep );
///avoid collision response between two static objects
if( solverBodyA == null || ( solverBodyA.m_invMass.fuzzyZero() && ( solverBodyB == null || solverBodyB.m_invMass.fuzzyZero() ) ) )
return;
int rollingFriction = 1;
btScalar.Dbg( DbgFlag.Manifolds, "Manifold cache points " + manifold.m_cachedPoints );
for( int j = 0; j < manifold.m_cachedPoints; j++ )
{
btManifoldPoint cp = manifold.getContactPoint( j );
if( cp.m_distance1 <= manifold.m_contactProcessingThreshold )
{
btVector3 rel_pos1;
btVector3 rel_pos2;
double relaxation;
int frictionIndex = m_tmpSolverContactConstraintPool.Count;
btSolverConstraint solverConstraint = BulletGlobals.SolverConstraintPool.Get();
m_tmpSolverContactConstraintPool.Add( solverConstraint );
btRigidBody rb0 = btRigidBody.upcast( colObj0 );
btRigidBody rb1 = btRigidBody.upcast( colObj1 );
solverConstraint.m_solverBodyA = solverBodyA;
solverConstraint.m_solverBodyB = solverBodyB;
solverConstraint.m_originalContactPoint = cp;
//btVector3 pos1 = cp.m_positionWorldOnA;
//btVector3 pos2 = cp.m_positionWorldOnB;
cp.m_positionWorldOnA.Sub( ref colObj0.m_worldTransform.m_origin, out rel_pos1 );
cp.m_positionWorldOnB.Sub( ref colObj1.m_worldTransform.m_origin, out rel_pos2 );
btVector3 vel1;// = rb0 ? rb0.getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
btVector3 vel2;// = rb1 ? rb1.getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
solverBodyA.getVelocityInLocalPointNoDelta( ref rel_pos1, out vel1 );
solverBodyB.getVelocityInLocalPointNoDelta( ref rel_pos2, out vel2 );
btVector3 vel; vel1.Sub( ref vel2, out vel );
double rel_vel = cp.m_normalWorldOnB.dot( vel );
setupContactConstraint( solverConstraint, solverBodyA, solverBodyB, cp, infoGlobal, out relaxation, ref rel_pos1, ref rel_pos2 );
/////setup the friction constraints
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.Count;
btVector3 relAngVel;
if( rb0 != null && rb1 != null )
rb1.m_angularVelocity.Sub( ref rb0.m_angularVelocity, out relAngVel );
else if( rb0 != null )
rb0.m_angularVelocity.Invert( out relAngVel );
else if( rb1 != null )
relAngVel = rb1.m_angularVelocity;
else
relAngVel = btVector3.Zero;
if( ( cp.m_combinedRollingFriction > 0 ) && ( rollingFriction > 0 ) )
{
//only a single rollingFriction per manifold
rollingFriction--;
if( relAngVel.length() > infoGlobal.m_singleAxisRollingFrictionThreshold )
{
relAngVel.normalize();
applyAnisotropicFriction( colObj0, ref relAngVel, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_ROLLING_FRICTION );
applyAnisotropicFriction( colObj1, ref relAngVel, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_ROLLING_FRICTION );
if( relAngVel.length() > 0.001 )
addRollingFrictionConstraint( ref relAngVel, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
}
else
{
addRollingFrictionConstraint( ref cp.m_normalWorldOnB, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
btVector3 axis0, axis1;
btVector3.btPlaneSpace1( ref cp.m_normalWorldOnB, out axis0, out axis1 );
applyAnisotropicFriction( colObj0, ref axis0, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_ROLLING_FRICTION );
applyAnisotropicFriction( colObj1, ref axis0, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_ROLLING_FRICTION );
applyAnisotropicFriction( colObj0, ref axis1, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_ROLLING_FRICTION );
applyAnisotropicFriction( colObj1, ref axis1, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_ROLLING_FRICTION );
if( axis0.length() > 0.001 )
addRollingFrictionConstraint( ref axis0, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
if( axis1.length() > 0.001 )
addRollingFrictionConstraint( ref axis1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
}
}
///Bullet has several options to set the friction directions
///By default, each contact has only a single friction direction that is recomputed automatically very frame
///based on the relative linear velocity.
///If the relative velocity it zero, it will automatically compute a friction direction.
///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
///
///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
///
///The user can manually override the friction directions for certain contacts using a contact callback,
///and set the cp.m_lateralFrictionInitialized to true
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
///
if( ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_ENABLE_FRICTION_DIRECTION_CACHING ) == 0 )
|| !cp.m_lateralFrictionInitialized )
{
vel.AddScale( ref cp.m_normalWorldOnB, -rel_vel, out cp.m_lateralFrictionDir1 );
double lat_rel_vel = cp.m_lateralFrictionDir1.length2();
if( ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) == 0 ) && lat_rel_vel > btScalar.SIMD_EPSILON )
{
cp.m_lateralFrictionDir1.Mult( 1 / btScalar.btSqrt( lat_rel_vel ), out cp.m_lateralFrictionDir1 );
applyAnisotropicFriction( colObj0, ref cp.m_lateralFrictionDir1, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
applyAnisotropicFriction( colObj1, ref cp.m_lateralFrictionDir1, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
addFrictionConstraint( ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
{
cp.m_lateralFrictionDir1.cross( ref cp.m_normalWorldOnB, out cp.m_lateralFrictionDir2 );
cp.m_lateralFrictionDir2.normalize();//??
applyAnisotropicFriction( colObj0, ref cp.m_lateralFrictionDir2, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
applyAnisotropicFriction( colObj1, ref cp.m_lateralFrictionDir2, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
addFrictionConstraint( ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
}
}
else
{
btVector3.btPlaneSpace1( ref cp.m_normalWorldOnB, out cp.m_lateralFrictionDir1, out cp.m_lateralFrictionDir2 );
applyAnisotropicFriction( colObj0, ref cp.m_lateralFrictionDir1, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
applyAnisotropicFriction( colObj1, ref cp.m_lateralFrictionDir1, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
addFrictionConstraint( ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
{
applyAnisotropicFriction( colObj0, ref cp.m_lateralFrictionDir2, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
applyAnisotropicFriction( colObj1, ref cp.m_lateralFrictionDir2, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
addFrictionConstraint( ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
}
if( ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
&& ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) != 0 ) )
{
cp.m_lateralFrictionInitialized = true;
}
}
}
else
{
addFrictionConstraint( ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion1, cp.m_contactCFM1 );
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
addFrictionConstraint( ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2 );
}
setFrictionConstraintImpulse( solverConstraint, solverBodyA, solverBodyB, cp, infoGlobal );
}
}
}
