public override void addConstraint( btTypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies = false )
{
m_constraints.Add( constraint );
//Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?)
Debug.Assert( constraint.getRigidBodyA() != constraint.getRigidBodyB() );
if( disableCollisionsBetweenLinkedBodies )
{
constraint.m_rbA.addConstraintRef( constraint );
constraint.m_rbB.addConstraintRef( constraint );
}
}
public override void removeConstraint( btTypedConstraint constraint )
{
m_constraints.Remove( constraint );
constraint.m_rbA.removeConstraintRef( constraint );
constraint.m_rbB.removeConstraintRef( constraint );
}
public void setup( btContactSolverInfo solverInfo
, btTypedConstraint[] sortedConstraints, int numConstraints
, btIDebugDraw debugDrawer )
{
Debug.Assert( solverInfo != null );
m_solverInfo = solverInfo;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_bodies.Count = ( 0 );
m_manifolds.Count = ( 0 );
m_constraints.Count = ( 0 );
}
public static int btGetConstraintIslandId( btTypedConstraint lhs )
{
int islandId;
btCollisionObject rcolObj0 = lhs.getRigidBodyA();
btCollisionObject rcolObj1 = lhs.getRigidBodyB();
islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
return islandId;
}
static bool compare( btTypedConstraint lhs, btTypedConstraint rhs )
{
int rIslandId0, lIslandId0;
rIslandId0 = btDiscreteDynamicsWorld.btGetConstraintIslandId( rhs );
lIslandId0 = btDiscreteDynamicsWorld.btGetConstraintIslandId( lhs );
return lIslandId0 < rIslandId0;
}
internal void debugDrawConstraint( btTypedConstraint constraint )
{
bool drawFrames = ( getDebugDrawer().getDebugMode() & btIDebugDraw.DebugDrawModes.DBG_DrawConstraints ) != 0;
bool drawLimits = ( getDebugDrawer().getDebugMode() & btIDebugDraw.DebugDrawModes.DBG_DrawConstraintLimits ) != 0;
double dbgDrawSize = constraint.getDbgDrawSize();
if( dbgDrawSize <= (double)( 0 ) )
{
return;
}
switch( constraint.getConstraintType() )
{
case btObjectTypes.POINT2POINT_CONSTRAINT_TYPE:
{
btPoint2PointConstraint p2pC = (btPoint2PointConstraint)constraint;
btTransform tr = btTransform.Identity;
btVector3 pivot;
p2pC.getPivotInA( out pivot );
btVector3 tmp;
p2pC.getRigidBodyA().m_worldTransform.Apply( ref pivot, out tmp );
tr.setOrigin( ref tmp );
getDebugDrawer().drawTransform( ref tr, dbgDrawSize );
// that ideally should draw the same frame
p2pC.getPivotInB( out pivot );
p2pC.getRigidBodyB().m_worldTransform.Apply( ref pivot, out tmp );
tr.setOrigin( ref tmp );
if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize );
}
break;
case btObjectTypes.HINGE_CONSTRAINT_TYPE:
{
btHingeConstraint pHinge = (btHingeConstraint)constraint;
btTransform tr; pHinge.getRigidBodyA().m_worldTransform.Apply( ref pHinge.m_rbAFrame, out tr );
if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize );
pHinge.getRigidBodyB().m_worldTransform.Apply( ref pHinge.m_rbBFrame, out tr );
if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize );
double minAng = pHinge.getLowerLimit();
double maxAng = pHinge.getUpperLimit();
if( minAng == maxAng )
{
break;
}
bool drawSect = true;
if( !pHinge.hasLimit() )
{
minAng = (double)( 0 );
maxAng = btScalar.SIMD_2_PI;
drawSect = false;
}
if( drawLimits )
{
btVector3 center = tr.m_origin;
btVector3 normal; tr.m_basis.getColumn( 2, out normal );
btVector3 axis; tr.m_basis.getColumn( 0, out axis );
getDebugDrawer().drawArc( ref tr.m_origin, ref normal, ref axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, ref btVector3.Zero, drawSect );
}
}
break;
case btObjectTypes.CONETWIST_CONSTRAINT_TYPE:
{
btConeTwistConstraint pCT = (btConeTwistConstraint)constraint;
btTransform tr; pCT.getRigidBodyA().m_worldTransform.Apply( ref pCT.m_rbAFrame, out tr );
if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize );
pCT.m_rbB.m_worldTransform.Apply( ref pCT.m_rbBFrame, out tr );
if( drawFrames ) getDebugDrawer().drawTransform( ref tr, dbgDrawSize );
if( drawLimits )
{
//double length = (double)(5);
double length = dbgDrawSize;
int nSegments = 8 * 4;
double fAngleInRadians = btScalar.SIMD_2_PI * (double)( nSegments - 1 ) / (double)( nSegments );
btVector3 tmp;
btVector3 pPrev; pCT.GetPointForAngle( fAngleInRadians, length, out tmp );
tr.Apply( ref tmp, out pPrev );
for( int i = 0; i < nSegments; i++ )
{
fAngleInRadians = btScalar.SIMD_2_PI * (double)i / (double)( nSegments );
btVector3 pCur; pCT.GetPointForAngle( fAngleInRadians, length, out pCur );
//btVector3 tmp;
tr.Apply( ref pCur, out tmp );
getDebugDrawer().drawLine( ref pPrev, ref tmp, ref btVector3.Zero );
if( i % ( nSegments / 8 ) == 0 )
getDebugDrawer().drawLine( ref tr.m_origin, ref pCur, ref btVector3.Zero );
pPrev = pCur;
}
double tws = pCT.getTwistSpan();
double twa = pCT.getTwistAngle();
bool useFrameB = ( pCT.getRigidBodyB().getInvMass() > (double)( 0 ) );
if( useFrameB )
{
pCT.getRigidBodyB().m_worldTransform.Apply( ref pCT.m_rbBFrame, out tr );
}
else
{
pCT.getRigidBodyA().m_worldTransform.Apply( ref pCT.m_rbBFrame, out tr );
}
btVector3 pivot = tr.m_origin;
btVector3 normal; tr.m_basis.getColumn( 0, out normal );
btVector3 axis1; tr.m_basis.getColumn( 1, out axis1 );
getDebugDrawer().drawArc( ref pivot, ref normal, ref axis1, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, ref btVector3.Zero, true );
}
}
break;
case btObjectTypes.D6_SPRING_CONSTRAINT_TYPE:
case btObjectTypes.D6_CONSTRAINT_TYPE:
{
btGeneric6DofConstraint p6DOF = (btGeneric6DofConstraint)constraint;
btTransform tr;// = p6DOF.getCalculatedTransformA();
if( drawFrames ) getDebugDrawer().drawTransform( ref p6DOF.m_calculatedTransformA, dbgDrawSize );
//tr = p6DOF.getCalculatedTransformB();
if( drawFrames ) getDebugDrawer().drawTransform( ref p6DOF.m_calculatedTransformB, dbgDrawSize );
if( drawLimits )
{
tr = p6DOF.m_calculatedTransformA;
btVector3 center = p6DOF.m_calculatedTransformB.m_origin;
btVector3 up = tr.m_basis.getColumn( 2 );
btVector3 axis = tr.m_basis.getColumn( 0 );
double minTh = p6DOF.getRotationalLimitMotor( 1 ).m_loLimit;
double maxTh = p6DOF.getRotationalLimitMotor( 1 ).m_hiLimit;
double minPs = p6DOF.getRotationalLimitMotor( 2 ).m_loLimit;
double maxPs = p6DOF.getRotationalLimitMotor( 2 ).m_hiLimit;
getDebugDrawer().drawSpherePatch( ref center, ref up, ref axis, dbgDrawSize * (double)( .9f ), minTh, maxTh, minPs, maxPs, ref btVector3.Zero );
axis = tr.m_basis.getColumn( 1 );
double ay = p6DOF.getAngle( 1 );
double az = p6DOF.getAngle( 2 );
double cy = btScalar.btCos( ay );
double sy = btScalar.btSin( ay );
double cz = btScalar.btCos( az );
double sz = btScalar.btSin( az );
btVector3 ref_point;
ref_point.x = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2];
ref_point.y = -sz * axis[0] + cz * axis[1];
ref_point.z = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2];
ref_point.w = 0;
tr = p6DOF.m_calculatedTransformB;
btVector3 normal; tr.m_basis.getColumn( 0, out normal );
normal.Invert( out normal );
double minFi = p6DOF.getRotationalLimitMotor( 0 ).m_loLimit;
double maxFi = p6DOF.getRotationalLimitMotor( 0 ).m_hiLimit;
if( minFi > maxFi )
{
getDebugDrawer().drawArc( ref center, ref normal, ref ref_point, dbgDrawSize, dbgDrawSize, -btScalar.SIMD_PI, btScalar.SIMD_PI, ref btVector3.Zero, false );
}
else if( minFi < maxFi )
{
getDebugDrawer().drawArc( ref center, ref normal, ref ref_point, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref btVector3.Zero, true );
}
tr = p6DOF.m_calculatedTransformA;
btVector3 bbMin = p6DOF.getTranslationalLimitMotor().m_lowerLimit;
btVector3 bbMax = p6DOF.getTranslationalLimitMotor().m_upperLimit;
getDebugDrawer().drawBox( ref bbMin, ref bbMax, ref tr, ref btVector3.Zero );
}
}
break;
///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage
case btObjectTypes.D6_SPRING_2_CONSTRAINT_TYPE:
{
{
btGeneric6DofSpring2Constraint p6DOF = (btGeneric6DofSpring2Constraint)constraint;
//btTransform tr;// = p6DOF.getCalculatedTransformA();
if( drawFrames ) getDebugDrawer().drawTransform( ref p6DOF.m_calculatedTransformA, dbgDrawSize );
//tr = p6DOF.getCalculatedTransformB();
if( drawFrames ) getDebugDrawer().drawTransform( ref p6DOF.m_calculatedTransformB, dbgDrawSize );
if( drawLimits )
{
//tr = p6DOF.getCalculatedTransformA();
btVector3 center = p6DOF.m_calculatedTransformB.m_origin;
btVector3 up = p6DOF.m_calculatedTransformA.m_basis.getColumn( 2 );
btVector3 axis = p6DOF.m_calculatedTransformA.m_basis.getColumn( 0 );
double minTh = p6DOF.getRotationalLimitMotor( 1 ).m_loLimit;
double maxTh = p6DOF.getRotationalLimitMotor( 1 ).m_hiLimit;
double minPs = p6DOF.getRotationalLimitMotor( 2 ).m_loLimit;
double maxPs = p6DOF.getRotationalLimitMotor( 2 ).m_hiLimit;
getDebugDrawer().drawSpherePatch( ref center, ref up, ref axis, dbgDrawSize * (double)( .9f ), minTh, maxTh, minPs, maxPs, ref btVector3.Zero );
axis = p6DOF.m_calculatedTransformA.m_basis.getColumn( 1 );
double ay = p6DOF.getAngle( 1 );
double az = p6DOF.getAngle( 2 );
double cy = btScalar.btCos( ay );
double sy = btScalar.btSin( ay );
double cz = btScalar.btCos( az );
double sz = btScalar.btSin( az );
btVector3 ref_point;
ref_point.x = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2];
ref_point.y = -sz * axis[0] + cz * axis[1];
ref_point.z = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2];
ref_point.w = 0;
//tr = p6DOF.getCalculatedTransformB();
btVector3 normal; p6DOF.m_calculatedTransformB.m_basis.getColumn( 0, out normal );
normal.Invert( out normal );
double minFi = p6DOF.getRotationalLimitMotor( 0 ).m_loLimit;
double maxFi = p6DOF.getRotationalLimitMotor( 0 ).m_hiLimit;
if( minFi > maxFi )
{
getDebugDrawer().drawArc( ref center, ref normal, ref ref_point, dbgDrawSize, dbgDrawSize, -btScalar.SIMD_PI, btScalar.SIMD_PI, ref btVector3.Zero, false );
}
else if( minFi < maxFi )
{
getDebugDrawer().drawArc( ref center, ref normal, ref ref_point, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref btVector3.Zero, true );
}
//tr = p6DOF.getCalculatedTransformA();
btVector3 bbMin = p6DOF.getTranslationalLimitMotor().m_lowerLimit;
btVector3 bbMax = p6DOF.getTranslationalLimitMotor().m_upperLimit;
getDebugDrawer().drawBox( ref bbMin, ref bbMax, ref p6DOF.m_calculatedTransformA, ref btVector3.Zero );
}
}
break;
}
case btObjectTypes.SLIDER_CONSTRAINT_TYPE:
{
btSliderConstraint pSlider = (btSliderConstraint)constraint;
//btTransform tr = pSlider.m_calculatedTransformA;
if( drawFrames ) getDebugDrawer().drawTransform( ref pSlider.m_calculatedTransformA, dbgDrawSize );
//tr = pSlider.getCalculatedTransformB();
if( drawFrames ) getDebugDrawer().drawTransform( ref pSlider.m_calculatedTransformB, dbgDrawSize );
if( drawLimits )
{
btTransform tr = pSlider.getUseLinearReferenceFrameA()
? pSlider.m_calculatedTransformA
: pSlider.m_calculatedTransformB;
btVector3 tmp = new btVector3( pSlider.getLowerLinLimit(), 0, 0 );
btVector3 li_min; tr.Apply( ref tmp, out li_min );
tmp = new btVector3( pSlider.getUpperLinLimit(), 0, 0 );
btVector3 li_max; tr.Apply( ref tmp, out li_max );
getDebugDrawer().drawLine( ref li_min, ref li_max, ref btVector3.Zero );
btVector3 normal = tr.m_basis.getColumn( 0 );
btVector3 axis = tr.m_basis.getColumn( 1 );
double a_min = pSlider.getLowerAngLimit();
double a_max = pSlider.getUpperAngLimit();
btVector3 center = pSlider.m_calculatedTransformB.m_origin;
getDebugDrawer().drawArc( ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, a_min, a_max, ref btVector3.Zero, true );
}
}
break;
default:
break;
}
return;
}
public virtual void addConstraint( btTypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies = false )
{
//(void)constraint; (void)disableCollisionsBetweenLinkedBodies;
}
protected virtual double solveGroupCacheFriendlySetup( btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer )
{
if( m_fixedBody != null )
{
BulletGlobals.SolverBodyPool.Free( m_fixedBody );
m_fixedBody = null;
}
//m_fixedBodyId = -1;
CProfileSample sample = new CProfileSample( "solveGroupCacheFriendlySetup" );
//(void)debugDrawer;
m_maxOverrideNumSolverIterations = 0;
#if BT_ADDITIONAL_DEBUG
//make sure that dynamic bodies exist for all (enabled)raints
for (int i=0;i<numConstraints;i++)
{
btTypedConstraint* constraint = constraints[i];
if (constraint.isEnabled())
{
if (!constraint.getRigidBodyA().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (&constraint.getRigidBodyA()==bodies[b])
{
found = true;
break;
}
}
Debug.Assert(found);
}
if (!constraint.getRigidBodyB().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (&constraint.getRigidBodyB()==bodies[b])
{
found = true;
break;
}
}
Debug.Assert(found);
}
}
}
//make sure that dynamic bodies exist for all contact manifolds
for (int i=0;i<numManifolds;i++)
{
if (!manifoldPtr[i].getBody0().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (manifoldPtr[i].getBody0()==bodies[b])
{
found = true;
break;
}
}
Debug.Assert(found);
}
if (!manifoldPtr[i].getBody1().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (manifoldPtr[i].getBody1()==bodies[b])
{
found = true;
break;
}
}
Debug.Assert(found);
}
}
#endif //BT_ADDITIONAL_DEBUG
for( int i = 0; i < numBodies; i++ )
{
bodies[i].setCompanionBody( null );
}
m_tmpSolverBodyPool.Capacity = ( numBodies + 1 );
m_tmpSolverBodyPool.Count = ( 0 );
//btSolverBody fixedBody = m_tmpSolverBodyPool.expand();
//initSolverBody(&fixedBody,0);
//convert all bodies
for( int i = 0; i < numBodies; i++ )
{
//int bodyId = getOrInitSolverBody( bodies[i], infoGlobal.m_timeStep );
btRigidBody body = btRigidBody.upcast( bodies[i] );
if( body != null && body.getInvMass() != 0 )
{
btSolverBody solverBody = getOrInitSolverBody( bodies[i], infoGlobal.m_timeStep );
btVector3 gyroForce = btVector3.Zero;
if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT ) != 0 )
{
body.computeGyroscopicForceExplicit( infoGlobal.m_maxGyroscopicForce, out gyroForce );
btVector3 tmp;
body.m_invInertiaTensorWorld.ApplyInverse( ref gyroForce, out tmp );
//solverBody.m_externalTorqueImpulse -= gyroForce * body.m_invInertiaTensorWorld * infoGlobal.m_timeStep;
solverBody.m_externalTorqueImpulse.SubScale( ref tmp, infoGlobal.m_timeStep, out solverBody.m_externalTorqueImpulse );
}
if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD ) != 0 )
{
body.computeGyroscopicImpulseImplicit_World( infoGlobal.m_timeStep, out gyroForce );
solverBody.m_externalTorqueImpulse.Add( ref gyroForce, out solverBody.m_externalTorqueImpulse );
}
if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY ) != 0 )
{
body.computeGyroscopicImpulseImplicit_Body( infoGlobal.m_timeStep, out gyroForce );
btScalar.Dbg( "Gyroforce " + gyroForce );
solverBody.m_externalTorqueImpulse.Add( ref gyroForce, out solverBody.m_externalTorqueImpulse );
}
}
}
if( true )
{
int j;
for( j = 0; j < numConstraints; j++ )
{
btTypedConstraint constraint = constraints[j + startConstraint];
constraint.buildJacobian();
constraint.internalSetAppliedImpulse( 0.0f );
}
}
//btRigidBody rb0=0,*rb1=0;
//if (1)
{
{
int totalNumRows = 0;
int i;
m_tmpConstraintSizesPool.Capacity = ( numConstraints );
//calculate the total number of contraint rows
for( i = 0; i < numConstraints; i++ )
{
int infoNumConstraintRows = m_tmpConstraintSizesPool[i].m_numConstraintRows;
//btTypedConstraint.btConstraintInfo1 info1 = m_tmpConstraintSizesPool[i];
btTypedConstraint.btJointFeedback fb = constraints[i + startConstraint].getJointFeedback();
if( fb != null )
{
fb.m_appliedForceBodyA.setZero();
fb.m_appliedTorqueBodyA.setZero();
fb.m_appliedForceBodyB.setZero();
fb.m_appliedTorqueBodyB.setZero();
}
if( constraints[i + startConstraint].isEnabled() )
{
constraints[i + startConstraint].getInfo1( ref m_tmpConstraintSizesPool.InternalArray[i] );
}
else
{
m_tmpConstraintSizesPool.InternalArray[i].m_numConstraintRows = 0;
m_tmpConstraintSizesPool.InternalArray[i].nub = 0;
}
totalNumRows += m_tmpConstraintSizesPool.InternalArray[i].m_numConstraintRows;
}
m_tmpSolverNonContactConstraintPool.Count = ( totalNumRows );
for( i = 0; i < totalNumRows; i++ )
m_tmpSolverNonContactConstraintPool[i] = BulletGlobals.SolverConstraintPool.Get();
///setup the btSolverConstraints
int currentRow = 0;
for( i = 0; i < numConstraints; i++ )
{
int infoConstraintRows = m_tmpConstraintSizesPool[i].m_numConstraintRows;
if( infoConstraintRows != 0 )
{
Debug.Assert( currentRow < totalNumRows );
btSolverConstraint currentConstraintRow = m_tmpSolverNonContactConstraintPool[currentRow];
btTypedConstraint constraint = constraints[i + startConstraint];
btRigidBody rbA = constraint.getRigidBodyA();
btRigidBody rbB = constraint.getRigidBodyB();
//int solverBodyIdA = ;
//int solverBodyIdB = ;
btSolverBody bodyAPtr = getOrInitSolverBody( rbA, infoGlobal.m_timeStep );
btSolverBody bodyBPtr = getOrInitSolverBody( rbB, infoGlobal.m_timeStep );
int overrideNumSolverIterations = constraint.getOverrideNumSolverIterations() > 0 ? constraint.getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
if( overrideNumSolverIterations > m_maxOverrideNumSolverIterations )
m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
int j;
for( j = 0; j < infoConstraintRows; j++ )
{
btSolverConstraint current = m_tmpSolverNonContactConstraintPool[currentRow + j];
current.Clear();
//memset( ¤tConstraintRow[j], 0, sizeof( btSolverConstraint ) );
current.m_lowerLimit = btScalar.SIMD_NEG_INFINITY;
current.m_upperLimit = btScalar.SIMD_INFINITY;
current.m_appliedImpulse = 0;
current.m_appliedPushImpulse = 0;
current.m_solverBodyA = bodyAPtr;
current.m_solverBodyB = bodyBPtr;
current.m_overrideNumSolverIterations = overrideNumSolverIterations;
}
bodyAPtr.Clear();
btTypedConstraint.btConstraintInfo2 info2 = m_tmpConstraintInfo2Pool.Get();// new btTypedConstraint.btConstraintInfo2();
info2.m_numRows = infoConstraintRows;
for( j = 0; j < infoConstraintRows; ++j )
{
info2.m_solverConstraints[j] = m_tmpSolverNonContactConstraintPool[currentRow + j];
}
info2.fps = 1 / infoGlobal.m_timeStep;
info2.erp = infoGlobal.m_erp;
#if OLD_CONSTRAINT_INFO_INIT
info2.m_J1linearAxis = currentConstraintRow.m_contactNormal1;
info2.m_J1angularAxis = currentConstraintRow.m_relpos1CrossNormal;
info2.m_J2linearAxis = currentConstraintRow.m_contactNormal2;
info2.m_J2angularAxis = currentConstraintRow.m_relpos2CrossNormal;
info2.rowskip = 0;// sizeof( btSolverConstraint ) / sizeof( double );//check this
///the size of btSolverConstraint needs be a multiple of double
//Debug.Assert( info2.rowskip * sizeof( double ) == sizeof( btSolverConstraint ) );
info2.m_constraintError = currentConstraintRow.m_rhs;
info2.cfm = currentConstraintRow.m_cfm;
info2.m_lowerLimit = currentConstraintRow.m_lowerLimit;
info2.m_upperLimit = currentConstraintRow.m_upperLimit;
#endif
currentConstraintRow.m_cfm = infoGlobal.m_globalCfm;
info2.m_damping = infoGlobal.m_damping;
info2.m_numIterations = infoGlobal.m_numIterations;
constraint.getInfo2( info2 );
///finalize the constraint setup
for( j = 0; j < infoConstraintRows; j++ )
{
btSolverConstraint solverConstraint = m_tmpSolverNonContactConstraintPool[currentRow + j];
if( solverConstraint.m_upperLimit >= constraint.getBreakingImpulseThreshold() )
{
solverConstraint.m_upperLimit = constraint.getBreakingImpulseThreshold();
}
if( solverConstraint.m_lowerLimit <= -constraint.getBreakingImpulseThreshold() )
{
solverConstraint.m_lowerLimit = -constraint.getBreakingImpulseThreshold();
}
solverConstraint.m_originalContactPoint = constraint;
btVector3 tmp;
{
//solverConstraint.m_angularComponentA = constraint.getRigidBodyA().m_invInertiaTensorWorld
// *solverConstraint.m_relpos1CrossNormal * constraint.getRigidBodyA().getAngularFactor();
constraint.m_rbA.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp );
tmp.Mult( ref constraint.m_rbA.m_angularFactor, out solverConstraint.m_angularComponentA );
}
{
//solverConstraint.m_angularComponentB = constraint.getRigidBodyB().m_invInertiaTensorWorld
// * solverConstraint.m_relpos2CrossNormal * constraint.getRigidBodyB().getAngularFactor();
constraint.m_rbB.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp );
tmp.Mult( ref constraint.m_rbB.m_angularFactor, out solverConstraint.m_angularComponentB );
}
{
btVector3 iMJlA; solverConstraint.m_contactNormal1.Mult( rbA.m_inverseMass, out iMJlA );
btVector3 iMJaA; rbA.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out iMJaA );
btVector3 iMJlB; solverConstraint.m_contactNormal2.Mult( rbB.m_inverseMass, out iMJlB );//sign of normal?
btVector3 iMJaB; rbB.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out iMJaB );
double sum = iMJlA.dot( ref solverConstraint.m_contactNormal1 );
sum += iMJaA.dot( ref solverConstraint.m_relpos1CrossNormal );
sum += iMJlB.dot( ref solverConstraint.m_contactNormal2 );
sum += iMJaB.dot( ref solverConstraint.m_relpos2CrossNormal );
double fsum = btScalar.btFabs( sum );
Debug.Assert( fsum > btScalar.SIMD_EPSILON );
btScalar.Dbg( "m_jacDiagABInv 4 set to " + ( fsum > btScalar.SIMD_EPSILON ? btScalar.BT_ONE / sum : 0 ).ToString( "g17" ) );
solverConstraint.m_jacDiagABInv = fsum > btScalar.SIMD_EPSILON ? btScalar.BT_ONE / sum : 0;
}
{
double rel_vel;
btVector3 externalForceImpulseA = bodyAPtr.m_originalBody != null ? bodyAPtr.m_externalForceImpulse : btVector3.Zero;
btVector3 externalTorqueImpulseA = bodyAPtr.m_originalBody != null ? bodyAPtr.m_externalTorqueImpulse : btVector3.Zero;
btVector3 externalForceImpulseB = bodyBPtr.m_originalBody != null ? bodyBPtr.m_externalForceImpulse : btVector3.Zero;
btVector3 externalTorqueImpulseB = bodyBPtr.m_originalBody != null ? bodyBPtr.m_externalTorqueImpulse : btVector3.Zero;
btScalar.Dbg( "external torque2 impulses " + externalTorqueImpulseA + externalTorqueImpulseB );
double vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref rbA.m_linearVelocity, ref externalForceImpulseA )
+ solverConstraint.m_relpos1CrossNormal.dotAdded( ref rbA.m_angularVelocity, ref externalTorqueImpulseA );
double vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref rbB.m_linearVelocity, ref externalForceImpulseB )
+ solverConstraint.m_relpos2CrossNormal.dotAdded( ref rbB.m_angularVelocity, ref externalTorqueImpulseB );
rel_vel = vel1Dotn + vel2Dotn;
double restitution = 0;
double positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
double velocityError = restitution - rel_vel * info2.m_damping;
double penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
btScalar.Dbg( "Constraint 5 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_appliedImpulse = 0;
}
}
}
currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows;
}
}
btScalar.Dbg( "About to convert contacts " + start_manifold + " " + numManifolds );
convertContacts( manifoldPtr, start_manifold, numManifolds, infoGlobal );
}
// btContactSolverInfo info = infoGlobal;
int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count;
int numConstraintPool = m_tmpSolverContactConstraintPool.Count;
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count;
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
m_orderNonContactConstraintPool.Capacity = ( numNonContactPool );
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
m_orderTmpConstraintPool.Count = m_orderTmpConstraintPool.Capacity = ( numConstraintPool * 2 );
else
m_orderTmpConstraintPool.Count = m_orderTmpConstraintPool.Capacity = ( numConstraintPool );
m_orderFrictionConstraintPool.Count = m_orderFrictionConstraintPool.Capacity = ( numFrictionPool );
{
int i;
for( i = 0; i < numNonContactPool; i++ )
{
m_orderNonContactConstraintPool[i] = i;
}
for( i = 0; i < numConstraintPool; i++ )
{
m_orderTmpConstraintPool[i] = i;
}
for( i = 0; i < numFrictionPool; i++ )
{
m_orderFrictionConstraintPool[i] = i;
}
}
return 0;
}
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
internal override double solveGroup( btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints
, btContactSolverInfo infoGlobal
, btIDebugDraw debugDrawer, btDispatcher dispatcher )
{
CProfileSample sample = new CProfileSample( "solveGroup" );
//you need to provide at least some bodies
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer );
solveGroupCacheFriendlyIterations( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer );
solveGroupCacheFriendlyFinish( bodies, numBodies, infoGlobal );
return 0;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
public override void setParam( btTypedConstraint.btConstraintParams num, double value, int axis = -1 )
{
if( ( axis == -1 ) || ( axis == 5 ) )
{
switch( num )
{
case btConstraintParams.BT_CONSTRAINT_STOP_ERP:
m_stopERP = value;
m_flags |= btHingeFlags.BT_HINGE_FLAGS_ERP_STOP;
break;
case btConstraintParams.BT_CONSTRAINT_STOP_CFM:
m_stopCFM = value;
m_flags |= btHingeFlags.BT_HINGE_FLAGS_CFM_STOP;
break;
case btConstraintParams.BT_CONSTRAINT_CFM:
m_normalCFM = value;
m_flags |= btHingeFlags.BT_HINGE_FLAGS_CFM_NORM;
break;
case btConstraintParams.BT_CONSTRAINT_ERP:
m_normalERP = value;
m_flags |= btHingeFlags.BT_HINGE_FLAGS_ERP_NORM;
break;
default:
btAssertConstrParams( false );
break;
}
}
else
{
btAssertConstrParams( false );
}
}
protected virtual double solveGroupCacheFriendlyIterations( btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints
, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer )
{
CProfileSample sample = new CProfileSample( "solveGroupCacheFriendlyIterations" );
{
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer );
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
for( int iteration = 0; iteration < maxIterations; iteration++ )
//for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
{
solveSingleIteration( iteration, bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer );
}
}
return 0;
}
protected virtual void solveGroupCacheFriendlySplitImpulseIterations( btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer )
{
int iteration;
if( infoGlobal.m_splitImpulse )
{
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_SIMD ) != 0 )
{
for( iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ )
{
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
int j;
for( j = 0; j < numPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSplitPenetrationSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
}
}
else
{
for( iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ )
{
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
int j;
for( j = 0; j < numPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSplitPenetrationImpulseCacheFriendly( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
}
}
}
}
protected virtual double solveSingleIteration( int iteration, btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints
, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer )
{
int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count;
int numConstraintPool = m_tmpSolverContactConstraintPool.Count;
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count;
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_RANDMIZE_ORDER ) != 0 )
{
if( true ) // uncomment this for a bit less random ((iteration & 7) == 0)
{
for( int j = 0; j < numNonContactPool; ++j )
{
int tmp = m_orderNonContactConstraintPool[j];
int swapi = btRandInt2( j + 1 );
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
m_orderNonContactConstraintPool[swapi] = tmp;
}
//contact/friction constraints are not solved more than
if( iteration < infoGlobal.m_numIterations )
{
for( int j = 0; j < numConstraintPool; ++j )
{
int tmp = m_orderTmpConstraintPool[j];
int swapi = btRandInt2( j + 1 );
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
m_orderTmpConstraintPool[swapi] = tmp;
}
for( int j = 0; j < numFrictionPool; ++j )
{
int tmp = m_orderFrictionConstraintPool[j];
int swapi = btRandInt2( j + 1 );
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
m_orderFrictionConstraintPool[swapi] = tmp;
}
}
}
}
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_SIMD ) != 0 )
{
///solve all joint constraints, using SIMD, if available
for( int j = 0; j < m_tmpSolverNonContactConstraintPool.Count; j++ )
{
btSolverConstraint constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if( iteration < constraint.m_overrideNumSolverIterations )
resolveSingleConstraintRowGenericSIMD( constraint.m_solverBodyA
, constraint.m_solverBodyB, constraint );
}
if( iteration < infoGlobal.m_numIterations )
{
for( int j = 0; j < numConstraints; j++ )
{
btTypedConstraint constraint = constraints[j + startConstraint];
if( constraint.isEnabled() )
{
btSolverBody bodyA = getOrInitSolverBody( constraint.getRigidBodyA(), infoGlobal.m_timeStep );
btSolverBody bodyB = getOrInitSolverBody( constraint.getRigidBodyB(), infoGlobal.m_timeStep );
constraint.solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep );
}
}
///solve all contact constraints using SIMD, if available
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS ) != 0 )
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
int multiplier = ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 ) ? 2 : 1;
for( int c = 0; c < numPoolConstraints; c++ )
{
double totalImpulse = 0;
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
resolveSingleConstraintRowLowerLimitSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
totalImpulse = solveManifold.m_appliedImpulse;
}
bool applyFriction = true;
if( applyFriction )
{
{
btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
if( totalImpulse > (double)( 0 ) )
{
solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse );
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
{
btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
if( totalImpulse > (double)( 0 ) )
{
solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse );
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
}
}
}
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
{
//solve the friction constraints after all contact constraints, don't interleave them
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
int j;
for( j = 0; j < numPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
//resolveSingleConstraintRowLowerLimitSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
gResolveSingleConstraintRowLowerLimit_scalar_reference( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
///solve all friction constraints, using SIMD, if available
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.Count;
for( j = 0; j < numFrictionPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
double totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if( totalImpulse > (double)( 0 ) )
{
solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse );
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
btScalar.Dbg( "PreFriction Impulse?" + solveManifold.m_appliedImpulse.ToString( "g17" ) );
gResolveSingleConstraintRowGeneric_scalar_reference( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
//resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
btScalar.Dbg( "Friction Impulse?" + solveManifold.m_appliedImpulse.ToString( "g17" ) );
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.Count;
for( j = 0; j < numRollingFrictionPoolConstraints; j++ )
{
btSolverConstraint rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
double totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if( totalImpulse > (double)( 0 ) )
{
double rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
if( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction )
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
gResolveSingleConstraintRowGeneric_scalar_reference( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint );
//resolveSingleConstraintRowGenericSIMD( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint );
}
}
}
}
}
else
{
//non-SIMD version
///solve all joint constraints
for( int j = 0; j < m_tmpSolverNonContactConstraintPool.Count; j++ )
{
btSolverConstraint constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if( iteration < constraint.m_overrideNumSolverIterations )
resolveSingleConstraintRowGeneric( constraint.m_solverBodyA, constraint.m_solverBodyB, constraint );
}
if( iteration < infoGlobal.m_numIterations )
{
for( int j = 0; j < numConstraints; j++ )
{
btTypedConstraint constraint = constraints[j + startConstraint];
if( constraint.isEnabled() )
{
//int bodyAid = ;
//int bodyBid = ;
btSolverBody bodyA = getOrInitSolverBody( constraint.getRigidBodyA(), infoGlobal.m_timeStep );
btSolverBody bodyB = getOrInitSolverBody( constraint.getRigidBodyB(), infoGlobal.m_timeStep );
constraint.solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep );
}
}
///solve all contact constraints
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
for( int j = 0; j < numPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRowLowerLimit( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.Count;
for( int j = 0; j < numFrictionPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
double totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if( totalImpulse > (double)( 0 ) )
{
solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse );
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
resolveSingleConstraintRowGeneric( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.Count;
for( int j = 0; j < numRollingFrictionPoolConstraints; j++ )
{
btSolverConstraint rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
double totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if( totalImpulse > (double)( 0 ) )
{
double rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
if( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction )
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
resolveSingleConstraintRowGeneric( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint );
}
}
}
}
return 0;
}
internal void addConstraintRef( btTypedConstraint c )
{
///disable collision with the 'other' body
int index = m_constraintRefs.IndexOf( c );
//don't add constraints that are already referenced
//Debug.Assert(index == m_constraintRefs.Count);
if( index == -1 )
{
m_constraintRefs.Add( c );
btCollisionObject colObjA = c.getRigidBodyA();
btCollisionObject colObjB = c.getRigidBodyB();
if( colObjA == this )
{
colObjA.setIgnoreCollisionCheck( colObjB, true );
}
else
{
colObjB.setIgnoreCollisionCheck( colObjA, true );
}
}
}
public virtual void removeConstraint( btTypedConstraint constraint ) { }
internal void removeConstraintRef( btTypedConstraint c )
{
int index = m_constraintRefs.IndexOf( c );
//don't remove constraints that are not referenced
if( index != -1 )
{
m_constraintRefs.Remove( c );
btCollisionObject colObjA = c.getRigidBodyA();
btCollisionObject colObjB = c.getRigidBodyB();
if( colObjA == this )
{
colObjA.setIgnoreCollisionCheck( colObjB, false );
}
else
{
colObjB.setIgnoreCollisionCheck( colObjA, false );
}
}
}
///solve a group of constraints internal abstract double solveGroup( btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifold, int first_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints , btContactSolverInfo info , btIDebugDraw debugDrawer , btDispatcher dispatcher );
