Esempio n. 1
0
		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 );
				}
			}
		}
Esempio n. 2
0
		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 );
				}
			}
		}
Esempio n. 3
0
		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 );
						}
					}
				}
			}
		}
Esempio n. 4
0
		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();

		}
Esempio n. 5
0
		///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 );
		}
Esempio n. 6
0
		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;
		}
Esempio n. 8
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( &currentConstraintRow[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;
		}
Esempio n. 11
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
		}
Esempio n. 12
0
		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 );
						}
					}
				}
			}
		}
Esempio n. 13
0
		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 );
			}
		}
Esempio n. 14
0
		///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;
		}
Esempio n. 15
0
		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 );
							}
						}
					}
				}
			}

		}
Esempio n. 16
0
		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 );
			}
		}
Esempio n. 17
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 );
		}
Esempio n. 18
0
		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 );
					}
				}
			}
		}
Esempio n. 19
0
		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 );
				}
			}
		}