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 ); }
internal void solveConstraints( btContactSolverInfo solverInfo ) { CProfileSample sample = new CProfileSample( "solveConstraints" ); if( m_constraints.Count > 0 ) { if( m_sortedConstraints.Count < m_constraints.Count ) m_sortedConstraints.Count = m_constraints.Count; m_sortedConstraints[m_constraints.Count - 1] = null; //m_sortedConstraints.resize( m_constraints.Count ); int i; for( i = 0; i < m_constraints.Count; i++ ) { m_sortedConstraints[i] = m_constraints[i]; } m_sortedConstraints.quickSort( compare ); } // Debug.Assert(false); btTypedConstraint[] constraintsPtr = ( getNumConstraints() != 0 ) ? m_sortedConstraints.InternalArray : null; m_solverIslandCallback.setup( solverInfo, constraintsPtr, m_sortedConstraints.Count, getDebugDrawer() ); m_constraintSolver.prepareSolve( getNumCollisionObjects(), m_dispatcher1.getNumManifolds() ); /// solve all the constraints for this island m_islandManager.buildAndProcessIslands( m_dispatcher1, this, m_solverIslandCallback ); m_solverIslandCallback.processConstraints(); m_constraintSolver.allSolved( solverInfo, m_debugDrawer ); }
internal InplaceSolverIslandCallback( btConstraintSolver solver, btDispatcher dispatcher ) { m_solverInfo = null; m_solver = ( solver ); m_sortedConstraints = null; m_numConstraints = ( 0 ); m_debugDrawer = null; m_dispatcher = ( dispatcher ); }
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; } } }
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 ); } } }
internal override void allSolved( btContactSolverInfo info, btIDebugDraw debugDrawer ) {; }
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; } }
protected virtual double solveGroupCacheFriendlyFinish( btCollisionObject[] bodies, int numBodies, btContactSolverInfo infoGlobal ) { int numPoolConstraints = m_tmpSolverContactConstraintPool.Count; int i, j; if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 ) { for( j = 0; j < numPoolConstraints; j++ ) { btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[j]; btManifoldPoint pt = solveManifold.m_originalContactPoint as btManifoldPoint; Debug.Assert( pt != null ); pt.m_appliedImpulse = solveManifold.m_appliedImpulse; // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; // Console.WriteLine("pt.m_appliedImpulseLateral1 = %f\n", f); pt.m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; btScalar.Dbg( "New manifold source is " + pt.m_appliedImpulseLateral1.ToString( "g17" ) + " from " + solveManifold.m_frictionIndex ); //Console.WriteLine("pt.m_appliedImpulseLateral1 = %f\n", pt.m_appliedImpulseLateral1); if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 ) { pt.m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse; } //do a callback here? } } numPoolConstraints = m_tmpSolverNonContactConstraintPool.Count; for( j = 0; j < numPoolConstraints; j++ ) { btSolverConstraint solverConstr = m_tmpSolverNonContactConstraintPool[j]; btTypedConstraint constr = (btTypedConstraint)solverConstr.m_originalContactPoint; btTypedConstraint.btJointFeedback fb = constr.getJointFeedback(); if( fb != null ) { double scalar = solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; btVector3 tmp; solverConstr.m_contactNormal1.Mult2( ref constr.m_rbA.m_linearFactor, scalar, out tmp ); fb.m_appliedForceBodyA.Add( ref tmp, out fb.m_appliedForceBodyA ); //fb.m_appliedForceBodyA += solverConstr.m_contactNormal1 * solverConstr.m_appliedImpulse * constr.getRigidBodyA().getLinearFactor() / infoGlobal.m_timeStep; solverConstr.m_contactNormal2.Mult2( ref constr.m_rbB.m_linearFactor, scalar, out tmp ); fb.m_appliedForceBodyB.Add( ref tmp, out fb.m_appliedForceBodyB ); //fb.m_appliedForceBodyB += solverConstr.m_contactNormal2 * solverConstr.m_appliedImpulse * constr.getRigidBodyB().getLinearFactor() / infoGlobal.m_timeStep; solverConstr.m_relpos1CrossNormal.Mult2( ref constr.m_rbA.m_angularFactor, scalar, out tmp ); fb.m_appliedTorqueBodyA.Add( ref tmp, out fb.m_appliedTorqueBodyA ); //fb.m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * constr.getRigidBodyA().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; solverConstr.m_relpos2CrossNormal.Mult2( ref constr.m_rbB.m_angularFactor, scalar, out tmp ); fb.m_appliedTorqueBodyB.Add( ref tmp, out fb.m_appliedTorqueBodyB ); //fb.m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal * constr.getRigidBodyB().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; /*RGM ???? */ } constr.internalSetAppliedImpulse( solverConstr.m_appliedImpulse ); if( btScalar.btFabs( solverConstr.m_appliedImpulse ) >= constr.getBreakingImpulseThreshold() ) { constr.setEnabled( false ); } } for( i = 0; i < m_tmpSolverBodyPool.Count; i++ ) { btSolverBody solverBody = m_tmpSolverBodyPool[i]; btRigidBody body = solverBody.m_originalBody; if( body != null ) { if( infoGlobal.m_splitImpulse ) solverBody.writebackVelocityAndTransform( infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp ); else solverBody.writebackVelocity(); btVector3 tmp; solverBody.m_linearVelocity.Add( ref solverBody.m_externalForceImpulse, out tmp ); solverBody.m_originalBody.setLinearVelocity( ref tmp ); solverBody.m_angularVelocity.Add( ref solverBody.m_externalTorqueImpulse, out tmp ); solverBody.m_originalBody.setAngularVelocity( ref tmp ); if( infoGlobal.m_splitImpulse && solverBody.modified ) { btScalar.Dbg( DbgFlag.PredictedTransform, "Solver body transform is " + solverBody.m_worldTransform ); solverBody.m_originalBody.setWorldTransform( ref solverBody.m_worldTransform ); } BulletGlobals.SolverBodyPool.Free( solverBody ); body.setCompanionBody( null ); } } foreach( btSolverConstraint constraint in m_tmpSolverContactConstraintPool ) BulletGlobals.SolverConstraintPool.Free( constraint ); foreach( btSolverConstraint constraint in m_tmpSolverNonContactConstraintPool ) BulletGlobals.SolverConstraintPool.Free( constraint ); foreach( btSolverConstraint constraint in m_tmpSolverContactFrictionConstraintPool ) BulletGlobals.SolverConstraintPool.Free( constraint ); foreach( btSolverConstraint constraint in m_tmpSolverContactRollingFrictionConstraintPool ) BulletGlobals.SolverConstraintPool.Free( constraint ); m_tmpSolverContactConstraintPool.Count = 0; // resizeNoInitialize( 0 ); m_tmpSolverNonContactConstraintPool.Count = 0; //resizeNoInitialize( 0 ); m_tmpSolverContactFrictionConstraintPool.Count = 0; //resizeNoInitialize( 0 ); m_tmpSolverContactRollingFrictionConstraintPool.Count = 0; //resizeNoInitialize( 0 ); m_tmpSolverBodyPool.Count = 0; //resizeNoInitialize( 0 ); m_fixedBody = null; 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; }
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 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 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; }
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; }
protected void convertContacts( btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds, btContactSolverInfo infoGlobal ) { int i; btPersistentManifold manifold; // btCollisionObject colObj0=0,*colObj1=0; for( i = 0; i < numManifolds; i++ ) { manifold = manifoldPtr[i + start_manifold]; convertContact( manifold, infoGlobal ); } }
internal abstract void allSolved( btContactSolverInfo info, btIDebugDraw debugDrawer );
///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 );