public virtual void predictUnconstraintMotion( double timeStep ) { CProfileSample sample = new CProfileSample( "predictUnconstraintMotion" ); for( int i = 0; i < m_nonStaticRigidBodies.Count; i++ ) { btRigidBody body = m_nonStaticRigidBodies[i]; if( !body.isStaticOrKinematicObject() ) { //don't integrate/update velocities here, it happens in the constraint solver body.applyDamping( timeStep ); body.predictIntegratedTransform( timeStep, out body.m_interpolationWorldTransform ); } } }
internal void synchronizeMotionStates() { CProfileSample sample = new CProfileSample( "synchronizeMotionStates" ); if( m_synchronizeAllMotionStates ) { //iterate over all collision objects for( int i = 0; i < m_collisionObjects.Count; i++ ) { btCollisionObject colObj = m_collisionObjects[i]; btRigidBody body = btRigidBody.upcast( colObj ); if( body != null ) synchronizeSingleMotionState( body ); } } else { //iterate over all active rigid bodies for( int i = 0; i < m_nonStaticRigidBodies.Count; i++ ) { btRigidBody body = m_nonStaticRigidBodies[i]; if( body.isActive() ) synchronizeSingleMotionState( body ); } } }
internal void createPredictiveContacts( double timeStep ) { CProfileSample sample = new CProfileSample( "createPredictiveContacts" ); { CProfileSample sub_sample = new CProfileSample( "release predictive contact manifolds" ); for( int i = 0; i < m_predictiveManifolds.Count; i++ ) { btPersistentManifold manifold = m_predictiveManifolds[i]; this.m_dispatcher1.releaseManifold( manifold ); } m_predictiveManifolds.Clear(); } btTransform predictedTrans; for( int i = 0; i < m_nonStaticRigidBodies.Count; i++ ) { btRigidBody body = m_nonStaticRigidBodies[i]; body.setHitFraction( 1 ); if( body.isActive() & ( !body.isStaticOrKinematicObject() ) ) { body.predictIntegratedTransform( timeStep, out predictedTrans ); btVector3 posDelta; predictedTrans.m_origin.Sub( ref body.m_worldTransform.m_origin, out posDelta ); double squareMotion = posDelta.length2(); if( getDispatchInfo().m_useContinuous && 0 != body.getCcdSquareMotionThreshold() && body.getCcdSquareMotionThreshold() < squareMotion ) { CProfileSample subsample = new CProfileSample( "predictive convexSweepTest" ); if( body.getCollisionShape().isConvex() ) { gNumClampedCcdMotions++; #if PREDICTIVE_CONTACT_USE_STATIC_ONLY StaticOnlyCallback sweepResults( body, body.getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase().getOverlappingPairCache(),getDispatcher()); #else btClosestNotMeConvexResultCallback sweepResults = BulletGlobals.ClosestNotMeConvexResultCallbackPool.Get(); sweepResults.Initialize( body, ref body.m_worldTransform.m_origin , ref predictedTrans.m_origin, getBroadphase().getOverlappingPairCache(), getDispatcher() ); #endif //btConvexShape* convexShape = static_cast<btConvexShape*>(body.getCollisionShape()); btSphereShape tmpSphere = BulletGlobals.SphereShapePool.Get(); tmpSphere.Initialize( body.getCcdSweptSphereRadius() );//btConvexShape* convexShape = static_cast<btConvexShape*>(body.getCollisionShape()); sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration; sweepResults.m_collisionFilterGroup = body.getBroadphaseProxy().m_collisionFilterGroup; sweepResults.m_collisionFilterMask = body.getBroadphaseProxy().m_collisionFilterMask; btTransform modifiedPredictedTrans = predictedTrans; modifiedPredictedTrans.m_basis = body.m_worldTransform.m_basis; convexSweepTest( tmpSphere, ref body.m_worldTransform, ref modifiedPredictedTrans, sweepResults ); if( sweepResults.hasHit() && ( sweepResults.m_closestHitFraction < 1 ) ) { btVector3 tmp; btVector3 distVec; posDelta.Mult( sweepResults.m_closestHitFraction, out distVec ); sweepResults.m_hitNormalWorld.Invert( out tmp ); double distance = distVec.dot( ref tmp ); btPersistentManifold manifold = m_dispatcher1.getNewManifold( body, sweepResults.m_hitCollisionObject ); m_predictiveManifolds.Add( manifold ); btVector3 worldPointB; body.m_worldTransform.m_origin.Add( ref distVec, out worldPointB ); btTransform tmpT; sweepResults.m_hitCollisionObject.m_worldTransform.inverse( out tmpT ); btVector3 localPointB; tmpT.Apply( ref worldPointB, out localPointB ); btManifoldPoint newPoint = BulletGlobals.ManifoldPointPool.Get(); newPoint.Initialize( ref btVector3.Zero, ref localPointB, ref sweepResults.m_hitNormalWorld, distance ); bool isPredictive = true; int index = manifold.addManifoldPoint( newPoint, isPredictive ); btManifoldPoint pt = manifold.getContactPoint( index ); pt.m_combinedRestitution = 0; pt.m_combinedFriction = btManifoldResult.calculateCombinedFriction( body, sweepResults.m_hitCollisionObject ); body.m_worldTransform.getOrigin( out pt.m_positionWorldOnA ); pt.m_positionWorldOnB = worldPointB; BulletGlobals.ManifoldPointPool.Free( newPoint ); } BulletGlobals.SphereShapePool.Free( tmpSphere ); BulletGlobals.ClosestNotMeConvexResultCallbackPool.Free( sweepResults ); } } } } }
public override void debugDrawWorld() { if( getDebugDrawer() == null ) return; CProfileSample sample = new CProfileSample( "debugDrawWorld" ); base.debugDrawWorld(); bool drawConstraints = false; btIDebugDraw.DebugDrawModes mode = getDebugDrawer().getDebugMode(); if( ( mode & ( btIDebugDraw.DebugDrawModes.DBG_DrawConstraints | btIDebugDraw.DebugDrawModes.DBG_DrawConstraintLimits ) ) != 0 ) { drawConstraints = true; } if( drawConstraints ) { for( int i = getNumConstraints() - 1; i >= 0; i-- ) { btTypedConstraint constraint = getConstraint( i ); debugDrawConstraint( constraint ); } } if( 0 != ( getDebugDrawer().getDebugMode() & ( btIDebugDraw.DebugDrawModes.DBG_DrawWireframe | btIDebugDraw.DebugDrawModes.DBG_DrawAabb | btIDebugDraw.DebugDrawModes.DBG_DrawNormals ) ) ) { int i; if( getDebugDrawer().getDebugMode() != 0 ) { for( i = 0; i < m_actions.Count; i++ ) { m_actions[i].debugDraw( m_debugDrawer ); } } } getDebugDrawer().flushLines(); }
///the computeOverlappingPairs is usually already called by performDiscreteCollisionDetection (or stepSimulation) ///it can be useful to use if you perform ray tests without collision detection/simulation public virtual void computeOverlappingPairs() { CProfileSample sample = new CProfileSample( "calculateOverlappingPairs" ); m_broadphasePairCache.calculateOverlappingPairs( m_dispatcher1 ); }
internal void calculateSimulationIslands() { CProfileSample sample = new CProfileSample( "calculateSimulationIslands" ); m_islandManager.updateActivationState( this, getDispatcher() ); { //merge islands based on speculative contact manifolds too for( int i = 0; i < this.m_predictiveManifolds.Count; i++ ) { btPersistentManifold manifold = m_predictiveManifolds[i]; btCollisionObject colObj0 = manifold.m_body0; btCollisionObject colObj1 = manifold.m_body1; if( ( ( colObj0 != null ) && ( !( colObj0 ).isStaticOrKinematicObject() ) ) && ( ( colObj1 != null ) && ( !( colObj1 ).isStaticOrKinematicObject() ) ) ) { m_islandManager.getUnionFind().unite( ( colObj0 ).getIslandTag(), ( colObj1 ).getIslandTag() ); } } } { int i; int numConstraints = m_constraints.Count; for( i = 0; i < numConstraints; i++ ) { btTypedConstraint constraint = m_constraints[i]; if( constraint.isEnabled() ) { btRigidBody colObj0 = constraint.getRigidBodyA(); btRigidBody colObj1 = constraint.getRigidBodyB(); if( ( ( colObj0 != null ) && ( !( colObj0 ).isStaticOrKinematicObject() ) ) && ( ( colObj1 != null ) && ( !( colObj1 ).isStaticOrKinematicObject() ) ) ) { m_islandManager.getUnionFind().unite( ( colObj0 ).getIslandTag(), ( colObj1 ).getIslandTag() ); } } } } //Store the island id in each body m_islandManager.storeIslandActivationState( this ); }
/// 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; }
internal void updateActions( double timeStep ) { CProfileSample sample = new CProfileSample( "updateActions" ); for( int i = 0; i < m_actions.Count; i++ ) { m_actions[i].updateAction( this, timeStep ); } }
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 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; }
/// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback /// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback. internal void convexSweepTest( btConvexShape castShape, ref btTransform convexFromWorld, ref btTransform convexToWorld, ConvexResultCallback resultCallback, double allowedCcdPenetration = btScalar.BT_ZERO ) { CProfileSample sample = new CProfileSample( "convexSweepTest" ); /// use the broadphase to accelerate the search for objects, based on their aabb /// and for each object with ray-aabb overlap, perform an exact ray test /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical btTransform convexFromTrans, convexToTrans; convexFromTrans = convexFromWorld; convexToTrans = convexToWorld; btVector3 castShapeAabbMin, castShapeAabbMax; /* Compute AABB that encompasses angular movement */ { btVector3 linVel, angVel; btTransformUtil.calculateVelocity( ref convexFromWorld, ref convexToWorld, 1.0f, out linVel, out angVel ); btVector3 zeroLinVel = btVector3.Zero; btTransform R = btTransform.Identity; R.m_basis = convexFromWorld.m_basis; castShape.calculateTemporalAabb( ref R, ref zeroLinVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax ); } #if !USE_BRUTEFORCE_RAYBROADPHASE btSingleSweepCallback convexCB = BulletGlobals.SingleSweepCallbackPool.Get(); convexCB.Initialize( castShape, ref convexFromWorld, ref convexToWorld, this, resultCallback, allowedCcdPenetration ); m_broadphasePairCache.rayTest( ref convexFromTrans.m_origin, ref convexToTrans.m_origin, convexCB, ref castShapeAabbMin, ref castShapeAabbMax ); BulletGlobals.SingleSweepCallbackPool.Free( convexCB ); #else /// go over all objects, and if the ray intersects their aabb + cast shape aabb, // do a ray-shape query using convexCaster (CCD) int i; for( i = 0; i < m_collisionObjects.Count; i++ ) { btCollisionObject collisionObject = m_collisionObjects[i]; //only perform raycast if filterMask matches if( resultCallback.needsCollision( collisionObject.getBroadphaseHandle() ) ) { //RigidcollisionObject collisionObject = ctrl.GetRigidcollisionObject(); btVector3 collisionObjectAabbMin, collisionObjectAabbMax; collisionObject.getCollisionShape().getAabb( collisionObject.getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax ); AabbExpand( collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax ); double hitLambda = (double)( 1.0 ); //could use resultCallback.m_closestHitFraction, but needs testing btVector3 hitNormal; if( btRayAabb( convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal ) ) { objectQuerySingle( castShape, convexFromTrans, convexToTrans, collisionObject, collisionObject.getCollisionShape(), collisionObject.getWorldTransform(), resultCallback, allowedCcdPenetration ); } } } #endif //USE_BRUTEFORCE_RAYBROADPHASE }
public static void objectQuerySingleInternal( btConvexShape castShape, ref btTransform convexFromTrans, ref btTransform convexToTrans , btCollisionShape collisionShape//btCollisionObjectWrapper colObjWrap , btCollisionObject collisionObject , ref btTransform colObjTransform , ConvexResultCallback resultCallback, double allowedPenetration ) { //btCollisionShape collisionShape = colObjWrap.getCollisionShape(); //btTransform colObjWorldTransform = colObjWrap.m_worldTransform; if( collisionShape.isConvex() ) { //CProfileSample sample = new CProfileSample("convexSweepConvex"); btConvexCast.CastResult castResult = new btConvexCast.CastResult(); castResult.m_allowedPenetration = allowedPenetration; castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar.BT_ONE;//?? btConvexShape convexShape = (btConvexShape)collisionShape; btVoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get(); btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get(); // new btGjkEpaPenetrationDepthSolver(); btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get(); convexCaster1.Initialize( castShape, convexShape, simplexSolver, gjkEpaPenetrationSolver ); //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver); //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver); btConvexCast castPtr = convexCaster1; if( castPtr.calcTimeOfImpact( ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) ) { //add hit if( castResult.m_normal.length2() > (double)( 0.0001 ) ) { if( castResult.m_fraction < resultCallback.m_closestHitFraction ) { castResult.m_normal.normalize(); LocalConvexResult localConvexResult = new LocalConvexResult ( collisionObject, null, ref castResult.m_normal, ref castResult.m_hitPoint, castResult.m_fraction ); bool normalInWorldSpace = true; resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace ); } } } BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free( gjkEpaPenetrationSolver ); BulletGlobals.VoronoiSimplexSolverPool.Free( simplexSolver ); BulletGlobals.ContinuousConvexCollisionPool.Free( convexCaster1 ); } else { if( collisionShape.isConcave() ) { if( collisionShape.getShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE ) { //CProfileSample sample = new CProfileSample("convexSweepbtBvhTriangleMesh"); btConcaveShape triangleMesh = (btConcaveShape)collisionShape; btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject ); btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal ); btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal ); // rotation of box in local mesh space = MeshRotation^-1 ConvexToRotation btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis ); rotationXform.m_origin = btVector3.Zero; //ConvexCast::CastResult BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get(); tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans , resultCallback, collisionObject , triangleMesh, ref colObjTransform ); tccb.m_hitFraction = resultCallback.m_closestHitFraction; tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal ); triangleMesh.performConvexcast( tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal ); BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb ); } else { if( collisionShape.getShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE ) { btConvexCast.CastResult castResult = BulletGlobals.CastResultPool.Get(); castResult.m_allowedPenetration = allowedPenetration; castResult.m_fraction = resultCallback.m_closestHitFraction; btStaticPlaneShape planeShape = (btStaticPlaneShape)collisionShape; btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get(); convexCaster1.Initialize( castShape, planeShape ); btConvexCast castPtr = convexCaster1; if( castPtr.calcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) ) { //add hit if( castResult.m_normal.length2() > (double)( 0.0001 ) ) { if( castResult.m_fraction < resultCallback.m_closestHitFraction ) { castResult.m_normal.normalize(); LocalConvexResult localConvexResult = new LocalConvexResult ( collisionObject, null, ref castResult.m_normal, ref castResult.m_hitPoint, castResult.m_fraction ); bool normalInWorldSpace = true; resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace ); } } } } else { //CProfileSample sample = new CProfileSample("convexSweepConcave"); btConcaveShape concaveShape = (btConcaveShape)collisionShape; btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject ); btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal ); btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal ); // rotation of box in local mesh space = MeshRotation^-1 ConvexToRotation btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis ); rotationXform.m_origin = btVector3.Zero; BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get(); tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject , concaveShape, ref colObjTransform ); tccb.m_hitFraction = resultCallback.m_closestHitFraction; tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal ); btVector3 rayAabbMinLocal = convexFromLocal; rayAabbMinLocal.setMin( ref convexToLocal ); btVector3 rayAabbMaxLocal = convexFromLocal; rayAabbMaxLocal.setMax( ref convexToLocal ); rayAabbMinLocal += boxMinLocal; rayAabbMaxLocal += boxMaxLocal; concaveShape.processAllTriangles( tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal ); BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb ); } } } else { ///@todo : use AABB tree or other BVH acceleration structure! if( collisionShape.isCompound() ) { CProfileSample sample = new CProfileSample( "convexSweepCompound" ); btCompoundShape compoundShape = (btCompoundShape)( collisionShape ); int i = 0; for( i = 0; i < compoundShape.getNumChildShapes(); i++ ) { //btTransform childTrans = compoundShape.getChildTransform( i ); btCollisionShape childCollisionShape = compoundShape.getChildShape( i ); btTransform childWorldTrans; colObjTransform.Apply( ref compoundShape.m_children.InternalArray[i].m_transform , out childWorldTrans ); LocalInfoAdder my_cb = new LocalInfoAdder( i, resultCallback ); //btCollisionObjectWrapper tmpObj = BulletGlobals.CollisionObjectWrapperPool.Get(); //tmpObj.Initialize( colObjWrap, childCollisionShape, colObjWrap.m_collisionObject, ref childWorldTrans, -1, i ); objectQuerySingleInternal( castShape, ref convexFromTrans, ref convexToTrans , childCollisionShape , collisionObject , ref childWorldTrans , my_cb, allowedPenetration ); //BulletGlobals.CollisionObjectWrapperPool.Free( tmpObj ); } } } } }
public virtual void performDiscreteCollisionDetection() { CProfileSample sample = new CProfileSample( "performDiscreteCollisionDetection" ); //btDispatcherInfo dispatchInfo = m_dispatchInfo; updateAabbs(); computeOverlappingPairs(); //btDispatcher dispatcher = m_dispatcher1; { CProfileSample sample2 = new CProfileSample( "dispatchAllCollisionPairs" ); if( m_dispatcher1 != null ) m_dispatcher1.dispatchAllCollisionPairs( m_broadphasePairCache.getOverlappingPairCache(), m_dispatchInfo, m_dispatcher1 ); } }
///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's public override int stepSimulation( double timeStep, int maxSubSteps = 1, double fixedTimeStep = btScalar.BT_ONE_OVER_SIXTY ) { startProfiling( timeStep ); CProfileSample sample = new CProfileSample( "stepSimulation" ); int numSimulationSubSteps = 0; if( maxSubSteps != 0 ) { //fixed timestep with interpolation m_fixedTimeStep = fixedTimeStep; m_localTime += timeStep; if( m_localTime >= fixedTimeStep ) { numSimulationSubSteps = (int)( m_localTime / fixedTimeStep ); m_localTime -= numSimulationSubSteps * fixedTimeStep; } } else { //variable timestep fixedTimeStep = timeStep; m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; m_fixedTimeStep = 0; if( btScalar.btFuzzyZero( timeStep ) ) { numSimulationSubSteps = 0; maxSubSteps = 0; } else { numSimulationSubSteps = 1; maxSubSteps = 1; } } //process some debugging flags if( getDebugDrawer() != null ) { btIDebugDraw debugDrawer = getDebugDrawer(); btRigidBody.gDisableDeactivation = ( debugDrawer.getDebugMode() & btIDebugDraw.DebugDrawModes.DBG_NoDeactivation ) != 0; } if( numSimulationSubSteps != 0 ) { //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt int clampedSimulationSteps = ( numSimulationSubSteps > maxSubSteps ) ? maxSubSteps : numSimulationSubSteps; saveKinematicState( fixedTimeStep * clampedSimulationSteps ); applyGravity(); for( int i = 0; i < clampedSimulationSteps; i++ ) { internalSingleStepSimulation( fixedTimeStep ); synchronizeMotionStates(); } } else { synchronizeMotionStates(); } clearForces(); #if !BT_NO_PROFILE CProfileManager.Increment_Frame_Counter(); #endif //BT_NO_PROFILE return numSimulationSubSteps; }
public virtual void integrateTransforms( double timeStep ) { CProfileSample sample = new CProfileSample( "integrateTransforms" ); btTransform predictedTrans; for( int i = 0; i < m_nonStaticRigidBodies.Count; i++ ) { btRigidBody body = m_nonStaticRigidBodies[i]; body.setHitFraction( 1 ); if( body.isActive() && ( !body.isStaticOrKinematicObject() ) ) { body.predictIntegratedTransform( timeStep, out predictedTrans ); btVector3 delta; predictedTrans.m_origin.Sub( ref body.m_worldTransform.m_origin, out delta ); double squareMotion = ( delta ).length2(); if( getDispatchInfo().m_useContinuous && body.getCcdSquareMotionThreshold() != 0 && body.getCcdSquareMotionThreshold() < squareMotion ) { CProfileSample sample2 = new CProfileSample( "CCD motion clamping" ); if( body.getCollisionShape().isConvex() ) { gNumClampedCcdMotions++; #if USE_STATIC_ONLY StaticOnlyCallback sweepResults( body, body.getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase().getOverlappingPairCache(),getDispatcher()); #else btClosestNotMeConvexResultCallback sweepResults = BulletGlobals.ClosestNotMeConvexResultCallbackPool.Get(); sweepResults.Initialize( body, ref body.m_worldTransform.m_origin , ref predictedTrans.m_origin, getBroadphase().getOverlappingPairCache(), getDispatcher() ); #endif //btConvexShape* convexShape = static_cast<btConvexShape*>(body.getCollisionShape()); using( btSphereShape tmpSphere = BulletGlobals.SphereShapePool.Get() ) { tmpSphere.Initialize( body.getCcdSweptSphereRadius() );//btConvexShape* convexShape = static_cast<btConvexShape*>(body.getCollisionShape()); sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration; sweepResults.m_collisionFilterGroup = body.getBroadphaseProxy().m_collisionFilterGroup; sweepResults.m_collisionFilterMask = body.getBroadphaseProxy().m_collisionFilterMask; btTransform modifiedPredictedTrans = predictedTrans; modifiedPredictedTrans.setBasis( ref body.m_worldTransform.m_basis ); convexSweepTest( tmpSphere, ref body.m_worldTransform, ref modifiedPredictedTrans, sweepResults ); if( sweepResults.hasHit() && ( sweepResults.m_closestHitFraction < 1 ) ) { //Console.WriteLine("clamped integration to hit fraction = %f\n",fraction); body.setHitFraction( sweepResults.m_closestHitFraction ); body.predictIntegratedTransform( timeStep * body.getHitFraction(), out predictedTrans ); body.setHitFraction( 0 ); body.proceedToTransform( ref predictedTrans ); #if false btVector3 linVel = body.getLinearVelocity(); double maxSpeed = body.getCcdMotionThreshold()/getSolverInfo().m_timeStep; double maxSpeedSqr = maxSpeed*maxSpeed; if (linVel.length2()>maxSpeedSqr) { linVel.normalize(); linVel*= maxSpeed; body.setLinearVelocity(linVel); double ms2 = body.getLinearVelocity().length2(); body.predictIntegratedTransform(timeStep, predictedTrans); double sm2 = (predictedTrans.getOrigin()-body.getWorldTransform().getOrigin()).length2(); double smt = body.getCcdSquareMotionThreshold(); Console.WriteLine("sm2=%f\n",sm2); } #else //don't apply the collision response right now, it will happen next frame //if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution. //double appliedImpulse = 0; //double depth = 0; //appliedImpulse = resolveSingleCollision(body,(btCollisionObject)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth); #endif BulletGlobals.ClosestNotMeConvexResultCallbackPool.Free( sweepResults ); continue; } BulletGlobals.ClosestNotMeConvexResultCallbackPool.Free( sweepResults ); } } } btScalar.Dbg( DbgFlag.PredictedTransform, predictedTrans.ToString( "predicted orient\t", "\t\t\t", "predicted origin\t" ) ); body.proceedToTransform( ref predictedTrans ); } } ///this should probably be switched on by default, but it is not well tested yet if( m_applySpeculativeContactRestitution ) { CProfileSample sub_sample = new CProfileSample( "apply speculative contact restitution" ); for( int i = 0; i < m_predictiveManifolds.Count; i++ ) { btPersistentManifold manifold = m_predictiveManifolds[i]; btRigidBody body0 = btRigidBody.upcast( (btCollisionObject)manifold.m_body0 ); btRigidBody body1 = btRigidBody.upcast( (btCollisionObject)manifold.m_body1 ); for( int p = 0; p < manifold.m_cachedPoints; p++ ) { btManifoldPoint pt = manifold.getContactPoint( p ); double combinedRestitution = btManifoldResult.calculateCombinedRestitution( body0, body1 ); if( combinedRestitution > 0 && pt.m_appliedImpulse != 0 ) //if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0) { btVector3 imp; pt.m_normalWorldOnB.Mult( combinedRestitution * pt.m_appliedImpulse, out imp ); //btVector3 pos1 = pt.m_positionWorldOnA; //btVector3 pos2 = pt.m_positionWorldOnB; btVector3 rel_pos0; pt.m_positionWorldOnA.Sub( ref body0.m_worldTransform.m_origin, out rel_pos0 ); btVector3 rel_pos1; pt.m_positionWorldOnB.Sub( ref body1.m_worldTransform.m_origin, out rel_pos1 ); if( body0 != null ) body0.applyImpulse( ref imp, ref rel_pos0 ); if( body1 != null ) { imp.Invert( out imp ); body1.applyImpulse( ref imp, ref rel_pos1 ); } } } } } }
internal void internalSingleStepSimulation( double timeStep ) { CProfileSample sample = new CProfileSample( "internalSingleStepSimulation" ); if( null != m_internalPreTickCallback ) { m_internalPreTickCallback( this, timeStep ); } ///apply gravity, predict motion predictUnconstraintMotion( timeStep ); btDispatcherInfo dispatchInfo = getDispatchInfo(); dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; dispatchInfo.m_debugDraw = getDebugDrawer(); createPredictiveContacts( timeStep ); ///perform collision detection performDiscreteCollisionDetection(); calculateSimulationIslands(); getSolverInfo().m_timeStep = timeStep; ///solve contact and other joint constraints solveConstraints( m_solverInfo ); ///CallbackTriggers(); ///integrate transforms integrateTransforms( timeStep ); ///update vehicle simulation updateActions( timeStep ); updateActivationState( timeStep ); if( null != m_internalTickCallback ) { m_internalTickCallback( this, timeStep ); } }
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 void updateActivationState( double timeStep ) { CProfileSample sample = new CProfileSample( "updateActivationState" ); for( int i = 0; i < m_nonStaticRigidBodies.Count; i++ ) { btRigidBody body = m_nonStaticRigidBodies[i]; if( body != null ) { body.updateDeactivation( timeStep ); if( body.wantsSleeping() ) { if( body.isStaticOrKinematicObject() ) { body.setActivationState( ActivationState.ISLAND_SLEEPING ); } else { if( body.m_activationState1 == ActivationState.ACTIVE_TAG ) body.setActivationState( ActivationState.WANTS_DEACTIVATION ); if( body.m_activationState1 == ActivationState.ISLAND_SLEEPING ) { body.setAngularVelocity( ref btVector3.Zero ); body.setLinearVelocity( ref btVector3.Zero ); } } } else { if( body.getActivationState() != ActivationState.DISABLE_DEACTIVATION ) body.setActivationState( ActivationState.ACTIVE_TAG ); } } } }
public virtual void updateAabbs() { CProfileSample sample = new CProfileSample( "updateAabbs" ); //btTransform predictedTrans; for( int i = 0; i < m_collisionObjects.Count; i++ ) { btCollisionObject colObj = m_collisionObjects[i]; //only update aabb of active objects if( m_forceUpdateAllAabbs || colObj.isActive() ) { updateSingleAabb( colObj ); } } }