most basic rigid body simulation

lacks object deactivation and constraints, vehicles etc.
2006-09-26 21:31:44 +00:00
parent 37a53ee7d9
commit 9531ec4b30
2 changed files with 159 additions and 0 deletions
--- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -0,0 +1,110 @@
+
+#include "btSimpleDynamicsWorld.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
+
+btSimpleDynamicsWorld::btSimpleDynamicsWorld()
+:CollisionWorld(new	CollisionDispatcher(),new SimpleBroadphase()),
+m_constraintSolver(new SequentialImpulseConstraintSolver)
+{
+
+}
+
+btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
+{
+	delete m_constraintSolver;
+
+	//delete the dispatcher and paircache
+	delete m_dispatcher1;
+	m_dispatcher1 = 0;
+	delete m_pairCache;
+	m_pairCache = 0;
+}
+
+void	btSimpleDynamicsWorld::stepSimulation(float timeStep)
+{
+	///apply gravity, predict motion
+	predictUnconstraintMotion(timeStep);
+
+	///perform collision detection
+	PerformDiscreteCollisionDetection();
+
+	///solve contact constraints
+	PersistentManifold** manifoldPtr = ((CollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
+	int numManifolds = m_dispatcher1->GetNumManifolds();
+	ContactSolverInfo infoGlobal;
+	infoGlobal.m_timeStep = timeStep;
+	IDebugDraw* debugDrawer=0;
+	m_constraintSolver->SolveGroup(manifoldPtr, numManifolds,infoGlobal,debugDrawer);
+
+	///integrate transforms
+	integrateTransforms(timeStep);
+		
+	updateAabbs();
+
+}
+
+void	btSimpleDynamicsWorld::updateAabbs()
+{
+	SimdTransform predictedTrans;
+	for (int i=0;i<m_collisionObjects.size();i++)
+	{
+		CollisionObject* colObj = m_collisionObjects[i];
+		if (colObj->m_internalOwner)
+		{
+			RigidBody* body = (RigidBody*)colObj->m_internalOwner;
+			if (body->IsActive() && (!body->IsStatic()))
+			{
+				SimdPoint3 minAabb,maxAabb;
+				colObj->m_collisionShape->GetAabb(colObj->m_worldTransform, minAabb,maxAabb);
+				SimpleBroadphase* bp = (SimpleBroadphase*)m_pairCache;
+				bp->SetAabb(body->m_broadphaseHandle,minAabb,maxAabb);
+			}
+		}
+	}
+}
+
+void	btSimpleDynamicsWorld::integrateTransforms(float timeStep)
+{
+	SimdTransform predictedTrans;
+	for (int i=0;i<m_collisionObjects.size();i++)
+	{
+		CollisionObject* colObj = m_collisionObjects[i];
+		if (colObj->m_internalOwner)
+		{
+			RigidBody* body = (RigidBody*)colObj->m_internalOwner;
+			if (body->IsActive() && (!body->IsStatic()))
+			{
+				body->predictIntegratedTransform(timeStep, predictedTrans);
+				body->proceedToTransform( predictedTrans);
+			}
+		}
+	}
+}
+
+
+
+void	btSimpleDynamicsWorld::predictUnconstraintMotion(float timeStep)
+{
+	for (int i=0;i<m_collisionObjects.size();i++)
+	{
+		CollisionObject* colObj = m_collisionObjects[i];
+		if (colObj->m_internalOwner)
+		{
+			RigidBody* body = (RigidBody*)colObj->m_internalOwner;
+			body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
+			if (body->IsActive() && (!body->IsStatic()))
+			{
+				body->applyForces( timeStep);
+				body->integrateVelocities( timeStep);
+				body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
+
+			}
+		}
+	}
+}