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; } }
/// 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(); }