mirror of
https://github.com/bulletphysics/bullet3.git
synced 2026-10-17 22:35:45 +00:00
110 lines
3.1 KiB
C++
110 lines
3.1 KiB
C++
|
|
#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);
|
|
|
|
}
|
|
}
|
|
}
|
|
} |