diff --git a/Extras/PhysicsInterface/CMakeLists.txt b/Extras/PhysicsInterface/CMakeLists.txt deleted file mode 100644 index 1702f9822..000000000 --- a/Extras/PhysicsInterface/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -SUBDIRS( CcdPhysics Common ) diff --git a/Extras/PhysicsInterface/CcdPhysics/CMakeLists.txt b/Extras/PhysicsInterface/CcdPhysics/CMakeLists.txt deleted file mode 100644 index ca40c6531..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -INCLUDE_DIRECTORIES( -${BULLET_PHYSICS_SOURCE_DIR}/src ${BULLET_PHYSICS_SOURCE_DIR}/Extras/PhysicsInterface/Common ${BULLET_PHYSICS_SOURCE_DIR}/Extras/PhysicsInterface/CcdPhysics } -) - -ADD_LIBRARY(LibCcdPhysicsInterface - CcdPhysicsController.cpp - CcdPhysicsEnvironment.cpp - ParallelIslandDispatcher.cpp - ParallelPhysicsEnvironment.cpp - SimulationIsland.cpp -) diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysics.dsp b/Extras/PhysicsInterface/CcdPhysics/CcdPhysics.dsp deleted file mode 100644 index 649ade1f3..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysics.dsp +++ /dev/null @@ -1,112 +0,0 @@ -# Microsoft Developer Studio Project File - Name="CcdPhysics" - Package Owner=<4> -# Microsoft Developer Studio Generated Build File, Format Version 6.00 -# ** DO NOT EDIT ** - -# TARGTYPE "Win32 (x86) Static Library" 0x0104 - -CFG=CcdPhysics - Win32 Debug -!MESSAGE This is not a valid makefile. To build this project using NMAKE, -!MESSAGE use the Export Makefile command and run -!MESSAGE -!MESSAGE NMAKE /f "CcdPhysics.mak". -!MESSAGE -!MESSAGE You can specify a configuration when running NMAKE -!MESSAGE by defining the macro CFG on the command line. For example: -!MESSAGE -!MESSAGE NMAKE /f "CcdPhysics.mak" CFG="CcdPhysics - Win32 Debug" -!MESSAGE -!MESSAGE Possible choices for configuration are: -!MESSAGE -!MESSAGE "CcdPhysics - Win32 Release" (based on "Win32 (x86) Static Library") -!MESSAGE "CcdPhysics - Win32 Debug" (based on "Win32 (x86) Static Library") -!MESSAGE - -# Begin Project -# PROP AllowPerConfigDependencies 0 -# PROP Scc_ProjName "" -# PROP Scc_LocalPath "" -CPP=cl.exe -RSC=rc.exe - -!IF "$(CFG)" == "CcdPhysics - Win32 Release" - -# PROP BASE Use_MFC 0 -# PROP BASE Use_Debug_Libraries 0 -# PROP BASE Output_Dir "CcdPhysics___Win32_Release" -# PROP BASE Intermediate_Dir "CcdPhysics___Win32_Release" -# PROP BASE Target_Dir "" -# PROP Use_MFC 0 -# PROP Use_Debug_Libraries 0 -# PROP Output_Dir "CcdPhysics___Win32_Release" -# PROP Intermediate_Dir "CcdPhysics___Win32_Release" -# PROP Target_Dir "" -LINK32=link.exe -lib -MTL=midl.exe -# ADD BASE CPP /nologo /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /YX /FD /c -# ADD CPP /nologo /W3 /GX /O2 /I "..\..\..\Bullet" /I "..\..\..\LinearMath" /I "..\..\..\BulletDynamics" /I "..\..\..\Extras\PhysicsInterface\Common" /I "..\..\..\Extras\PhysicsInterface\CcdPhysics" /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /YX /FD /c -# ADD BASE RSC /l 0x809 /d "NDEBUG" -# ADD RSC /l 0x809 /d "NDEBUG" -BSC32=bscmake.exe -# ADD BASE BSC32 /nologo -# ADD BSC32 /nologo -LIB32=link.exe -lib -# ADD BASE LIB32 /nologo -# ADD LIB32 /nologo - -!ELSEIF "$(CFG)" == "CcdPhysics - Win32 Debug" - -# PROP BASE Use_MFC 0 -# PROP BASE Use_Debug_Libraries 1 -# PROP BASE Output_Dir "CcdPhysics___Win32_Debug" -# PROP BASE Intermediate_Dir "CcdPhysics___Win32_Debug" -# PROP BASE Target_Dir "" -# PROP Use_MFC 0 -# PROP Use_Debug_Libraries 1 -# PROP Output_Dir "CcdPhysics___Win32_Debug" -# PROP Intermediate_Dir "CcdPhysics___Win32_Debug" -# PROP Target_Dir "" -LINK32=link.exe -lib -MTL=midl.exe -# ADD BASE CPP /nologo /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /YX /FD /GZ /c -# ADD CPP /nologo /W3 /Gm /GX /ZI /Od /I "..\..\..\Bullet" /I "..\..\..\LinearMath" /I "..\..\..\BulletDynamics" /I "..\..\..\Extras\PhysicsInterface\Common" /I "..\..\..\Extras\PhysicsInterface\CcdPhysics" /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /YX /FD /GZ /c -# ADD BASE RSC /l 0x809 /d "_DEBUG" -# ADD RSC /l 0x809 /d "_DEBUG" -BSC32=bscmake.exe -# ADD BASE BSC32 /nologo -# ADD BSC32 /nologo -LIB32=link.exe -lib -# ADD BASE LIB32 /nologo -# ADD LIB32 /nologo - -!ENDIF - -# Begin Target - -# Name "CcdPhysics - Win32 Release" -# Name "CcdPhysics - Win32 Debug" -# Begin Group "Source Files" - -# PROP Default_Filter "cpp;c;cxx;rc;def;r;odl;idl;hpj;bat" -# Begin Source File - -SOURCE=.\CcdPhysicsController.cpp -# End Source File -# Begin Source File - -SOURCE=.\CcdPhysicsEnvironment.cpp -# End Source File -# End Group -# Begin Group "Header Files" - -# PROP Default_Filter "h;hpp;hxx;hm;inl" -# Begin Source File - -SOURCE=.\CcdPhysicsController.h -# End Source File -# Begin Source File - -SOURCE=.\CcdPhysicsEnvironment.h -# End Source File -# End Group -# End Target -# End Project diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp deleted file mode 100644 index 6e964184f..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp +++ /dev/null @@ -1,587 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "CcdPhysicsController.h" - -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "PHY_IMotionState.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "CcdPhysicsEnvironment.h" -#include "LinearMath/btTransformUtil.h" - -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btConeShape.h" - -class BP_Proxy; - -///todo: fill all the empty CcdPhysicsController methods, hook them up to the btRigidBody class - -//'temporarily' global variables -extern float gDeactivationTime; -extern bool gDisableDeactivation; - -extern float gLinearSleepingTreshold; -extern float gAngularSleepingTreshold; - -#include "BulletDynamics/Dynamics/btMassProps.h" - -btVector3 startVel(0,0,0);//-10000); -CcdPhysicsController::CcdPhysicsController (const btCcdConstructionInfo& ci) -:m_cci(ci) -{ - m_collisionDelay = 0; - m_newClientInfo = 0; - - m_MotionState = ci.m_MotionState; - - - - CreateRigidbody(); - - - - #ifdef WIN32 - if (m_body->getInvMass()) - m_body->setLinearVelocity(startVel); - #endif - -} - -btTransform CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState) -{ - btTransform trans; - float tmp[3]; - motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]); - trans.setOrigin(btVector3(tmp[0],tmp[1],tmp[2])); - - btQuaternion orn; - motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]); - trans.setRotation(orn); - return trans; - -} - -void CcdPhysicsController::CreateRigidbody() -{ - - btTransform trans = GetTransformFromMotionState(m_cci.m_MotionState); - - btMassProps mp(m_cci.m_mass, m_cci.m_localInertiaTensor); - - m_body = new btRigidBody(mp,0,0,m_cci.m_friction,m_cci.m_restitution); - m_body->m_collisionShape = m_cci.m_collisionShape; - - - // - // init the rigidbody properly - // - - m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor); - //setMassProps this also sets collisionFlags - m_body->m_collisionFlags = m_cci.m_collisionFlags; - - m_body->setGravity( m_cci.m_gravity); - m_body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping); - m_body->setCenterOfMassTransform( trans ); - - -} - -CcdPhysicsController::~CcdPhysicsController() -{ - //will be reference counted, due to sharing - if (m_cci.m_physicsEnv) - m_cci.m_physicsEnv->removeCcdPhysicsController(this); - - ///this should not be deleted, it has been allocated by the user//delete m_MotionState; - delete m_body; -} - - /** - SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding') - */ -bool CcdPhysicsController::SynchronizeMotionStates(float time) -{ - //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.) - - if (!m_body->IsStatic()) - { - const btVector3& worldPos = m_body->getCenterOfMassPosition(); - m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]); - - const btQuaternion& worldquat = m_body->getOrientation(); - m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]); - - m_MotionState->calculateWorldTransformations(); - - float scale[3]; - m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]); - btVector3 scaling(scale[0],scale[1],scale[2]); - getCollisionShape()->setLocalScaling(scaling); - } else - { - btVector3 worldPos; - btQuaternion worldquat; - - m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]); - m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]); - btTransform oldTrans = m_body->getCenterOfMassTransform(); - btTransform newTrans(worldquat,worldPos); - - m_body->setCenterOfMassTransform(newTrans); - //need to keep track of previous position for friction effects... - - m_MotionState->calculateWorldTransformations(); - - float scale[3]; - m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]); - btVector3 scaling(scale[0],scale[1],scale[2]); - getCollisionShape()->setLocalScaling(scaling); - } - return true; - -} - - /** - WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding') - */ - -void CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly) -{ - -} -void CcdPhysicsController::WriteDynamicsToMotionState() -{ -} - // controller replication -void CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl) -{ - m_MotionState = motionstate; - - - - m_body = 0; - CreateRigidbody(); - - m_cci.m_physicsEnv->addCcdPhysicsController(this); - - -/* SM_Object* dynaparent=0; - SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl; - - if (sumoparentctrl) - { - dynaparent = sumoparentctrl->GetSumoObject(); - } - - SM_Object* orgsumoobject = m_sumoObj; - - - m_sumoObj = new SM_Object( - orgsumoobject->getShapeHandle(), - orgsumoobject->getMaterialProps(), - orgsumoobject->getShapeProps(), - dynaparent); - - m_sumoObj->setRigidBody(orgsumoobject->isRigidBody()); - - m_sumoObj->setMargin(orgsumoobject->getMargin()); - m_sumoObj->setPosition(orgsumoobject->getPosition()); - m_sumoObj->setOrientation(orgsumoobject->getOrientation()); - //if it is a dyna, register for a callback - m_sumoObj->registerCallback(*this); - - m_sumoScene->add(* (m_sumoObj)); - */ - - - -} - - // kinematic methods -void CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local) -{ - if (m_body) - { - m_body->activate(); - - btVector3 dloc(dlocX,dlocY,dlocZ); - btTransform xform = m_body->getCenterOfMassTransform(); - - if (local) - { - dloc = xform.getBasis()*dloc; - } - - xform.setOrigin(xform.getOrigin() + dloc); - m_body->setCenterOfMassTransform(xform); - } - -} - -void CcdPhysicsController::RelativeRotate(const float rotval[9],bool local) -{ - if (m_body ) - { - m_body->activate(); - - btMatrix3x3 drotmat( rotval[0],rotval[1],rotval[2], - rotval[4],rotval[5],rotval[6], - rotval[8],rotval[9],rotval[10]); - - - btMatrix3x3 currentOrn; - GetWorldOrientation(currentOrn); - - btTransform xform = m_body->getCenterOfMassTransform(); - - xform.setBasis(xform.getBasis()*(local ? - drotmat : (currentOrn.inverse() * drotmat * currentOrn))); - - m_body->setCenterOfMassTransform(xform); - } - -} - -void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& mat) -{ - float orn[4]; - m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]); - btQuaternion quat(orn[0],orn[1],orn[2],orn[3]); - mat.setRotation(quat); -} - -void CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal) -{ - btQuaternion q = m_body->getCenterOfMassTransform().getRotation(); - quatImag0 = q[0]; - quatImag1 = q[1]; - quatImag2 = q[2]; - quatReal = q[3]; -} -void CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal) -{ - m_body->activate(); - m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal); - btTransform xform = m_body->getCenterOfMassTransform(); - xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal)); - m_body->setCenterOfMassTransform(xform); - -} - -void CcdPhysicsController::setPosition(float posX,float posY,float posZ) -{ - m_body->activate(); - m_MotionState->setWorldPosition(posX,posY,posZ); - btTransform xform = m_body->getCenterOfMassTransform(); - xform.setOrigin(btVector3(posX,posY,posZ)); - m_body->setCenterOfMassTransform(xform); - -} -void CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ) -{ -} - -void CcdPhysicsController::getPosition(PHY__Vector3& pos) const -{ - const btTransform& xform = m_body->getCenterOfMassTransform(); - pos[0] = xform.getOrigin().x(); - pos[1] = xform.getOrigin().y(); - pos[2] = xform.getOrigin().z(); -} - -void CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ) -{ - if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) || - !btFuzzyZero(m_cci.m_scaling.y()-scaleY) || - !btFuzzyZero(m_cci.m_scaling.z()-scaleZ)) - { - m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ); - - if (m_body && m_body->getCollisionShape()) - { - m_body->getCollisionShape()->setLocalScaling(m_cci.m_scaling); - m_body->getCollisionShape()->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor); - m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor); - } - } -} - - // physics methods -void CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local) -{ - btVector3 torque(torqueX,torqueY,torqueZ); - btTransform xform = m_body->getCenterOfMassTransform(); - if (local) - { - torque = xform.getBasis()*torque; - } - m_body->applyTorque(torque); -} - -void CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local) -{ - btVector3 force(forceX,forceY,forceZ); - btTransform xform = m_body->getCenterOfMassTransform(); - if (local) - { - force = xform.getBasis()*force; - } - m_body->applyCentralForce(force); -} -void CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local) -{ - btVector3 angvel(ang_velX,ang_velY,ang_velZ); - if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON)) - { - m_body->activate(); - } - - { - btTransform xform = m_body->getCenterOfMassTransform(); - if (local) - { - angvel = xform.getBasis()*angvel; - } - - m_body->setAngularVelocity(angvel); - } - -} -void CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local) -{ - - btVector3 linVel(lin_velX,lin_velY,lin_velZ); - if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON)) - { - m_body->activate(); - } - - { - btTransform xform = m_body->getCenterOfMassTransform(); - if (local) - { - linVel = xform.getBasis()*linVel; - } - m_body->setLinearVelocity(linVel); - } -} -void CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ) -{ - btVector3 impulse(impulseX,impulseY,impulseZ); - - if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON)) - { - m_body->activate(); - - btVector3 pos(attachX,attachY,attachZ); - - m_body->activate(); - - m_body->applyImpulse(impulse,pos); - } - -} -void CcdPhysicsController::SetActive(bool active) -{ -} - // reading out information from physics -void CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ) -{ - const btVector3& linvel = this->m_body->getLinearVelocity(); - linvX = linvel.x(); - linvY = linvel.y(); - linvZ = linvel.z(); - -} - -void CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ) -{ - const btVector3& angvel= m_body->getAngularVelocity(); - angVelX = angvel.x(); - angVelY = angvel.y(); - angVelZ = angvel.z(); -} - -void CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ) -{ - btVector3 pos(posX,posY,posZ); - btVector3 rel_pos = pos-m_body->getCenterOfMassPosition(); - btVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos); - linvX = linvel.x(); - linvY = linvel.y(); - linvZ = linvel.z(); -} -void CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ) -{ -} - - // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted -void CcdPhysicsController::setRigidBody(bool rigid) -{ - if (!rigid) - { - //fake it for now - btVector3 inertia = m_body->getInvInertiaDiagLocal(); - inertia[1] = 0.f; - m_body->setInvInertiaDiagLocal(inertia); - m_body->updateInertiaTensor(); - } -} - - // clientinfo for raycasts for example -void* CcdPhysicsController::getNewClientInfo() -{ - return m_newClientInfo; -} -void CcdPhysicsController::setNewClientInfo(void* clientinfo) -{ - m_newClientInfo = clientinfo; -} - - -void CcdPhysicsController::UpdateDeactivation(float timeStep) -{ - if ( (m_body->GetActivationState() == ISLAND_SLEEPING) || (m_body->GetActivationState() == DISABLE_DEACTIVATION)) - return; - - if ((m_body->getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) && - (m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold)) - { - m_body->m_deactivationTime += timeStep; - } else - { - m_body->m_deactivationTime=0.f; - m_body->SetActivationState(0); - } - -} - -bool CcdPhysicsController::wantsSleeping() -{ - - if (m_body->GetActivationState() == DISABLE_DEACTIVATION) - return false; - - //disable deactivation - if (gDisableDeactivation || (gDeactivationTime == 0.f)) - return false; - - if ( (m_body->GetActivationState() == ISLAND_SLEEPING) || (m_body->GetActivationState() == WANTS_DEACTIVATION)) - return true; - - if (m_body->m_deactivationTime> gDeactivationTime) - { - return true; - } - return false; -} - -PHY_IPhysicsController* CcdPhysicsController::GetReplica() -{ - //very experimental, shape sharing is not implemented yet. - //just support btSphereShape/ConeShape for now - - btCcdConstructionInfo cinfo = m_cci; - if (cinfo.m_collisionShape) - { - switch (cinfo.m_collisionShape->getShapeType()) - { - case SPHERE_SHAPE_PROXYTYPE: - { - btSphereShape* orgShape = (btSphereShape*)cinfo.m_collisionShape; - cinfo.m_collisionShape = new btSphereShape(*orgShape); - break; - } - - case CONE_SHAPE_PROXYTYPE: - { - btConeShape* orgShape = (btConeShape*)cinfo.m_collisionShape; - cinfo.m_collisionShape = new btConeShape(*orgShape); - break; - } - - - default: - { - return 0; - } - } - } - - cinfo.m_MotionState = new DefaultMotionState(); - - CcdPhysicsController* replica = new CcdPhysicsController(cinfo); - return replica; -} - -/////////////////////////////////////////////////////////// -///A small utility class, DefaultMotionState -/// -/////////////////////////////////////////////////////////// - -DefaultMotionState::DefaultMotionState() -{ - m_worldTransform.setIdentity(); - m_localScaling.setValue(1.f,1.f,1.f); -} - - -DefaultMotionState::~DefaultMotionState() -{ - -} - -void DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ) -{ - posX = m_worldTransform.getOrigin().x(); - posY = m_worldTransform.getOrigin().y(); - posZ = m_worldTransform.getOrigin().z(); -} - -void DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ) -{ - scaleX = m_localScaling.getX(); - scaleY = m_localScaling.getY(); - scaleZ = m_localScaling.getZ(); -} - -void DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal) -{ - quatIma0 = m_worldTransform.getRotation().x(); - quatIma1 = m_worldTransform.getRotation().y(); - quatIma2 = m_worldTransform.getRotation().z(); - quatReal = m_worldTransform.getRotation()[3]; -} - -void DefaultMotionState::setWorldPosition(float posX,float posY,float posZ) -{ - btPoint3 pos(posX,posY,posZ); - m_worldTransform.setOrigin( pos ); -} - -void DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal) -{ - btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal); - m_worldTransform.setRotation( orn ); -} - -void DefaultMotionState::calculateWorldTransformations() -{ - -} - diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h deleted file mode 100644 index 4c4f14fb3..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h +++ /dev/null @@ -1,254 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BULLET2_PHYSICSCONTROLLER_H -#define BULLET2_PHYSICSCONTROLLER_H - -#include "PHY_IPhysicsController.h" - -/// PHY_IPhysicsController is the abstract simplified Interface to a physical object. -/// It contains the IMotionState and IDeformableMesh Interfaces. -#include "LinearMath/btVector3.h" -#include "LinearMath/btScalar.h" -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btTransform.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" - -#include "PHY_IMotionState.h" - -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for btCollisionShape access -class btCollisionShape; - -extern float gDeactivationTime; -extern float gLinearSleepingTreshold; -extern float gAngularSleepingTreshold; -extern bool gDisableDeactivation; -class CcdPhysicsEnvironment; - - - - -struct btCcdConstructionInfo -{ - - ///CollisionFilterGroups provides some optional usage of basic collision filtering - ///this is done during broadphase, so very early in the pipeline - ///more advanced collision filtering should be done in btCollisionDispatcher::needsCollision - enum CollisionFilterGroups - { - DefaultFilter = 1, - StaticFilter = 2, - KinematicFilter = 4, - DebrisFilter = 8, - AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter, - }; - - - btCcdConstructionInfo() - : m_gravity(0,0,0), - m_scaling(1.f,1.f,1.f), - m_mass(0.f), - m_restitution(0.1f), - m_friction(0.5f), - m_linearDamping(0.1f), - m_angularDamping(0.1f), - m_collisionFlags(0), - m_collisionFilterGroup(DefaultFilter), - m_collisionFilterMask(AllFilter), - m_MotionState(0), - m_physicsEnv(0), - m_inertiaFactor(1.f) - { - } - - btVector3 m_localInertiaTensor; - btVector3 m_gravity; - btVector3 m_scaling; - btScalar m_mass; - btScalar m_restitution; - btScalar m_friction; - btScalar m_linearDamping; - btScalar m_angularDamping; - int m_collisionFlags; - - ///optional use of collision group/mask: - ///only collision with object goups that match the collision mask. - ///this is very basic early out. advanced collision filtering should be - ///done in the btCollisionDispatcher::needsCollision and needsResponse - ///both values default to 1 - short int m_collisionFilterGroup; - short int m_collisionFilterMask; - - - btCollisionShape* m_collisionShape; - class PHY_IMotionState* m_MotionState; - - CcdPhysicsEnvironment* m_physicsEnv; //needed for self-replication - float m_inertiaFactor;//tweak the inertia (hooked up to Blender 'formfactor' -}; - - -class btRigidBody; - -///CcdPhysicsController is a physics object that supports continuous collision detection and time of impact based physics resolution. -class CcdPhysicsController : public PHY_IPhysicsController -{ - btRigidBody* m_body; - class PHY_IMotionState* m_MotionState; - - - void* m_newClientInfo; - - btCcdConstructionInfo m_cci;//needed for replication - void GetWorldOrientation(btMatrix3x3& mat); - - void CreateRigidbody(); - - public: - - int m_collisionDelay; - - - CcdPhysicsController (const btCcdConstructionInfo& ci); - - virtual ~CcdPhysicsController(); - - - btRigidBody* getRigidBody() { return m_body;} - - btCollisionShape* getCollisionShape() { - return m_body->getCollisionShape(); - } - //////////////////////////////////// - // PHY_IPhysicsController interface - //////////////////////////////////// - - - /** - SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding') - */ - virtual bool SynchronizeMotionStates(float time); - /** - WriteMotionStateToDynamics ynchronizes dynas, kinematic and deformable entities (and do 'late binding') - */ - - virtual void WriteMotionStateToDynamics(bool nondynaonly); - virtual void WriteDynamicsToMotionState(); - // controller replication - virtual void PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl); - - // kinematic methods - virtual void RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local); - virtual void RelativeRotate(const float drot[9],bool local); - virtual void getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal); - virtual void setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal); - virtual void setPosition(float posX,float posY,float posZ); - virtual void getPosition(PHY__Vector3& pos) const; - - virtual void setScaling(float scaleX,float scaleY,float scaleZ); - - // physics methods - virtual void ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local); - virtual void ApplyForce(float forceX,float forceY,float forceZ,bool local); - virtual void SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local); - virtual void SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local); - virtual void applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ); - virtual void SetActive(bool active); - - // reading out information from physics - virtual void GetLinearVelocity(float& linvX,float& linvY,float& linvZ); - virtual void GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ); - virtual void GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ); - virtual void getReactionForce(float& forceX,float& forceY,float& forceZ); - - // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted - virtual void setRigidBody(bool rigid); - - - virtual void resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ); - - // clientinfo for raycasts for example - virtual void* getNewClientInfo(); - virtual void setNewClientInfo(void* clientinfo); - virtual PHY_IPhysicsController* GetReplica(); - - ///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors - short int GetCollisionFilterGroup() const - { - return m_cci.m_collisionFilterGroup; - } - ///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors - short int GetCollisionFilterMask() const - { - return m_cci.m_collisionFilterMask; - } - - - virtual void calcXform() {} ; - virtual void setMargin(float margin) {}; - virtual float getMargin() const {return 0.f;}; - - - bool wantsSleeping(); - - void UpdateDeactivation(float timeStep); - - static btTransform GetTransformFromMotionState(PHY_IMotionState* motionState); - - void setAabb(const btVector3& aabbMin,const btVector3& aabbMax); - - - class PHY_IMotionState* GetMotionState() - { - return m_MotionState; - } - - const class PHY_IMotionState* GetMotionState() const - { - return m_MotionState; - } - - -}; - - - - -///DefaultMotionState implements standard motionstate, using btTransform -class DefaultMotionState : public PHY_IMotionState - -{ - public: - DefaultMotionState(); - - virtual ~DefaultMotionState(); - - virtual void getWorldPosition(float& posX,float& posY,float& posZ); - virtual void getWorldScaling(float& scaleX,float& scaleY,float& scaleZ); - virtual void getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal); - - virtual void setWorldPosition(float posX,float posY,float posZ); - virtual void setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal); - - virtual void calculateWorldTransformations(); - - btTransform m_worldTransform; - btVector3 m_localScaling; - -}; - - -#endif //BULLET2_PHYSICSCONTROLLER_H diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp deleted file mode 100644 index cd782a86f..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ /dev/null @@ -1,1854 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - - -#include "CcdPhysicsEnvironment.h" -#include "CcdPhysicsController.h" - -#include -#include "LinearMath/btTransform.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/BroadphaseCollision/btAxisSweep3.h" - -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" - -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/CollisionShapes/btConeShape.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" - -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" - - - -//profiling/timings -#include "LinearMath/btQuickprof.h" - - -#include "LinearMath/btIDebugDraw.h" - -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" - - -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "PHY_IMotionState.h" - -#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" - - - -#include "BulletCollision/CollisionShapes/btSphereShape.h" - -bool useIslands = true; - -#ifdef NEW_BULLET_VEHICLE_SUPPORT -#include "BulletDynamics/Vehicle/btRaycastVehicle.h" -#include "BulletDynamics/Vehicle/btVehicleRaycaster.h" - -#include "BulletDynamics/Vehicle/btWheelInfo.h" -#include "PHY_IVehicle.h" -btRaycastVehicle::btVehicleTuning gTuning; - -#endif //NEW_BULLET_VEHICLE_SUPPORT -#include "LinearMath/btAabbUtil2.h" - -#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" -#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" -#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" - - -//#include "BroadphaseCollision/QueryDispatcher.h" -//#include "BroadphaseCollision/QueryBox.h" -//todo: change this to allow dynamic registration of types! - -#ifdef WIN32 -void DrawRasterizerLine(const float* from,const float* to,int color); -#endif - - -#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" - - -#include - -#ifdef NEW_BULLET_VEHICLE_SUPPORT -class WrapperVehicle : public PHY_IVehicle -{ - - btRaycastVehicle* m_vehicle; - PHY_IPhysicsController* m_chassis; - -public: - - WrapperVehicle(btRaycastVehicle* vehicle,PHY_IPhysicsController* chassis) - :m_vehicle(vehicle), - m_chassis(chassis) - { - } - - btRaycastVehicle* GetVehicle() - { - return m_vehicle; - } - - PHY_IPhysicsController* GetChassis() - { - return m_chassis; - } - - virtual void addWheel( - PHY_IMotionState* motionState, - PHY__Vector3 connectionPoint, - PHY__Vector3 downDirection, - PHY__Vector3 axleDirection, - float suspensionRestLength, - float wheelRadius, - bool hasSteering - ) - { - btVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]); - btVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]); - btVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]); - - - btWheelInfo& info = m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle, - suspensionRestLength,wheelRadius,gTuning,hasSteering); - info.m_clientInfo = motionState; - - } - - void SyncWheels() - { - int numWheels = getNumWheels(); - int i; - for (i=0;igetWheelInfo(i); - PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ; - m_vehicle->updateWheelTransform(i); - btTransform trans = m_vehicle->getWheelTransformWS(i); - btQuaternion orn = trans.getRotation(); - const btVector3& pos = trans.getOrigin(); - motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]); - motionState->setWorldPosition(pos.x(),pos.y(),pos.z()); - - } - } - - virtual int getNumWheels() const - { - return m_vehicle->getNumWheels(); - } - - virtual void GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const - { - btTransform trans = m_vehicle->getWheelTransformWS(wheelIndex); - posX = trans.getOrigin().x(); - posY = trans.getOrigin().y(); - posZ = trans.getOrigin().z(); - } - virtual void GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const - { - btTransform trans = m_vehicle->getWheelTransformWS(wheelIndex); - btQuaternion quat = trans.getRotation(); - btMatrix3x3 orn2(quat); - - quatX = trans.getRotation().x(); - quatY = trans.getRotation().y(); - quatZ = trans.getRotation().z(); - quatW = trans.getRotation()[3]; - - - //printf("test"); - - - } - - virtual float GetWheelRotation(int wheelIndex) const - { - float rotation = 0.f; - - if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels())) - { - btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex); - rotation = info.m_rotation; - } - return rotation; - - } - - - - virtual int getUserConstraintId() const - { - return m_vehicle->getUserConstraintId(); - } - - virtual int getUserConstraintType() const - { - return m_vehicle->getUserConstraintType(); - } - - virtual void setSteeringValue(float steering,int wheelIndex) - { - m_vehicle->setSteeringValue(steering,wheelIndex); - } - - virtual void applyEngineForce(float force,int wheelIndex) - { - m_vehicle->applyEngineForce(force,wheelIndex); - } - - virtual void ApplyBraking(float braking,int wheelIndex) - { - if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels())) - { - btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex); - info.m_brake = braking; - } - } - - virtual void SetWheelFriction(float friction,int wheelIndex) - { - if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels())) - { - btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex); - info.m_frictionSlip = friction; - } - - } - - virtual void SetSuspensionStiffness(float suspensionStiffness,int wheelIndex) - { - if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels())) - { - btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex); - info.m_suspensionStiffness = suspensionStiffness; - - } - } - - virtual void SetSuspensionDamping(float suspensionDamping,int wheelIndex) - { - if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels())) - { - btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex); - info.m_wheelsDampingRelaxation = suspensionDamping; - } - } - - virtual void SetSuspensionCompression(float suspensionCompression,int wheelIndex) - { - if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels())) - { - btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex); - info.m_wheelsDampingCompression = suspensionCompression; - } - } - - - - virtual void SetRollInfluence(float rollInfluence,int wheelIndex) - { - if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels())) - { - btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex); - info.m_rollInfluence = rollInfluence; - } - } - - virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex) - { - m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex); - } - - - -}; -#endif //NEW_BULLET_VEHICLE_SUPPORT - - - -static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color) -{ - btVector3 halfExtents = (to-from)* 0.5f; - btVector3 center = (to+from) *0.5f; - int i,j; - - btVector3 edgecoord(1.f,1.f,1.f),pa,pb; - for (i=0;i<4;i++) - { - for (j=0;j<3;j++) - { - pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], - edgecoord[2]*halfExtents[2]); - pa+=center; - - int othercoord = j%3; - edgecoord[othercoord]*=-1.f; - pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], - edgecoord[2]*halfExtents[2]); - pb+=center; - - debugDrawer->drawLine(pa,pb,color); - } - edgecoord = btVector3(-1.f,-1.f,-1.f); - if (i<3) - edgecoord[i]*=-1.f; - } - - -} - - - - - - -CcdPhysicsEnvironment::CcdPhysicsEnvironment(btDispatcher* dispatcher,btOverlappingPairCache* pairCache) -: m_numIterations(10), -m_numTimeSubSteps(1), -m_ccdMode(0), -m_solverType(-1), -m_profileTimings(0), -m_enableSatCollisionDetection(false), -m_scalingPropagated(false) -{ - - for (int i=0;igetRigidBody(); - - //this m_userPointer is just used for triggers, see CallbackTriggers - body->m_internalOwner = ctrl; - - body->setGravity( m_gravity ); - m_controllers.push_back(ctrl); - - m_collisionWorld->addCollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask()); - - assert(body->m_broadphaseHandle); - - btCollisionShape* shapeinterface = ctrl->getCollisionShape(); - - assert(shapeinterface); - - const btTransform& t = ctrl->getRigidBody()->getCenterOfMassTransform(); - - body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); - - btPoint3 minAabb,maxAabb; - - shapeinterface->getAabb(t,minAabb,maxAabb); - - float timeStep = 0.02f; - - - //extent it with the motion - - btVector3 linMotion = body->getLinearVelocity()*timeStep; - - float maxAabbx = maxAabb.getX(); - float maxAabby = maxAabb.getY(); - float maxAabbz = maxAabb.getZ(); - float minAabbx = minAabb.getX(); - float minAabby = minAabb.getY(); - float minAabbz = minAabb.getZ(); - - if (linMotion.x() > 0.f) - maxAabbx += linMotion.x(); - else - minAabbx += linMotion.x(); - if (linMotion.y() > 0.f) - maxAabby += linMotion.y(); - else - minAabby += linMotion.y(); - if (linMotion.z() > 0.f) - maxAabbz += linMotion.z(); - else - minAabbz += linMotion.z(); - - - minAabb = btVector3(minAabbx,minAabby,minAabbz); - maxAabb = btVector3(maxAabbx,maxAabby,maxAabbz); - - - - -} - -void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl) -{ - - //also remove constraint - - { - std::vector::iterator i; - - for (i=m_constraints.begin(); - !(i==m_constraints.end()); i++) - { - btTypedConstraint* constraint = (*i); - if ((&constraint->getRigidBodyA() == ctrl->getRigidBody() || - (&constraint->getRigidBodyB() == ctrl->getRigidBody()))) - { - removeConstraint(constraint->getUserConstraintId()); - //only 1 constraint per constroller - break; - } - } - } - - { - std::vector::iterator i; - - for (i=m_constraints.begin(); - !(i==m_constraints.end()); i++) - { - btTypedConstraint* constraint = (*i); - if ((&constraint->getRigidBodyA() == ctrl->getRigidBody() || - (&constraint->getRigidBodyB() == ctrl->getRigidBody()))) - { - removeConstraint(constraint->getUserConstraintId()); - //only 1 constraint per constroller - break; - } - } - } - - - m_collisionWorld->removeCollisionObject(ctrl->getRigidBody()); - - - { - std::vector::iterator i = - std::find(m_controllers.begin(), m_controllers.end(), ctrl); - if (!(i == m_controllers.end())) - { - std::swap(*i, m_controllers.back()); - m_controllers.pop_back(); - } - } - - //remove it from the triggers - { - std::vector::iterator i = - std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl); - if (!(i == m_triggerControllers.end())) - { - std::swap(*i, m_triggerControllers.back()); - m_triggerControllers.pop_back(); - } - } - - -} - - -void CcdPhysicsEnvironment::beginFrame() -{ - -} - - -bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) -{ - //don't simulate without timesubsteps - if (m_numTimeSubSteps<1) - return true; - - //printf("proceedDeltaTime\n"); - - -#ifdef USE_QUICKPROF - //toggle btProfiler - if ( m_debugDrawer && m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_ProfileTimings) - { - if (!m_profileTimings) - { - m_profileTimings = 1; - // To disable profiling, simply comment out the following line. - static int counter = 0; - - char filename[128]; - sprintf(filename,"quickprof_bullet_timings%i.csv",counter++); - btProfiler::init(filename, btProfiler::BLOCK_CYCLE_SECONDS);//BLOCK_TOTAL_MICROSECONDS - - } - } else - { - if (m_profileTimings) - { - m_profileTimings = 0; - btProfiler::destroy(); - } - } -#endif //USE_QUICKPROF - - - - if (!btFuzzyZero(timeStep)) - { - - { - //do the kinematic calculation here, over the full timestep - std::vector::iterator i; - for (i=m_controllers.begin(); - !(i==m_controllers.end()); i++) - { - - CcdPhysicsController* ctrl = *i; - - btTransform predictedTrans; - btRigidBody* body = ctrl->getRigidBody(); - if (body->GetActivationState() != ISLAND_SLEEPING) - { - - if (body->IsStatic()) - { - //to calculate velocities next frame - body->saveKinematicState(timeStep); - } - } - } - } - - - int i; - float subTimeStep = timeStep / float(m_numTimeSubSteps); - - for (i=0;im_numTimeSubSteps;i++) - { - proceedDeltaTimeOneStep(subTimeStep); - } - } else - { - //todo: interpolate - } - - return true; -} - - - - - - - - - - - - - -/// Perform an integration step of duration 'timeStep'. -bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) -{ - - - //printf("CcdPhysicsEnvironment::proceedDeltaTime\n"); - - if (btFuzzyZero(timeStep)) - return true; - - if (m_debugDrawer) - { - gDisableDeactivation = (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation); - } - - -#ifdef USE_QUICKPROF - btProfiler::beginBlock("SyncMotionStates"); -#endif //USE_QUICKPROF - - - //this is needed because scaling is not known in advance, and scaling has to propagate to the shape - if (!m_scalingPropagated) - { - SyncMotionStates(timeStep); - m_scalingPropagated = true; - } - - -#ifdef USE_QUICKPROF - btProfiler::endBlock("SyncMotionStates"); - - btProfiler::beginBlock("predictIntegratedTransform"); -#endif //USE_QUICKPROF - - { - // std::vector::iterator i; - - - - int k; - for (k=0;kgetRigidBody(); - - body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); - - if (body->IsActive()) - { - if (!body->IsStatic()) - { - body->applyForces( timeStep); - body->integrateVelocities( timeStep); - body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform); - } - } - - } - } - -#ifdef USE_QUICKPROF - btProfiler::endBlock("predictIntegratedTransform"); -#endif //USE_QUICKPROF - - btOverlappingPairCache* scene = m_collisionWorld->getPairCache(); - - - // - // collision detection (?) - // - - -#ifdef USE_QUICKPROF - btProfiler::beginBlock("dispatchAllCollisionPairs"); -#endif //USE_QUICKPROF - - - int numsubstep = m_numIterations; - - - btDispatcherInfo dispatchInfo; - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection; - dispatchInfo.m_debugDraw = this->m_debugDrawer; - - scene->refreshOverlappingPairs(); - - getCollisionWorld()->getDispatcher()->dispatchAllCollisionPairs(scene,dispatchInfo); - - -#ifdef USE_QUICKPROF - btProfiler::endBlock("dispatchAllCollisionPairs"); -#endif //USE_QUICKPROF - - m_islandManager->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); - - { - int i; - int numConstraints = m_constraints.size(); - for (i=0;i< numConstraints ; i++ ) - { - btTypedConstraint* constraint = m_constraints[i]; - - const btRigidBody* colObj0 = &constraint->getRigidBodyA(); - const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - - if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && - ((colObj1) && ((colObj1)->mergesSimulationIslands()))) - { - if (colObj0->IsActive() || colObj1->IsActive()) - { - - m_islandManager->getUnionFind().unite((colObj0)->m_islandTag1, - (colObj1)->m_islandTag1); - } - } - } - } - - m_islandManager->storeIslandActivationState(getCollisionWorld()); - - - //contacts -#ifdef USE_QUICKPROF - btProfiler::beginBlock("solveConstraint"); -#endif //USE_QUICKPROF - - - //solve the regular constraints (point 2 point, hinge, etc) - - for (int g=0;gbuildJacobian(); - constraint->solveConstraint( timeStep ); - - } - - - } - -#ifdef USE_QUICKPROF - btProfiler::endBlock("solveConstraint"); -#endif //USE_QUICKPROF - - //solve the vehicles - -#ifdef NEW_BULLET_VEHICLE_SUPPORT - //vehicles - int numVehicles = m_wrapperVehicles.size(); - for (int i=0;iGetVehicle(); - vehicle->updateVehicle( timeStep); - } -#endif //NEW_BULLET_VEHICLE_SUPPORT - - - struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback - { - - btContactSolverInfo& m_solverInfo; - btConstraintSolver* m_solver; - btIDebugDraw* m_debugDrawer; - - InplaceSolverIslandCallback( - btContactSolverInfo& solverInfo, - btConstraintSolver* solver, - btIDebugDraw* debugDrawer) - :m_solverInfo(solverInfo), - m_solver(solver), - m_debugDrawer(debugDrawer) - { - - } - - virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds) - { - m_solver->solveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer); - } - - }; - - - m_solverInfo.m_friction = 0.9f; - m_solverInfo.m_numIterations = m_numIterations; - m_solverInfo.m_timeStep = timeStep; - m_solverInfo.m_restitution = 0.f;//m_restitution; - - InplaceSolverIslandCallback solverCallback( - m_solverInfo, - m_solver, - m_debugDrawer); - -#ifdef USE_QUICKPROF - btProfiler::beginBlock("buildAndProcessIslands"); -#endif //USE_QUICKPROF - - /// solve all the contact points and contact friction - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),m_collisionWorld->getCollisionObjectArray(),&solverCallback); - -#ifdef USE_QUICKPROF - btProfiler::endBlock("buildAndProcessIslands"); - - btProfiler::beginBlock("CallbackTriggers"); -#endif //USE_QUICKPROF - - CallbackTriggers(); - -#ifdef USE_QUICKPROF - btProfiler::endBlock("CallbackTriggers"); - - - btProfiler::beginBlock("proceedToTransform"); - -#endif //USE_QUICKPROF - { - - - - { - - - - UpdateAabbs(timeStep); - - - float toi = 1.f; - - - - if (m_ccdMode == 3) - { - btDispatcherInfo dispatchInfo; - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS; - - //pairCache->refreshOverlappingPairs();//?? - getCollisionWorld()->getDispatcher()->dispatchAllCollisionPairs(scene,dispatchInfo); - - toi = dispatchInfo.m_timeOfImpact; - - } - - - - // - // integrating solution - // - - { - - std::vector::iterator i; - - for (i=m_controllers.begin(); - !(i==m_controllers.end()); i++) - { - - CcdPhysicsController* ctrl = *i; - - btTransform predictedTrans; - btRigidBody* body = ctrl->getRigidBody(); - - if (body->IsActive()) - { - - if (!body->IsStatic()) - { - if (body->m_hitFraction < 1.f) - { - //set velocity to zero... until we have proper CCD integrated - body->setLinearVelocity(body->getLinearVelocity()*0.5f); - } - body->predictIntegratedTransform(timeStep* toi, predictedTrans); - body->proceedToTransform( predictedTrans); - } - - } - } - - } - - - - - - // - // disable sleeping physics objects - // - - std::vector m_sleepingControllers; - - std::vector::iterator i; - - for (i=m_controllers.begin(); - !(i==m_controllers.end()); i++) - { - CcdPhysicsController* ctrl = (*i); - btRigidBody* body = ctrl->getRigidBody(); - - ctrl->UpdateDeactivation(timeStep); - - - if (ctrl->wantsSleeping()) - { - if (body->GetActivationState() == ACTIVE_TAG) - body->SetActivationState( WANTS_DEACTIVATION ); - } else - { - if (body->GetActivationState() != DISABLE_DEACTIVATION) - body->SetActivationState( ACTIVE_TAG ); - } - - if (useIslands) - { - if (body->GetActivationState() == ISLAND_SLEEPING) - { - m_sleepingControllers.push_back(ctrl); - } - } else - { - if (ctrl->wantsSleeping()) - { - m_sleepingControllers.push_back(ctrl); - } - } - } - - - - - } - - -#ifdef USE_QUICKPROF - btProfiler::endBlock("proceedToTransform"); - - btProfiler::beginBlock("SyncMotionStates"); -#endif //USE_QUICKPROF - - SyncMotionStates(timeStep); - -#ifdef USE_QUICKPROF - btProfiler::endBlock("SyncMotionStates"); - - btProfiler::endProfilingCycle(); -#endif //USE_QUICKPROF - - -#ifdef NEW_BULLET_VEHICLE_SUPPORT - //sync wheels for vehicles - int numVehicles = m_wrapperVehicles.size(); - for (int i=0;iSyncWheels(); - } -#endif //NEW_BULLET_VEHICLE_SUPPORT - } - - return true; -} - -void CcdPhysicsEnvironment::setDebugMode(int debugMode) -{ - if (m_debugDrawer){ - m_debugDrawer->setDebugMode(debugMode); - } -} - -void CcdPhysicsEnvironment::setNumIterations(int numIter) -{ - m_numIterations = numIter; -} -void CcdPhysicsEnvironment::setDeactivationTime(float dTime) -{ - gDeactivationTime = dTime; -} -void CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh) -{ - gLinearSleepingTreshold = linTresh; -} -void CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) -{ - gAngularSleepingTreshold = angTresh; -} -void CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold) -{ - gContactBreakingTreshold = contactBreakingTreshold; - -} - - -void CcdPhysicsEnvironment::setCcdMode(int ccdMode) -{ - m_ccdMode = ccdMode; -} - - -void CcdPhysicsEnvironment::setSolverSorConstant(float sor) -{ - m_solverInfo.m_sor = sor; -} - -void CcdPhysicsEnvironment::setSolverTau(float tau) -{ - m_solverInfo.m_tau = tau; -} -void CcdPhysicsEnvironment::setSolverDamping(float damping) -{ - m_solverInfo.m_damping = damping; -} - - -void CcdPhysicsEnvironment::setLinearAirDamping(float damping) -{ - gLinearAirDamping = damping; -} - -void CcdPhysicsEnvironment::setUseEpa(bool epa) -{ - gUseEpa = epa; -} - -void CcdPhysicsEnvironment::setSolverType(int solverType) -{ - - switch (solverType) - { - case 1: - { - if (m_solverType != solverType) - { - - m_solver = new btSequentialImpulseConstraintSolver(); - - break; - } - } - - case 0: - default: - if (m_solverType != solverType) - { -// m_solver = new OdeConstraintSolver(); - - break; - } - - }; - - m_solverType = solverType ; -} - - - - - -void CcdPhysicsEnvironment::SyncMotionStates(float timeStep) -{ - std::vector::iterator i; - - // - // synchronize the physics and graphics transformations - // - - for (i=m_controllers.begin(); - !(i==m_controllers.end()); i++) - { - CcdPhysicsController* ctrl = (*i); - ctrl->SynchronizeMotionStates(timeStep); - - } - -} -void CcdPhysicsEnvironment::setGravity(float x,float y,float z) -{ - m_gravity = btVector3(x,y,z); - - std::vector::iterator i; - - //todo: review this gravity stuff - for (i=m_controllers.begin(); - !(i==m_controllers.end()); i++) - { - - CcdPhysicsController* ctrl = (*i); - ctrl->getRigidBody()->setGravity(m_gravity); - - } -} - - -#ifdef NEW_BULLET_VEHICLE_SUPPORT - -class DefaultVehicleRaycaster : public btVehicleRaycaster -{ - CcdPhysicsEnvironment* m_physEnv; - PHY_IPhysicsController* m_chassis; - -public: - DefaultVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis): - m_physEnv(physEnv), - m_chassis(chassis) - { - } - - virtual ~DefaultVehicleRaycaster() - { - } - /* struct btVehicleRaycasterResult - { - btVehicleRaycasterResult() :m_distFraction(-1.f){}; - btVector3 m_hitPointInWorld; - btVector3 m_hitNormalInWorld; - btScalar m_distFraction; - }; - */ - virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) - { - - - float hit[3]; - float normal[3]; - - PHY_IPhysicsController* ignore = m_chassis; - void* hitObject = m_physEnv->rayTest(ignore,from.x(),from.y(),from.z(),to.x(),to.y(),to.z(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]); - if (hitObject) - { - result.m_hitPointInWorld[0] = hit[0]; - result.m_hitPointInWorld[1] = hit[1]; - result.m_hitPointInWorld[2] = hit[2]; - result.m_hitNormalInWorld[0] = normal[0]; - result.m_hitNormalInWorld[1] = normal[1]; - result.m_hitNormalInWorld[2] = normal[2]; - result.m_hitNormalInWorld.normalize(); - //calc fraction? or put it in the interface? - //calc for now - - result.m_distFraction = (result.m_hitPointInWorld-from).length() / (to-from).length(); - //some safety for 'explosion' due to sudden penetration of the full 'ray' - /* if (result.m_distFraction<0.1) - { - printf("Vehicle rayCast: avoided instability due to penetration. Consider moving the connection points deeper inside vehicle chassis"); - result.m_distFraction = 1.f; - hitObject = 0; - } - */ - - /* if (result.m_distFraction>1.) - { - printf("Vehicle rayCast: avoided instability 1Consider moving the connection points deeper inside vehicle chassis"); - result.m_distFraction = 1.f; - hitObject = 0; - } - */ - - - - } - //? - return hitObject; - } -}; -#endif //NEW_BULLET_VEHICLE_SUPPORT - -static int gConstraintUid = 1; - -int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type, - float pivotX,float pivotY,float pivotZ, - float axisX,float axisY,float axisZ) -{ - - - CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0; - CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1; - - btRigidBody* rb0 = c0 ? c0->getRigidBody() : 0; - btRigidBody* rb1 = c1 ? c1->getRigidBody() : 0; - - ASSERT(rb0); - - btVector3 pivotInA(pivotX,pivotY,pivotZ); - btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA; - btVector3 axisInA(axisX,axisY,axisZ); - btVector3 axisInB = rb1 ? - (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : - rb0->getCenterOfMassTransform().getBasis() * axisInA; - - bool angularOnly = false; - - switch (type) - { - case PHY_POINT2POINT_CONSTRAINT: - { - - btPoint2PointConstraint* p2p = 0; - - if (rb1) - { - p2p = new btPoint2PointConstraint(*rb0, - *rb1,pivotInA,pivotInB); - } else - { - p2p = new btPoint2PointConstraint(*rb0, - pivotInA); - } - - m_constraints.push_back(p2p); - p2p->setUserConstraintId(gConstraintUid++); - p2p->setUserConstraintType(type); - //64 bit systems can't cast pointer to int. could use size_t instead. - return p2p->getUserConstraintId(); - - break; - } - - case PHY_GENERIC_6DOF_CONSTRAINT: - { - btGeneric6DofConstraint* genericConstraint = 0; - - if (rb1) - { - btTransform frameInA; - btTransform frameInB; - - btVector3 axis1, axis2; - btPlaneSpace1( axisInA, axis1, axis2 ); - - frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(), - axisInA.y(), axis1.y(), axis2.y(), - axisInA.z(), axis1.z(), axis2.z() ); - - - btPlaneSpace1( axisInB, axis1, axis2 ); - frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(), - axisInB.y(), axis1.y(), axis2.y(), - axisInB.z(), axis1.z(), axis2.z() ); - - frameInA.setOrigin( pivotInA ); - frameInB.setOrigin( pivotInB ); - - genericConstraint = new btGeneric6DofConstraint( - *rb0,*rb1, - frameInA,frameInB); - - - } else - { - // TODO: Implement single body case... - - } - - - m_constraints.push_back(genericConstraint); - genericConstraint->setUserConstraintId(gConstraintUid++); - genericConstraint->setUserConstraintType(type); - //64 bit systems can't cast pointer to int. could use size_t instead. - return genericConstraint->getUserConstraintId(); - - break; - } - case PHY_ANGULAR_CONSTRAINT: - angularOnly = true; - - - case PHY_LINEHINGE_CONSTRAINT: - { - btHingeConstraint* hinge = 0; - - if (rb1) - { - hinge = new btHingeConstraint( - *rb0, - *rb1,pivotInA,pivotInB,axisInA,axisInB); - - - } else - { - hinge = new btHingeConstraint(*rb0, - pivotInA,axisInA); - - } - hinge->setAngularOnly(angularOnly); - - m_constraints.push_back(hinge); - hinge->setUserConstraintId(gConstraintUid++); - hinge->setUserConstraintType(type); - //64 bit systems can't cast pointer to int. could use size_t instead. - return hinge->getUserConstraintId(); - break; - } -#ifdef NEW_BULLET_VEHICLE_SUPPORT - - case PHY_VEHICLE_CONSTRAINT: - { - btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning(); - btRigidBody* chassis = rb0; - DefaultVehicleRaycaster* raycaster = new DefaultVehicleRaycaster(this,ctrl0); - btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster); - WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0); - m_wrapperVehicles.push_back(wrapperVehicle); - vehicle->setUserConstraintId(gConstraintUid++); - vehicle->setUserConstraintType(type); - return vehicle->getUserConstraintId(); - - break; - }; -#endif //NEW_BULLET_VEHICLE_SUPPORT - - default: - { - } - }; - - //RigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB - - return 0; - -} - - - - -//Following the COLLADA physics specification for constraints -int CcdPhysicsEnvironment::createUniversalD6Constraint( - class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther, - btTransform& frameInA, - btTransform& frameInB, - const btVector3& linearMinLimits, - const btVector3& linearMaxLimits, - const btVector3& angularMinLimits, - const btVector3& angularMaxLimits -) -{ - - //we could either add some logic to recognize ball-socket and hinge, or let that up to the user - //perhaps some warning or hint that hinge/ball-socket is more efficient? - - btGeneric6DofConstraint* genericConstraint = 0; - CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef; - CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther; - - btRigidBody* rb0 = ctrl0->getRigidBody(); - btRigidBody* rb1 = ctrl1->getRigidBody(); - - if (rb1) - { - - - genericConstraint = new btGeneric6DofConstraint( - *rb0,*rb1, - frameInA,frameInB); - genericConstraint->setLinearLowerLimit(linearMinLimits); - genericConstraint->setLinearUpperLimit(linearMaxLimits); - genericConstraint->setAngularLowerLimit(angularMinLimits); - genericConstraint->setAngularUpperLimit(angularMaxLimits); - } else - { - // TODO: Implement single body case... - //No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code - - } - - if (genericConstraint) - { - m_constraints.push_back(genericConstraint); - genericConstraint->setUserConstraintId(gConstraintUid++); - genericConstraint->setUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT); - //64 bit systems can't cast pointer to int. could use size_t instead. - return genericConstraint->getUserConstraintId(); - } - return 0; -} - - - -void CcdPhysicsEnvironment::removeConstraint(int constraintId) -{ - std::vector::iterator i; - - for (i=m_constraints.begin(); - !(i==m_constraints.end()); i++) - { - btTypedConstraint* constraint = (*i); - if (constraint->getUserConstraintId() == constraintId) - { - std::swap(*i, m_constraints.back()); - m_constraints.pop_back(); - break; - } - } - -} - - - struct FilterClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback - { - PHY_IPhysicsController* m_ignoreClient; - - FilterClosestRayResultCallback (PHY_IPhysicsController* ignoreClient,const btVector3& rayFrom,const btVector3& rayTo) - : btCollisionWorld::ClosestRayResultCallback(rayFrom,rayTo), - m_ignoreClient(ignoreClient) - { - - } - - virtual ~FilterClosestRayResultCallback() - { - } - - virtual float AddSingleResult(const btCollisionWorld::LocalRayResult& rayResult) - { - CcdPhysicsController* curHit = static_cast(rayResult.m_collisionObject->m_internalOwner); - //ignore client... - if (curHit != m_ignoreClient) - { - //if valid - return ClosestRayResultCallback::AddSingleResult(rayResult); - } - return m_closestHitFraction; - } - - }; - -PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, - float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ) -{ - btVector3 rayFrom(fromX,fromY,fromZ); - btVector3 rayTo(toX,toY,toZ); - - btVector3 hitPointWorld,normalWorld; - - //Either Ray Cast with or without filtering - - //CollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo); - FilterClosestRayResultCallback rayCallback(ignoreClient,rayFrom,rayTo); - - - PHY_IPhysicsController* nearestHit = 0; - - m_collisionWorld->rayTest(rayFrom,rayTo,rayCallback); - if (rayCallback.HasHit()) - { - nearestHit = static_cast(rayCallback.m_collisionObject->m_internalOwner); - hitX = rayCallback.m_hitPointWorld.getX(); - hitY = rayCallback.m_hitPointWorld.getY(); - hitZ = rayCallback.m_hitPointWorld.getZ(); - - normalX = rayCallback.m_hitNormalWorld.getX(); - normalY = rayCallback.m_hitNormalWorld.getY(); - normalZ = rayCallback.m_hitNormalWorld.getZ(); - - } - - - return nearestHit; -} - - - -int CcdPhysicsEnvironment::getNumContactPoints() -{ - return 0; -} - -void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ) -{ - -} - - - - -btBroadphaseInterface* CcdPhysicsEnvironment::getBroadphase() -{ - return m_collisionWorld->getBroadphase(); -} - - - - -CcdPhysicsEnvironment::~CcdPhysicsEnvironment() -{ - -#ifdef NEW_BULLET_VEHICLE_SUPPORT - m_wrapperVehicles.clear(); -#endif //NEW_BULLET_VEHICLE_SUPPORT - - //m_broadphase->DestroyScene(); - //delete broadphase ? release reference on broadphase ? - - //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher - //delete m_dispatcher; - delete m_collisionWorld; - - delete m_islandManager; - -} - - -int CcdPhysicsEnvironment::GetNumControllers() -{ - return m_controllers.size(); -} - - -CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index) -{ - return m_controllers[index]; -} - - - - - - -btTypedConstraint* CcdPhysicsEnvironment::getConstraintById(int constraintId) -{ - int numConstraint = m_constraints.size(); - int i; - for (i=0;igetUserConstraintId()==constraintId) - { - return constraint; - } - } - return 0; -} - - -void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl) -{ - - CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl; - std::vector::iterator i = - std::find(m_controllers.begin(), m_controllers.end(), ctrl); - if ((i == m_controllers.end())) - { - addCcdPhysicsController(ctrl1); - } - - requestCollisionCallback(ctrl); - //printf("addSensor\n"); -} - -void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl) -{ - std::vector::iterator i = - std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl); - if (!(i == m_triggerControllers.end())) - { - std::swap(*i, m_triggerControllers.back()); - m_triggerControllers.pop_back(); - } -} - - -void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl) -{ - removeCollisionCallback(ctrl); - //printf("removeSensor\n"); -} -void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user) -{ - /* printf("addTouchCallback\n(response class = %i)\n",response_class); - - //map PHY_ convention into SM_ convention - switch (response_class) - { - case PHY_FH_RESPONSE: - printf("PHY_FH_RESPONSE\n"); - break; - case PHY_SENSOR_RESPONSE: - printf("PHY_SENSOR_RESPONSE\n"); - break; - case PHY_CAMERA_RESPONSE: - printf("PHY_CAMERA_RESPONSE\n"); - break; - case PHY_OBJECT_RESPONSE: - printf("PHY_OBJECT_RESPONSE\n"); - break; - case PHY_STATIC_RESPONSE: - printf("PHY_STATIC_RESPONSE\n"); - break; - default: - assert(0); - return; - } - */ - - m_triggerCallbacks[response_class] = callback; - m_triggerCallbacksUserPtrs[response_class] = user; - -} -void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl) -{ - CcdPhysicsController* ccdCtrl = static_cast(ctrl); - - //printf("requestCollisionCallback\n"); - m_triggerControllers.push_back(ccdCtrl); -} - - -void CcdPhysicsEnvironment::CallbackTriggers() -{ - if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))) - { - //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback - int numManifolds = m_collisionWorld->getDispatcher()->getNumManifolds(); - for (int i=0;igetDispatcher()->getManifoldByIndexInternal(i); - int numContacts = manifold->getNumContacts(); - if (numContacts) - { - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)) - { - for (int j=0;jgetContactPoint(j); - if (m_debugDrawer) - m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); - } - } - btRigidBody* obj0 = static_cast(manifold->getBody0()); - btRigidBody* obj1 = static_cast(manifold->getBody1()); - - //m_internalOwner is set in 'addPhysicsController' - CcdPhysicsController* ctrl0 = static_cast(obj0->m_internalOwner); - CcdPhysicsController* ctrl1 = static_cast(obj1->m_internalOwner); - - std::vector::iterator i = - std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0); - if (i == m_triggerControllers.end()) - { - i = std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl1); - } - - if (!(i == m_triggerControllers.end())) - { - m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE], - ctrl0,ctrl1,0); - } - } - } - - - - } - - -} - - - - - - -#ifdef NEW_BULLET_VEHICLE_SUPPORT - -//complex constraint for vehicles -PHY_IVehicle* CcdPhysicsEnvironment::getVehicleConstraint(int constraintId) -{ - int i; - - int numVehicles = m_wrapperVehicles.size(); - for (i=0;iGetVehicle()->getUserConstraintId() == constraintId) - return wrapperVehicle; - } - - return 0; -} - -#endif //NEW_BULLET_VEHICLE_SUPPORT - - -int currentController = 0; -int numController = 0; - - -void CcdPhysicsEnvironment::UpdateAabbs(float timeStep) -{ - std::vector::iterator i; - btBroadphaseInterface* scene = getBroadphase(); - - numController = m_controllers.size(); - currentController = 0; - - // - // update aabbs, only for moving objects (!) - // - for (i=m_controllers.begin(); - !(i==m_controllers.end()); i++) - { - currentController++; - CcdPhysicsController* ctrl = (*i); - btRigidBody* body = ctrl->getRigidBody(); - - - btPoint3 minAabb,maxAabb; - btCollisionShape* shapeinterface = ctrl->getCollisionShape(); - - - - shapeinterface->calculateTemporalAabb(body->getCenterOfMassTransform(), - body->getLinearVelocity(), - //body->getAngularVelocity(), - btVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(), - timeStep,minAabb,maxAabb); - - - btVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold); - minAabb -= manifoldExtraExtents; - maxAabb += manifoldExtraExtents; - - btBroadphaseProxy* bp = body->m_broadphaseHandle; - if (bp) - { - - btVector3 color (1,1,0); - - if (m_debugDrawer) - { - //draw aabb - switch (body->GetActivationState()) - { - case ISLAND_SLEEPING: - { - color.setValue(1,1,1); - break; - } - case WANTS_DEACTIVATION: - { - color.setValue(0,0,1); - break; - } - case ACTIVE_TAG: - { - break; - } - case DISABLE_DEACTIVATION: - { - color.setValue(1,0,1); - }; - - }; - - if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb) - { - DrawAabb(m_debugDrawer,minAabb,maxAabb,color); - } - } - - - if ( (maxAabb-minAabb).length2() < 1e12f) - { - scene->setAabb(bp,minAabb,maxAabb); - } else - { - //something went wrong, investigate - //removeCcdPhysicsController(ctrl); - body->SetActivationState(DISABLE_SIMULATION); - - static bool reportMe = true; - if (reportMe) - { - reportMe = false; - printf("Overflow in AABB, object removed from simulation \n"); - printf("If you can reproduce this, please email bugs@continuousphysics.com\n"); - printf("Please include above information, your Platform, version of OS.\n"); - printf("Thanks.\n"); - } - - } - - } - } -} - -PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position) -{ - - btCcdConstructionInfo cinfo; - cinfo.m_collisionShape = new btSphereShape(radius); - cinfo.m_MotionState = 0; - cinfo.m_physicsEnv = this; - cinfo.m_collisionFlags |= btCollisionObject::noContactResponse; - DefaultMotionState* motionState = new DefaultMotionState(); - cinfo.m_MotionState = motionState; - motionState->m_worldTransform.setIdentity(); - motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2])); - - CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo); - - - return sphereController; -} - - -PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight) -{ - btCcdConstructionInfo cinfo; - cinfo.m_collisionShape = new btConeShape(coneradius,coneheight); - cinfo.m_MotionState = 0; - cinfo.m_physicsEnv = this; - DefaultMotionState* motionState = new DefaultMotionState(); - cinfo.m_MotionState = motionState; - motionState->m_worldTransform.setIdentity(); -// motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2])); - - CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo); - - - return sphereController; -} - -float CcdPhysicsEnvironment::getAppliedImpulse(int constraintid) -{ - std::vector::iterator i; - - for (i=m_constraints.begin(); - !(i==m_constraints.end()); i++) - { - btTypedConstraint* constraint = (*i); - if (constraint->getUserConstraintId() == constraintid) - { - return constraint->getAppliedImpulse(); - } - } - return 0.f; -} diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h deleted file mode 100644 index be7f71e7b..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h +++ /dev/null @@ -1,267 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef CCDPHYSICSENVIRONMENT -#define CCDPHYSICSENVIRONMENT - -#include "PHY_IPhysicsEnvironment.h" -#include -class CcdPhysicsController; -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" - - - - -class btTypedConstraint; -class btSimulationIslandManager; -class btCollisionDispatcher; -class btDispatcher; -//#include "btBroadphaseInterface.h" - -//switch on/off new vehicle support -#define NEW_BULLET_VEHICLE_SUPPORT 1 - -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -class WrapperVehicle; -class btPersistentManifold; -class btBroadphaseInterface; -class btOverlappingPairCache; -class btIDebugDraw; -class PHY_IVehicle; - -/// CcdPhysicsEnvironment is an experimental mainloop for physics simulation using optional continuous collision detection. -/// Physics Environment takes care of stepping the simulation and is a container for physics entities. -/// It stores rigidbodies,constraints, materials etc. -/// A derived class may be able to 'construct' entities by loading and/or converting -class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment -{ - btVector3 m_gravity; - - - -protected: - btIDebugDraw* m_debugDrawer; - //solver iterations - int m_numIterations; - - //timestep subdivisions - int m_numTimeSubSteps; - - - int m_ccdMode; - int m_solverType; - int m_profileTimings; - bool m_enableSatCollisionDetection; - - btContactSolverInfo m_solverInfo; - - btSimulationIslandManager* m_islandManager; - - public: - CcdPhysicsEnvironment(btDispatcher* dispatcher=0, btOverlappingPairCache* pairCache=0); - - virtual ~CcdPhysicsEnvironment(); - - ///////////////////////////////////// - //PHY_IPhysicsEnvironment interface - ///////////////////////////////////// - - /// Perform an integration step of duration 'timeStep'. - - virtual void setDebugDrawer(btIDebugDraw* debugDrawer) - { - m_debugDrawer = debugDrawer; - } - - virtual void setNumIterations(int numIter); - virtual void setNumTimeSubSteps(int numTimeSubSteps) - { - m_numTimeSubSteps = numTimeSubSteps; - } - virtual void setDeactivationTime(float dTime); - virtual void setDeactivationLinearTreshold(float linTresh) ; - virtual void setDeactivationAngularTreshold(float angTresh) ; - virtual void setContactBreakingTreshold(float contactBreakingTreshold) ; - virtual void setCcdMode(int ccdMode); - virtual void setSolverType(int solverType); - virtual void setSolverSorConstant(float sor); - virtual void setSolverTau(float tau); - virtual void setSolverDamping(float damping); - virtual void setLinearAirDamping(float damping); - virtual void setUseEpa(bool epa) ; - - virtual void beginFrame(); - virtual void endFrame() {}; - /// Perform an integration step of duration 'timeStep'. - virtual bool proceedDeltaTime(double curTime,float timeStep); - virtual bool proceedDeltaTimeOneStep(float timeStep); - - virtual void setFixedTimeStep(bool useFixedTimeStep,float fixedTimeStep){}; - //returns 0.f if no fixed timestep is used - - virtual float getFixedTimeStep(){ return 0.f;}; - - virtual void setDebugMode(int debugMode); - - virtual void setGravity(float x,float y,float z); - - virtual int createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type, - float pivotX,float pivotY,float pivotZ, - float axisX,float axisY,float axisZ); - - - //Following the COLLADA physics specification for constraints - virtual int createUniversalD6Constraint( - class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther, - btTransform& localAttachmentFrameRef, - btTransform& localAttachmentOther, - const btVector3& linearMinLimits, - const btVector3& linearMaxLimits, - const btVector3& angularMinLimits, - const btVector3& angularMaxLimits - ); - - - virtual void removeConstraint(int constraintid); - - virtual float getAppliedImpulse(int constraintid); - - - virtual void CallbackTriggers(); - - -#ifdef NEW_BULLET_VEHICLE_SUPPORT - //complex constraint for vehicles - virtual PHY_IVehicle* getVehicleConstraint(int constraintId); -#else - virtual class PHY_IVehicle* getVehicleConstraint(int constraintId) - { - return 0; - } -#endif //NEW_BULLET_VEHICLE_SUPPORT - - btTypedConstraint* getConstraintById(int constraintId); - - virtual PHY_IPhysicsController* rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, - float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ); - - - //Methods for gamelogic collision/physics callbacks - virtual void addSensor(PHY_IPhysicsController* ctrl); - virtual void removeSensor(PHY_IPhysicsController* ctrl); - virtual void addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user); - virtual void requestCollisionCallback(PHY_IPhysicsController* ctrl); - virtual void removeCollisionCallback(PHY_IPhysicsController* ctrl); - - virtual PHY_IPhysicsController* CreateSphereController(float radius,const PHY__Vector3& position); - virtual PHY_IPhysicsController* CreateConeController(float coneradius,float coneheight); - - - virtual int getNumContactPoints(); - - virtual void getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ); - - ////////////////////// - //CcdPhysicsEnvironment interface - //////////////////////// - - void addCcdPhysicsController(CcdPhysicsController* ctrl); - - void removeCcdPhysicsController(CcdPhysicsController* ctrl); - - btBroadphaseInterface* getBroadphase(); - - - - - - bool IsSatCollisionDetectionEnabled() const - { - return m_enableSatCollisionDetection; - } - - void EnableSatCollisionDetection(bool enableSat) - { - m_enableSatCollisionDetection = enableSat; - } - - void UpdateAabbs(float timeStep); - - int GetNumControllers(); - - CcdPhysicsController* GetPhysicsController( int index); - - - - const btPersistentManifold* getManifold(int index) const; - - std::vector m_constraints; - - void SyncMotionStates(float timeStep); - - - class btCollisionWorld* getCollisionWorld() - { - return m_collisionWorld; - } - - const class btCollisionWorld* getCollisionWorld() const - { - return m_collisionWorld; - } - - btSimulationIslandManager* getSimulationIslandManager() - { - return m_islandManager; - } - - const btSimulationIslandManager* getSimulationIslandManager() const - { - return m_islandManager; - } - - class btConstraintSolver* GetConstraintSolver() - { - return m_solver; - } - - protected: - - - - - std::vector m_controllers; - - std::vector m_triggerControllers; - - PHY_ResponseCallback m_triggerCallbacks[PHY_NUM_RESPONSE]; - void* m_triggerCallbacksUserPtrs[PHY_NUM_RESPONSE]; - - std::vector m_wrapperVehicles; - - class btCollisionWorld* m_collisionWorld; - - class btConstraintSolver* m_solver; - - bool m_scalingPropagated; - - - -}; - -#endif //CCDPHYSICSENVIRONMENT - diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysics_vc7.vcproj b/Extras/PhysicsInterface/CcdPhysics/CcdPhysics_vc7.vcproj deleted file mode 100644 index ad85f1cd5..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysics_vc7.vcproj +++ /dev/null @@ -1,131 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysics_vc8.vcproj b/Extras/PhysicsInterface/CcdPhysics/CcdPhysics_vc8.vcproj deleted file mode 100644 index 8f85c50cf..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysics_vc8.vcproj +++ /dev/null @@ -1,191 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Extras/PhysicsInterface/CcdPhysics/Jamfile b/Extras/PhysicsInterface/CcdPhysics/Jamfile deleted file mode 100644 index 87497faf8..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/Jamfile +++ /dev/null @@ -1,10 +0,0 @@ -SubDir TOP Extras PhysicsInterface CcdPhysics ; - -IncludeDir Extras/PhysicsInterface/Common ; - -Library bulletccdphysics : [ Wildcard *.h *.cpp ] ; -CFlags bulletccdphysics : [ FIncludes $(TOP)/Extras/PhysicsInterface/Common ] ; -LibDepends bulletccdphysics : bulletcollision bulletphysicsinterfacecommon bulletdynamics ; - - -InstallHeader [ Wildcard *.h ] : CcdPhysics ; diff --git a/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp b/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp deleted file mode 100644 index b6231fae0..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp +++ /dev/null @@ -1,350 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "ParallelIslandDispatcher.h" - - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" - -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include - -static int gNumManifold2 = 0; - - - - - -ParallelIslandDispatcher::ParallelIslandDispatcher (): - m_useIslands(true), - m_defaultManifoldResult(0,0,0), - m_count(0) -{ - int i; - - for (i=0;iclearManifold(); -} - - -void ParallelIslandDispatcher::releaseManifold(btPersistentManifold* manifold) -{ - - gNumManifold2--; - - //printf("releaseManifold: gNumManifold2 %d\n",gNumManifold2); - - clearManifold(manifold); - - std::vector::iterator i = - std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold); - if (!(i == m_manifoldsPtr.end())) - { - std::swap(*i, m_manifoldsPtr.back()); - m_manifoldsPtr.pop_back(); - delete manifold; - - } - - -} - - -// -// todo: this is random access, it can be walked 'cache friendly'! -// -void ParallelIslandDispatcher::buildAndProcessIslands(btCollisionObjectArray& collisionObjects, IslandCallback* callback) -{ - int numBodies = collisionObjects.size(); - - for (int islandId=0;islandId islandmanifold; - - //int numSleeping = 0; - - bool allSleeping = true; - - int i; - for (i=0;im_islandTag1 == islandId) - { - if (colObj0->GetActivationState()== ACTIVE_TAG) - { - allSleeping = false; - } - if (colObj0->GetActivationState()== DISABLE_DEACTIVATION) - { - allSleeping = false; - } - } - } - - - for (i=0;igetManifoldByIndexInternal(i); - - //filtering for response - - btCollisionObject* colObj0 = static_cast(manifold->getBody0()); - btCollisionObject* colObj1 = static_cast(manifold->getBody1()); - { - if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) || - ((colObj1) && (colObj1)->m_islandTag1 == (islandId))) - { - - if (needsResponse(*colObj0,*colObj1)) - islandmanifold.push_back(manifold); - } - } - } - if (allSleeping) - { - int i; - for (i=0;im_islandTag1 == islandId) - { - colObj0->SetActivationState( ISLAND_SLEEPING ); - } - } - - - } else - { - - int i; - for (i=0;im_islandTag1 == islandId) - { - if ( colObj0->GetActivationState() == ISLAND_SLEEPING) - { - colObj0->SetActivationState( WANTS_DEACTIVATION); - } - } - } - - /// Process the actual simulation, only if not sleeping/deactivated - if (islandmanifold.size()) - { - callback->ProcessIsland(&islandmanifold[0],islandmanifold.size()); - } - - } - } -} - - - -btCollisionAlgorithm* ParallelIslandDispatcher::internalFindAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1) -{ - m_count++; - btCollisionObject* body0 = (btCollisionObject*)proxy0.m_clientObject; - btCollisionObject* body1 = (btCollisionObject*)proxy1.m_clientObject; - - btCollisionAlgorithmConstructionInfo ci; - ci.m_dispatcher = this; - - if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConvex() ) - { - return new btConvexConvexAlgorithm(0,ci,&proxy0,&proxy1); - } - - if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConcave()) - { - return new btConvexConcaveCollisionAlgorithm(ci,&proxy0,&proxy1); - } - - if (body1->m_collisionShape->isConvex() && body0->m_collisionShape->isConcave()) - { - return new btConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0); - } - - if (body0->m_collisionShape->isCompound()) - { - return new btCompoundCollisionAlgorithm(ci,&proxy0,&proxy1); - } else - { - if (body1->m_collisionShape->isCompound()) - { - return new btCompoundCollisionAlgorithm(ci,&proxy1,&proxy0); - } - } - - - //failed to find an algorithm - return new btEmptyAlgorithm(ci); - -} - -bool ParallelIslandDispatcher::needsResponse(const btCollisionObject& colObj0,const btCollisionObject& colObj1) -{ - - - //here you can do filtering - bool hasResponse = - (!(colObj0.m_collisionFlags & btCollisionObject::noContactResponse)) && - (!(colObj1.m_collisionFlags & btCollisionObject::noContactResponse)); - hasResponse = hasResponse && - (colObj0.IsActive() || colObj1.IsActive()); - return hasResponse; -} - -bool ParallelIslandDispatcher::needsCollision(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1) -{ - - btCollisionObject* body0 = (btCollisionObject*)proxy0.m_clientObject; - btCollisionObject* body1 = (btCollisionObject*)proxy1.m_clientObject; - - assert(body0); - assert(body1); - - bool needsCollision = true; - - if ((body0->m_collisionFlags & btCollisionObject::isStatic) && - (body1->m_collisionFlags & btCollisionObject::isStatic)) - needsCollision = false; - - if ((!body0->IsActive()) && (!body1->IsActive())) - needsCollision = false; - - return needsCollision ; - -} - -///allows the user to get contact point callbacks -btManifoldResult* ParallelIslandDispatcher::getNewManifoldResult(btCollisionObject* obj0,btCollisionObject* obj1,btPersistentManifold* manifold) -{ - - - //in-place, this prevents parallel dispatching, but just adding a list would fix that. - btManifoldResult* manifoldResult = new (&m_defaultManifoldResult) btManifoldResult(obj0,obj1,manifold); - return manifoldResult; -} - -///allows the user to get contact point callbacks -void ParallelIslandDispatcher::releaseManifoldResult(btManifoldResult*) -{ - -} - - -void ParallelIslandDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo) -{ - //m_blockedForChanges = true; - - - - assert(0); - -/* - int dispatcherId = getUniqueId(); - int i; - for (i=0;i= 0) - { - //dispatcher will keep algorithms persistent in the collision pair - if (!pair.m_algorithms[dispatcherId]) - { - pair.m_algorithms[dispatcherId] = findAlgorithm( - *pair.m_pProxy0, - *pair.m_pProxy1); - } - - if (pair.m_algorithms[dispatcherId]) - { - if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) - { - pair.m_algorithms[dispatcherId]->processCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); - } else - { - float toi = pair.m_algorithms[dispatcherId]->calculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); - if (dispatchInfo.m_timeOfImpact > toi) - dispatchInfo.m_timeOfImpact = toi; - - } - } - } else - { - //non-persistent algorithm dispatcher - btCollisionAlgorithm* algo = findAlgorithm( - *pair.m_pProxy0, - *pair.m_pProxy1); - - if (algo) - { - if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) - { - algo->processCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); - } else - { - float toi = algo->calculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); - if (dispatchInfo.m_timeOfImpact > toi) - dispatchInfo.m_timeOfImpact = toi; - } - } - } - - } -*/ - //m_blockedForChanges = false; - -} - diff --git a/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h b/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h deleted file mode 100644 index f6dd79668..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h +++ /dev/null @@ -1,128 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef PARALLEL_ISLAND_DISPATCHER_H -#define PARALLEL_ISLAND_DISPATCHER_H - -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletCollision/CollisionDispatch/btUnionFind.h" -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" - -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include - -class btIDebugDraw; - - -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" - - - - -///ParallelIslandDispatcher dispatches entire simulation islands in parallel. -///For both collision detection and constraint solving. -///This development heads toward multi-core, CELL SPU and GPU approach -class ParallelIslandDispatcher : public btDispatcher -{ - - std::vector m_manifoldsPtr; - - btUnionFind m_unionFind; - - bool m_useIslands; - - btManifoldResult m_defaultManifoldResult; - - btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; - -public: - - btUnionFind& getUnionFind() { return m_unionFind;} - - struct IslandCallback - { - virtual ~IslandCallback() {}; - - virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds) = 0; - }; - - - int getNumManifolds() const - { - return m_manifoldsPtr.size(); - } - - btPersistentManifold* getManifoldByIndexInternal(int index) - { - return m_manifoldsPtr[index]; - } - - const btPersistentManifold* getManifoldByIndexInternal(int index) const - { - return m_manifoldsPtr[index]; - } - - void initUnionFind(int n) - { - if (m_useIslands) - m_unionFind.reset(n); - } - - void findUnions(); - - int m_count; - - ParallelIslandDispatcher (); - virtual ~ParallelIslandDispatcher() {}; - - virtual btPersistentManifold* getNewManifold(void* b0,void* b1); - - virtual void releaseManifold(btPersistentManifold* manifold); - - - virtual void buildAndProcessIslands(btCollisionObjectArray& collisionObjects, IslandCallback* callback); - - ///allows the user to get contact point callbacks - virtual btManifoldResult* getNewManifoldResult(btCollisionObject* obj0,btCollisionObject* obj1,btPersistentManifold* manifold); - - ///allows the user to get contact point callbacks - virtual void releaseManifoldResult(btManifoldResult*); - - virtual void clearManifold(btPersistentManifold* manifold); - - - btCollisionAlgorithm* findAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1) - { - btCollisionAlgorithm* algo = internalFindAlgorithm(proxy0,proxy1); - return algo; - } - - btCollisionAlgorithm* internalFindAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1); - - virtual bool needsCollision(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1); - - virtual bool needsResponse(const btCollisionObject& colObj0,const btCollisionObject& colObj1); - - virtual int getUniqueId() { return RIGIDBODY_DISPATCHER;} - - virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo); - - - -}; - -#endif //PARALLEL_ISLAND_DISPATCHER_H - diff --git a/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp b/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp deleted file mode 100644 index 493eec20f..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp +++ /dev/null @@ -1,206 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "ParallelPhysicsEnvironment.h" -#include "CcdPhysicsController.h" -#include "ParallelIslandDispatcher.h" -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include "SimulationIsland.h" - - -ParallelPhysicsEnvironment::ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher, btOverlappingPairCache* pairCache): -CcdPhysicsEnvironment(dispatcher,pairCache) -{ - -} - -ParallelPhysicsEnvironment::~ParallelPhysicsEnvironment() -{ - -} - - - -/// Perform an integration step of duration 'timeStep'. -bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) -{ - // Make sure the broadphase / overlapping AABB paircache is up-to-date - btOverlappingPairCache* scene = m_collisionWorld->getPairCache(); - scene->refreshOverlappingPairs(); - - // Find the connected sets that can be simulated in parallel - // Using union find - -#ifdef USE_QUICKPROF - btProfiler::beginBlock("IslandUnionFind"); -#endif //USE_QUICKPROF - - getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); - - { - int i; - int numConstraints = m_constraints.size(); - for (i=0;i< numConstraints ; i++ ) - { - btTypedConstraint* constraint = m_constraints[i]; - - const btRigidBody* colObj0 = &constraint->getRigidBodyA(); - const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - - if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && - ((colObj1) && ((colObj1)->mergesSimulationIslands()))) - { - if (colObj0->IsActive() || colObj1->IsActive()) - { - - getSimulationIslandManager()->getUnionFind().unite((colObj0)->m_islandTag1, - (colObj1)->m_islandTag1); - } - } - } - } - - //Store the island id in each body - getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - -#ifdef USE_QUICKPROF - btProfiler::endBlock("IslandUnionFind"); -#endif //USE_QUICKPROF - - - - ///build simulation islands - -#ifdef USE_QUICKPROF - btProfiler::beginBlock("BuildIslands"); -#endif //USE_QUICKPROF - - std::vector simulationIslands; - simulationIslands.resize(GetNumControllers()); - - int k; - for (k=0;kgetRigidBody()->m_islandTag1; - if (tag>=0) - { - simulationIslands[tag].m_controllers.push_back(ctrl); - } - } - - btDispatcher* dispatcher = getCollisionWorld()->getDispatcher(); - - - //this is a brute force approach, will rethink later about more subtle ways - int i; - - assert(0); -/* - for (i=0;i< scene->GetNumOverlappingPairs();i++) - { - - - btBroadphasePair* pair = &scene->GetOverlappingPair(i); - - btCollisionObject* col0 = static_cast(pair->m_pProxy0->m_clientObject); - btCollisionObject* col1 = static_cast(pair->m_pProxy1->m_clientObject); - - if (col0->m_islandTag1 > col1->m_islandTag1) - { - simulationIslands[col0->m_islandTag1].m_overlappingPairIndices.push_back(i); - } else - { - simulationIslands[col1->m_islandTag1].m_overlappingPairIndices.push_back(i); - } - - } - */ - - //store constraint indices for each island - for (unsigned int ui=0;ui constraint.getRigidBodyB().m_islandTag1) - { - simulationIslands[constraint.getRigidBodyA().m_islandTag1].m_constraintIndices.push_back(ui); - } else - { - simulationIslands[constraint.getRigidBodyB().m_islandTag1].m_constraintIndices.push_back(ui); - } - - } - - //add all overlapping pairs for each island - - for (i=0;igetNumManifolds();i++) - { - btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); - - //filtering for response - - btCollisionObject* colObj0 = static_cast(manifold->getBody0()); - btCollisionObject* colObj1 = static_cast(manifold->getBody1()); - { - int islandTag = colObj0->m_islandTag1; - if (colObj1->m_islandTag1 > islandTag) - islandTag = colObj1->m_islandTag1; - - if (dispatcher->needsResponse(*colObj0,*colObj1)) - simulationIslands[islandTag].m_manifolds.push_back(manifold); - - } - } - - #ifdef USE_QUICKPROF - btProfiler::endBlock("BuildIslands"); - #endif //USE_QUICKPROF - - -#ifdef USE_QUICKPROF - btProfiler::beginBlock("SimulateIsland"); -#endif //USE_QUICKPROF - - btTypedConstraint** constraintBase = 0; - if (m_constraints.size()) - constraintBase = &m_constraints[0]; - - - - assert(0); - /* - //Each simulation island can be processed in parallel (will be put on a job queue) - for (k=0;kGetOverlappingPair(0),dispatcher,getBroadphase(),m_solver,timeStep); - } - } - */ - - -#ifdef USE_QUICKPROF - btProfiler::endBlock("SimulateIsland"); -#endif //USE_QUICKPROF - - return true; - -} diff --git a/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.h b/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.h deleted file mode 100644 index 666e5ee7d..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.h +++ /dev/null @@ -1,44 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef PARALLELPHYSICSENVIRONMENT -#define PARALLELPHYSICSENVIRONMENT - -#include "CcdPhysicsEnvironment.h" -class ParallelIslandDispatcher; - - -/// ParallelPhysicsEnvironment is experimental parallel mainloop for physics simulation -/// Physics Environment takes care of stepping the simulation and is a container for physics entities. -/// It stores rigidbodies,constraints, materials etc. -/// A derived class may be able to 'construct' entities by loading and/or converting -class ParallelPhysicsEnvironment : public CcdPhysicsEnvironment -{ - - - public: - ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher=0, btOverlappingPairCache* pairCache=0); - - virtual ~ParallelPhysicsEnvironment(); - - - /// Perform an integration step of duration 'timeStep'. - virtual bool proceedDeltaTimeOneStep(float timeStep); - - //void BuildSimulationIslands(); - -}; - -#endif //PARALLELPHYSICSENVIRONMENT diff --git a/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp b/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp deleted file mode 100644 index ec2eda257..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp +++ /dev/null @@ -1,465 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SimulationIsland.h" -#include "LinearMath/btTransform.h" -#include "CcdPhysicsController.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "LinearMath/btIDebugDraw.h" - -extern float gContactBreakingTreshold; - -bool SimulationIsland::Simulate(btIDebugDraw* debugDrawer,int numSolverIterations,btTypedConstraint** constraintsBaseAddress,btBroadphasePair* overlappingPairBaseAddress, btDispatcher* dispatcher,btBroadphaseInterface* broadphase,class btConstraintSolver* solver,float timeStep) -{ - - -#ifdef USE_QUICKPROF - - btProfiler::beginBlock("predictIntegratedTransform"); -#endif //USE_QUICKPROF - - { - // std::vector::iterator i; - - - - int k; - for (k=0;kgetRigidBody(); - //todo: only do this when necessary, it's used for contact points - body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); - - if (body->IsActive()) - { - if (!body->IsStatic()) - { - body->applyForces( timeStep); - body->integrateVelocities( timeStep); - body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform); - } - } - - } - } - -#ifdef USE_QUICKPROF - btProfiler::endBlock("predictIntegratedTransform"); -#endif //USE_QUICKPROF - - //BroadphaseInterface* scene = getBroadphase(); - - - // - // collision detection (?) - // - - - #ifdef USE_QUICKPROF - btProfiler::beginBlock("dispatchAllCollisionPairs"); - #endif //USE_QUICKPROF - - -// int numsubstep = m_numIterations; - - - btDispatcherInfo dispatchInfo; - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_enableSatConvex = false;//m_enableSatCollisionDetection; - dispatchInfo.m_debugDraw = debugDrawer; - - std::vector overlappingPairs; - overlappingPairs.resize(this->m_overlappingPairIndices.size()); - - //gather overlapping pair info - unsigned int i; - for (i=0;irefreshOverlappingPairs(); - if (overlappingPairs.size()) - { - assert(0); - //dispatcher->dispatchAllCollisionPairs(&overlappingPairs[0],overlappingPairs.size(),dispatchInfo);///numsubstep,g); - } - - //scatter overlapping pair info, mainly the created algorithms/contact caches - - for (i=0;ibuildJacobian(); - constraint->solveConstraint( timeStep ); - - } - - - } - - #ifdef USE_QUICKPROF - btProfiler::endBlock("solveConstraint"); - #endif //USE_QUICKPROF - - /* - - //solve the vehicles - - #ifdef NEW_BULLET_VEHICLE_SUPPORT - //vehicles - int numVehicles = m_wrapperVehicles.size(); - for (int i=0;iGetVehicle(); - vehicle->updateVehicle( timeStep); - } - #endif //NEW_BULLET_VEHICLE_SUPPORT -*/ - - /* - - btProfiler::beginBlock("CallbackTriggers"); - #endif //USE_QUICKPROF - - CallbackTriggers(); - - #ifdef USE_QUICKPROF - btProfiler::endBlock("CallbackTriggers"); - - } - */ - - //btOverlappingPairCache* scene = getCollisionWorld()->getPairCache(); - - btContactSolverInfo solverInfo; - - solverInfo.m_friction = 0.9f; - solverInfo.m_numIterations = numSolverIterations; - solverInfo.m_timeStep = timeStep; - solverInfo.m_restitution = 0.f;//m_restitution; - - - if (m_manifolds.size()) - { - solver->solveGroup( &m_manifolds[0],m_manifolds.size(),solverInfo,0); - } - - -#ifdef USE_QUICKPROF - btProfiler::beginBlock("proceedToTransform"); -#endif //USE_QUICKPROF - { - - - - { - - - UpdateAabbs(debugDrawer,broadphase,timeStep); - - - float toi = 1.f; - - //experimental continuous collision detection - - /* if (m_ccdMode == 3) - { - btDispatcherInfo dispatchInfo; - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS; - - // getCollisionWorld()->getDispatcher()->dispatchAllCollisionPairs(scene,dispatchInfo); - toi = dispatchInfo.m_timeOfImpact; - - } - */ - - - - // - // integrating solution - // - - { - - std::vector::iterator i; - - for (i=m_controllers.begin(); - !(i==m_controllers.end()); i++) - { - - CcdPhysicsController* ctrl = *i; - - btTransform predictedTrans; - btRigidBody* body = ctrl->getRigidBody(); - - if (body->IsActive()) - { - - if (!body->IsStatic()) - { - body->predictIntegratedTransform(timeStep* toi, predictedTrans); - body->proceedToTransform( predictedTrans); - } - - } - } - - } - - - - - - // - // disable sleeping physics objects - // - - std::vector m_sleepingControllers; - - std::vector::iterator i; - - for (i=m_controllers.begin(); - !(i==m_controllers.end()); i++) - { - CcdPhysicsController* ctrl = (*i); - btRigidBody* body = ctrl->getRigidBody(); - - ctrl->UpdateDeactivation(timeStep); - - - if (ctrl->wantsSleeping()) - { - if (body->GetActivationState() == ACTIVE_TAG) - body->SetActivationState( WANTS_DEACTIVATION ); - } else - { - if (body->GetActivationState() != DISABLE_DEACTIVATION) - body->SetActivationState( ACTIVE_TAG ); - } - - if (true) - { - if (body->GetActivationState() == ISLAND_SLEEPING) - { - m_sleepingControllers.push_back(ctrl); - } - } else - { - if (ctrl->wantsSleeping()) - { - m_sleepingControllers.push_back(ctrl); - } - } - } - - - - - } - - -#ifdef USE_QUICKPROF - btProfiler::endBlock("proceedToTransform"); - - btProfiler::beginBlock("SyncMotionStates"); -#endif //USE_QUICKPROF - - SyncMotionStates(timeStep); - -#ifdef USE_QUICKPROF - btProfiler::endBlock("SyncMotionStates"); - -#endif //USE_QUICKPROF - - -#ifdef NEW_BULLET_VEHICLE_SUPPORT - //sync wheels for vehicles - int numVehicles = m_wrapperVehicles.size(); - for (int i=0;iSyncWheels(); - } -#endif //NEW_BULLET_VEHICLE_SUPPORT - - return true; - } -} - - - -void SimulationIsland::SyncMotionStates(float timeStep) -{ - std::vector::iterator i; - - // - // synchronize the physics and graphics transformations - // - - for (i=m_controllers.begin(); - !(i==m_controllers.end()); i++) - { - CcdPhysicsController* ctrl = (*i); - ctrl->SynchronizeMotionStates(timeStep); - - } - -} - - - -void SimulationIsland::UpdateAabbs(btIDebugDraw* debugDrawer,btBroadphaseInterface* scene,float timeStep) -{ - std::vector::iterator i; - - - // - // update aabbs, only for moving objects (!) - // - for (i=m_controllers.begin(); - !(i==m_controllers.end()); i++) - { - CcdPhysicsController* ctrl = (*i); - btRigidBody* body = ctrl->getRigidBody(); - - - btPoint3 minAabb,maxAabb; - btCollisionShape* shapeinterface = ctrl->getCollisionShape(); - - - - shapeinterface->calculateTemporalAabb(body->getCenterOfMassTransform(), - body->getLinearVelocity(), - //body->getAngularVelocity(), - btVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(), - timeStep,minAabb,maxAabb); - - - btVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold); - minAabb -= manifoldExtraExtents; - maxAabb += manifoldExtraExtents; - - btBroadphaseProxy* bp = body->m_broadphaseHandle; - if (bp) - { - - btVector3 color (1,1,0); - -/* - class btIDebugDraw* m_debugDrawer = 0; - if (m_debugDrawer) - { - //draw aabb - switch (body->GetActivationState()) - { - case ISLAND_SLEEPING: - { - color.setValue(1,1,1); - break; - } - case WANTS_DEACTIVATION: - { - color.setValue(0,0,1); - break; - } - case ACTIVE_TAG: - { - break; - } - case DISABLE_DEACTIVATION: - { - color.setValue(1,0,1); - }; - - }; - - if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb) - { - DrawAabb(m_debugDrawer,minAabb,maxAabb,color); - } - } -*/ - - - if ( (maxAabb-minAabb).length2() < 1e12f) - { - scene->setAabb(bp,minAabb,maxAabb); - } else - { - //something went wrong, investigate - //removeCcdPhysicsController(ctrl); - body->SetActivationState(DISABLE_SIMULATION); - - static bool reportMe = true; - if (reportMe) - { - reportMe = false; - assert(0); - /* - printf("Overflow in AABB, object removed from simulation \n"); - printf("If you can reproduce this, please email bugs@continuousphysics.com\n"); - printf("Please include above information, your Platform, version of OS.\n"); - printf("Thanks.\n"); - */ - } - - } - - } - } -} diff --git a/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h b/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h deleted file mode 100644 index e89e73da2..000000000 --- a/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h +++ /dev/null @@ -1,55 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SIMULATION_ISLAND_H -#define SIMULATION_ISLAND_H - -#include -class btBroadphaseInterface; -class btDispatcher; -class btIDebugDraw; - -///SimulationIsland groups all computations and data (for collision detection and dynamics) that can execute in parallel with other SimulationIsland's -///The ParallelPhysicsEnvironment and ParallelIslandDispatcher will dispatch SimulationIsland's -///At the start of the simulation timestep the simulation islands are re-calculated -///During one timestep there is no merging or splitting of Simulation Islands -class SimulationIsland -{ - - public: - std::vector m_controllers; - std::vector m_manifolds; - - std::vector m_overlappingPairIndices; - std::vector m_constraintIndices; - - bool Simulate(btIDebugDraw* debugDrawer,int numSolverIterations,class btTypedConstraint** constraintsBaseAddress,struct btBroadphasePair* overlappingPairBaseAddress, btDispatcher* dispatcher,btBroadphaseInterface* broadphase, class btConstraintSolver* solver, float timeStep); - - - int GetNumControllers() - { - return m_controllers.size(); - } - - - - - void SyncMotionStates(float timeStep); - void UpdateAabbs(btIDebugDraw* debugDrawer,btBroadphaseInterface* broadphase,float timeStep); -}; - -#endif //SIMULATION_ISLAND_H - - diff --git a/Extras/PhysicsInterface/Common/CMakeLists.txt b/Extras/PhysicsInterface/Common/CMakeLists.txt deleted file mode 100644 index 7aaecb726..000000000 --- a/Extras/PhysicsInterface/Common/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -INCLUDE_DIRECTORIES( -${BULLET_PHYSICS_SOURCE_DIR}src ${BULLET_PHYSICS_SOURCE_DIR}/Extras/PhysicsInterface/Common } -) - -ADD_LIBRARY(LibPhysicsCommonInterface - PHY_IMotionState.cpp - PHY_IPhysicsController.cpp - PHY_IPhysicsEnvironment.cpp - PHY_IVehicle.cpp -) diff --git a/Extras/PhysicsInterface/Common/Jamfile b/Extras/PhysicsInterface/Common/Jamfile deleted file mode 100644 index b6fce0394..000000000 --- a/Extras/PhysicsInterface/Common/Jamfile +++ /dev/null @@ -1,5 +0,0 @@ -SubDir TOP Extras PhysicsInterface Common ; - -Library bulletphysicsinterfacecommon : [ Wildcard *.h *.cpp ] ; - -InstallHeader [ Wildcard *.h ] ; diff --git a/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h b/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h deleted file mode 100644 index 395237718..000000000 --- a/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h +++ /dev/null @@ -1,92 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef __PHY_DYNAMIC_TYPES -#define __PHY_DYNAMIC_TYPES - - - -class PHY_ResponseTable; - -class PHY_Shape; - -struct PHY__Vector3 -{ - float m_vec[4]; - operator const float* () const - { - return &m_vec[0]; - } - operator float* () - { - return &m_vec[0]; - } -}; -//typedef float PHY__Vector3[4]; - -typedef enum -{ - PHY_FH_RESPONSE, - PHY_SENSOR_RESPONSE, /* Touch Sensors */ - PHY_CAMERA_RESPONSE, /* Visibility Culling */ - PHY_OBJECT_RESPONSE, /* Object Dynamic Geometry Response */ - PHY_STATIC_RESPONSE, /* Static Geometry Response */ - - PHY_NUM_RESPONSE -}; - - typedef struct PHY_CollData { - PHY__Vector3 m_point1; /* Point in object1 in world coordinates */ - PHY__Vector3 m_point2; /* Point in object2 in world coordinates */ - PHY__Vector3 m_normal; /* point2 - point1 */ - } PHY_CollData; - - - typedef bool (*PHY_ResponseCallback)(void *client_data, - void *client_object1, - void *client_object2, - const PHY_CollData *coll_data); - - - -/// PHY_PhysicsType enumerates all possible Physics Entities. -/// It is mainly used to create/add Physics Objects - -typedef enum PHY_PhysicsType { - PHY_CONVEX_RIGIDBODY=16386, - PHY_CONCAVE_RIGIDBODY=16399, - PHY_CONVEX_FIXEDBODY=16388,//'collision object' - PHY_CONCAVE_FIXEDBODY=16401, - PHY_CONVEX_KINEMATICBODY=16387,// - PHY_CONCAVE_KINEMATICBODY=16400, - PHY_CONVEX_PHANTOMBODY=16398, - PHY_CONCAVE_PHANTOMBODY=16402 -} PHY_PhysicsType; - -/// PHY_ConstraintType enumerates all supported Constraint Types -typedef enum PHY_ConstraintType { - PHY_POINT2POINT_CONSTRAINT=1, - PHY_LINEHINGE_CONSTRAINT=2, - PHY_ANGULAR_CONSTRAINT = 3,//hinge without ball socket - PHY_VEHICLE_CONSTRAINT=11,//complex 'constraint' that turns a rigidbody into a vehicle - PHY_GENERIC_6DOF_CONSTRAINT=12,//can leave any of the 6 degree of freedom 'free' or 'locked' - -} PHY_ConstraintType; - -typedef float PHY_Vector3[3]; - -#endif //__PHY_DYNAMIC_TYPES - diff --git a/Extras/PhysicsInterface/Common/PHY_IMotionState.cpp b/Extras/PhysicsInterface/Common/PHY_IMotionState.cpp deleted file mode 100644 index c3fd63530..000000000 --- a/Extras/PhysicsInterface/Common/PHY_IMotionState.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "PHY_IMotionState.h" - -PHY_IMotionState::~PHY_IMotionState() -{ - -} diff --git a/Extras/PhysicsInterface/Common/PHY_IMotionState.h b/Extras/PhysicsInterface/Common/PHY_IMotionState.h deleted file mode 100644 index 23dfdebae..000000000 --- a/Extras/PhysicsInterface/Common/PHY_IMotionState.h +++ /dev/null @@ -1,42 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef PHY__MOTIONSTATE_H -#define PHY__MOTIONSTATE_H - -/** - PHY_IMotionState is the Interface to explicitly synchronize the world transformation. - Default implementations for mayor graphics libraries like OpenGL and DirectX can be provided. -*/ -class PHY_IMotionState - -{ - public: - - virtual ~PHY_IMotionState(); - - virtual void getWorldPosition(float& posX,float& posY,float& posZ)=0; - virtual void getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)=0; - virtual void getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)=0; - - virtual void setWorldPosition(float posX,float posY,float posZ)=0; - virtual void setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)=0; - - virtual void calculateWorldTransformations()=0; -}; - -#endif //PHY__MOTIONSTATE_H - diff --git a/Extras/PhysicsInterface/Common/PHY_IPhysicsController.cpp b/Extras/PhysicsInterface/Common/PHY_IPhysicsController.cpp deleted file mode 100644 index 1ef7118ec..000000000 --- a/Extras/PhysicsInterface/Common/PHY_IPhysicsController.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "PHY_IPhysicsController.h" - -PHY_IPhysicsController::~PHY_IPhysicsController() -{ - -} - diff --git a/Extras/PhysicsInterface/Common/PHY_IPhysicsController.h b/Extras/PhysicsInterface/Common/PHY_IPhysicsController.h deleted file mode 100644 index 73926e226..000000000 --- a/Extras/PhysicsInterface/Common/PHY_IPhysicsController.h +++ /dev/null @@ -1,88 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef PHY_IPHYSICSCONTROLLER_H -#define PHY_IPHYSICSCONTROLLER_H - -#include "PHY_DynamicTypes.h" - -/** - PHY_IPhysicsController is the abstract simplified Interface to a physical object. - It contains the IMotionState and IDeformableMesh Interfaces. -*/ - - -class PHY_IPhysicsController -{ - - public: - - virtual ~PHY_IPhysicsController(); - /** - SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding') - */ - virtual bool SynchronizeMotionStates(float time)=0; - /** - WriteMotionStateToDynamics ynchronizes dynas, kinematic and deformable entities (and do 'late binding') - */ - - virtual void WriteMotionStateToDynamics(bool nondynaonly)=0; - virtual void WriteDynamicsToMotionState()=0; - // controller replication - virtual void PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)=0; - - // kinematic methods - virtual void RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)=0; - virtual void RelativeRotate(const float drot[12],bool local)=0; - virtual void getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)=0; - virtual void setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)=0; - virtual void setPosition(float posX,float posY,float posZ)=0; - virtual void getPosition(PHY__Vector3& pos) const=0; - virtual void setScaling(float scaleX,float scaleY,float scaleZ)=0; - - // physics methods - virtual void ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)=0; - virtual void ApplyForce(float forceX,float forceY,float forceZ,bool local)=0; - virtual void SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)=0; - virtual void SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)=0; - - virtual void applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)=0; - virtual void SetActive(bool active)=0; - - // reading out information from physics - virtual void GetLinearVelocity(float& linvX,float& linvY,float& linvZ)=0; - virtual void GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)=0; - virtual void getReactionForce(float& forceX,float& forceY,float& forceZ)=0; - - // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted - virtual void setRigidBody(bool rigid)=0; - - - // clientinfo for raycasts for example - virtual void* getNewClientInfo()=0; - virtual void setNewClientInfo(void* clientinfo)=0; - virtual PHY_IPhysicsController* GetReplica() {return 0;} - - virtual void calcXform() =0; - virtual void setMargin(float margin) =0; - virtual float getMargin() const=0; - virtual float getRadius() const { return 0.f;} - PHY__Vector3 GetWorldPosition(PHY__Vector3& localpos); - -}; - -#endif //PHY_IPHYSICSCONTROLLER_H - diff --git a/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.cpp b/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.cpp deleted file mode 100644 index f59efad56..000000000 --- a/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.cpp +++ /dev/null @@ -1,26 +0,0 @@ -/* - * Copyright (c) 2001-2005 Erwin Coumans - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies and - * that both that copyright notice and this permission notice appear - * in supporting documentation. Erwin Coumans makes no - * representations about the suitability of this software for any - * purpose. It is provided "as is" without express or implied warranty. - * - */ - -#include "PHY_IPhysicsEnvironment.h" - -/** -* Physics Environment takes care of stepping the simulation and is a container for physics entities (rigidbodies,constraints, materials etc.) -* A derived class may be able to 'construct' entities by loading and/or converting -*/ - - - -PHY_IPhysicsEnvironment::~PHY_IPhysicsEnvironment() -{ - -} diff --git a/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h b/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h deleted file mode 100644 index cbf909403..000000000 --- a/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h +++ /dev/null @@ -1,60 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef _IPHYSICSENVIRONMENT -#define _IPHYSICSENVIRONMENT - -#include -#include "PHY_DynamicTypes.h" - -/** -* Physics Environment takes care of stepping the simulation and is a container for physics entities (rigidbodies,constraints, materials etc.) -* A derived class may be able to 'construct' entities by loading and/or converting -*/ -class PHY_IPhysicsEnvironment -{ - public: - virtual ~PHY_IPhysicsEnvironment(); - /// Perform an integration step of duration 'timeStep'. - virtual bool proceedDeltaTime(double curTime,float timeStep)=0; - - - virtual void setGravity(float x,float y,float z)=0; - - virtual int createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type, - float pivotX,float pivotY,float pivotZ, - float axisX,float axisY,float axisZ)=0; - virtual void removeConstraint(int constraintid)=0; - - virtual float getAppliedImpulse(int constraintid){ return 0.f;} - - virtual PHY_IPhysicsController* rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, - float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)=0; - - - //Methods for gamelogic collision/physics callbacks - //todo: - virtual void addSensor(PHY_IPhysicsController* ctrl)=0; - virtual void removeSensor(PHY_IPhysicsController* ctrl)=0; - virtual void addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)=0; - virtual void requestCollisionCallback(PHY_IPhysicsController* ctrl)=0; - virtual PHY_IPhysicsController* CreateSphereController(float radius,const PHY__Vector3& position) =0; - virtual PHY_IPhysicsController* CreateConeController(float coneradius,float coneheight)=0; - -}; - -#endif //_IPHYSICSENVIRONMENT - diff --git a/Extras/PhysicsInterface/Common/PHY_IVehicle.cpp b/Extras/PhysicsInterface/Common/PHY_IVehicle.cpp deleted file mode 100644 index e22b7fd1a..000000000 --- a/Extras/PhysicsInterface/Common/PHY_IVehicle.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "PHY_IVehicle.h" - -PHY_IVehicle::~PHY_IVehicle() -{ - -} diff --git a/Extras/PhysicsInterface/Common/PHY_IVehicle.h b/Extras/PhysicsInterface/Common/PHY_IVehicle.h deleted file mode 100644 index 954066a1f..000000000 --- a/Extras/PhysicsInterface/Common/PHY_IVehicle.h +++ /dev/null @@ -1,72 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef PHY_IVEHICLE_H -#define PHY_IVEHICLE_H - -//PHY_IVehicle provides a generic interface for (raycast based) vehicles. Mostly targetting 4 wheel cars and 2 wheel motorbikes. - -class PHY_IMotionState; -#include "PHY_DynamicTypes.h" - -class PHY_IVehicle -{ -public: - - virtual ~PHY_IVehicle(); - - virtual void addWheel( - PHY_IMotionState* motionState, - PHY__Vector3 connectionPoint, - PHY__Vector3 downDirection, - PHY__Vector3 axleDirection, - float suspensionRestLength, - float wheelRadius, - bool hasSteering - ) = 0; - - - virtual int getNumWheels() const = 0; - - virtual void GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const = 0; - virtual void GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const = 0; - virtual float GetWheelRotation(int wheelIndex) const = 0; - - virtual int getUserConstraintId() const =0; - virtual int getUserConstraintType() const =0; - - //some basic steering/braking/tuning/balancing (bikes) - - virtual void setSteeringValue(float steering,int wheelIndex) = 0; - - virtual void applyEngineForce(float force,int wheelIndex) = 0; - - virtual void ApplyBraking(float braking,int wheelIndex) = 0; - - virtual void SetWheelFriction(float friction,int wheelIndex) = 0; - - virtual void SetSuspensionStiffness(float suspensionStiffness,int wheelIndex) = 0; - - virtual void SetSuspensionDamping(float suspensionStiffness,int wheelIndex) = 0; - - virtual void SetSuspensionCompression(float suspensionStiffness,int wheelIndex) = 0; - - virtual void SetRollInfluence(float rollInfluence,int wheelIndex) = 0; - - virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex) =0; - -}; - -#endif //PHY_IVEHICLE_H diff --git a/Extras/PhysicsInterface/Common/PHY_Pro.h b/Extras/PhysicsInterface/Common/PHY_Pro.h deleted file mode 100644 index 7ab4129ca..000000000 --- a/Extras/PhysicsInterface/Common/PHY_Pro.h +++ /dev/null @@ -1,47 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef PHY_PROPSH -#define PHY_PROPSH - - -class btCollisionShape; - -// Properties of dynamic objects -struct PHY_ShapeProps { - float m_mass; // Total mass - float m_inertia; // Inertia, should be a tensor some time - float m_lin_drag; // Linear drag (air, water) 0 = concrete, 1 = vacuum - float m_ang_drag; // Angular drag - float m_friction_scaling[3]; // Scaling for anisotropic friction. Component in range [0, 1] - bool m_do_anisotropic; // Should I do anisotropic friction? - bool m_do_fh; // Should the object have a linear Fh spring? - bool m_do_rot_fh; // Should the object have an angular Fh spring? - btCollisionShape* m_shape; -}; - - -// Properties of collidable objects (non-ghost objects) -struct PHY_MaterialProps { - float m_restitution; // restitution of energie after a collision 0 = inelastic, 1 = elastic - float m_friction; // Coulomb friction (= ratio between the normal en maximum friction force) - float m_fh_spring; // Spring constant (both linear and angular) - float m_fh_damping; // Damping factor (linear and angular) in range [0, 1] - float m_fh_distance; // The range above the surface where Fh is active. - bool m_fh_normal; // Should the object slide off slopes? -}; - -#endif //PHY_PROPSH - diff --git a/Extras/PhysicsInterface/Common/PhysicsInterface.dsp b/Extras/PhysicsInterface/Common/PhysicsInterface.dsp deleted file mode 100644 index 595359d11..000000000 --- a/Extras/PhysicsInterface/Common/PhysicsInterface.dsp +++ /dev/null @@ -1,128 +0,0 @@ -# Microsoft Developer Studio Project File - Name="PhysicsInterface" - Package Owner=<4> -# Microsoft Developer Studio Generated Build File, Format Version 6.00 -# ** DO NOT EDIT ** - -# TARGTYPE "Win32 (x86) Static Library" 0x0104 - -CFG=PhysicsInterface - Win32 Debug -!MESSAGE This is not a valid makefile. To build this project using NMAKE, -!MESSAGE use the Export Makefile command and run -!MESSAGE -!MESSAGE NMAKE /f "PhysicsInterface.mak". -!MESSAGE -!MESSAGE You can specify a configuration when running NMAKE -!MESSAGE by defining the macro CFG on the command line. For example: -!MESSAGE -!MESSAGE NMAKE /f "PhysicsInterface.mak" CFG="PhysicsInterface - Win32 Debug" -!MESSAGE -!MESSAGE Possible choices for configuration are: -!MESSAGE -!MESSAGE "PhysicsInterface - Win32 Release" (based on "Win32 (x86) Static Library") -!MESSAGE "PhysicsInterface - Win32 Debug" (based on "Win32 (x86) Static Library") -!MESSAGE - -# Begin Project -# PROP AllowPerConfigDependencies 0 -# PROP Scc_ProjName "" -# PROP Scc_LocalPath "" -CPP=cl.exe -RSC=rc.exe - -!IF "$(CFG)" == "PhysicsInterface - Win32 Release" - -# PROP BASE Use_MFC 0 -# PROP BASE Use_Debug_Libraries 0 -# PROP BASE Output_Dir "Release" -# PROP BASE Intermediate_Dir "Release" -# PROP BASE Target_Dir "" -# PROP Use_MFC 0 -# PROP Use_Debug_Libraries 0 -# PROP Output_Dir "Release" -# PROP Intermediate_Dir "Release" -# PROP Target_Dir "" -LINK32=link.exe -lib -MTL=midl.exe -# ADD BASE CPP /nologo /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /YX /FD /c -# ADD CPP /nologo /W3 /GX /O2 /I "..\..\..\LinearMath" /I "..\..\..\Extras\PhysicsInterface\Common" /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /YX /FD /c -# ADD BASE RSC /l 0x809 /d "NDEBUG" -# ADD RSC /l 0x809 /d "NDEBUG" -BSC32=bscmake.exe -# ADD BASE BSC32 /nologo -# ADD BSC32 /nologo -LIB32=link.exe -lib -# ADD BASE LIB32 /nologo -# ADD LIB32 /nologo - -!ELSEIF "$(CFG)" == "PhysicsInterface - Win32 Debug" - -# PROP BASE Use_MFC 0 -# PROP BASE Use_Debug_Libraries 1 -# PROP BASE Output_Dir "Debug" -# PROP BASE Intermediate_Dir "Debug" -# PROP BASE Target_Dir "" -# PROP Use_MFC 0 -# PROP Use_Debug_Libraries 1 -# PROP Output_Dir "Debug" -# PROP Intermediate_Dir "Debug" -# PROP Target_Dir "" -LINK32=link.exe -lib -MTL=midl.exe -# ADD BASE CPP /nologo /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /YX /FD /GZ /c -# ADD CPP /nologo /W3 /Gm /GX /ZI /Od /I "..\..\..\LinearMath" /I "..\..\..\Extras\PhysicsInterface\Common" /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /YX /FD /GZ /c -# ADD BASE RSC /l 0x809 /d "_DEBUG" -# ADD RSC /l 0x809 /d "_DEBUG" -BSC32=bscmake.exe -# ADD BASE BSC32 /nologo -# ADD BSC32 /nologo -LIB32=link.exe -lib -# ADD BASE LIB32 /nologo -# ADD LIB32 /nologo - -!ENDIF - -# Begin Target - -# Name "PhysicsInterface - Win32 Release" -# Name "PhysicsInterface - Win32 Debug" -# Begin Group "Source Files" - -# PROP Default_Filter "cpp;c;cxx;rc;def;r;odl;idl;hpj;bat" -# Begin Source File - -SOURCE=.\PHY_IMotionState.cpp -# End Source File -# Begin Source File - -SOURCE=.\PHY_IPhysicsController.cpp -# End Source File -# Begin Source File - -SOURCE=.\PHY_IPhysicsEnvironment.cpp -# End Source File -# End Group -# Begin Group "Header Files" - -# PROP Default_Filter "h;hpp;hxx;hm;inl" -# Begin Source File - -SOURCE=.\PHY_DynamicTypes.h -# End Source File -# Begin Source File - -SOURCE=.\PHY_IMotionState.h -# End Source File -# Begin Source File - -SOURCE=.\PHY_IPhysicsController.h -# End Source File -# Begin Source File - -SOURCE=.\PHY_IPhysicsEnvironment.h -# End Source File -# Begin Source File - -SOURCE=.\PHY_Pro.h -# End Source File -# End Group -# End Target -# End Project diff --git a/Extras/PhysicsInterface/Common/PhysicsInterfaceCommon/PhysicsInterfaceCommon_vc7.vcproj b/Extras/PhysicsInterface/Common/PhysicsInterfaceCommon/PhysicsInterfaceCommon_vc7.vcproj deleted file mode 100644 index c47e8b1b4..000000000 --- a/Extras/PhysicsInterface/Common/PhysicsInterfaceCommon/PhysicsInterfaceCommon_vc7.vcproj +++ /dev/null @@ -1,143 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Extras/PhysicsInterface/Common/PhysicsInterfaceCommon/PhysicsInterfaceCommon_vc8.vcproj b/Extras/PhysicsInterface/Common/PhysicsInterfaceCommon/PhysicsInterfaceCommon_vc8.vcproj deleted file mode 100644 index b0c5fe09e..000000000 --- a/Extras/PhysicsInterface/Common/PhysicsInterfaceCommon/PhysicsInterfaceCommon_vc8.vcproj +++ /dev/null @@ -1,211 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Extras/PhysicsInterface/Jamfile b/Extras/PhysicsInterface/Jamfile deleted file mode 100644 index ee7fd2fd1..000000000 --- a/Extras/PhysicsInterface/Jamfile +++ /dev/null @@ -1,4 +0,0 @@ -SubDir TOP Extras PhysicsInterface ; - -SubInclude TOP Extras PhysicsInterface Common ; -SubInclude TOP Extras PhysicsInterface CcdPhysics ; diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index f895521bf..5e4950613 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -21,13 +21,33 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" //for raycasting #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" - #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "LinearMath/btAabbUtil2.h" +//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor) +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" + #include +btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache) +:m_dispatcher1(dispatcher), +m_broadphasePairCache(pairCache), +m_ownsDispatcher(false), +m_ownsBroadphasePairCache(false) +{ +} + +btCollisionWorld::btCollisionWorld() +: m_dispatcher1(new btCollisionDispatcher()), +m_broadphasePairCache(new btSimpleBroadphase()), +m_ownsDispatcher(true), +m_ownsBroadphasePairCache(true) +{ +} + + btCollisionWorld::~btCollisionWorld() { //clean up remaining objects @@ -50,6 +70,11 @@ btCollisionWorld::~btCollisionWorld() } } + if (m_ownsDispatcher) + delete m_dispatcher1; + if (m_ownsBroadphasePairCache) + delete m_broadphasePairCache; + } @@ -100,14 +125,14 @@ void btCollisionWorld::performDiscreteCollisionDetection() { m_collisionObjects[i]->m_cachedInvertedWorldTransform = m_collisionObjects[i]->m_worldTransform.inverse(); m_collisionObjects[i]->m_collisionShape->getAabb(m_collisionObjects[i]->m_worldTransform,aabbMin,aabbMax); - m_pairCache->setAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax); + m_broadphasePairCache->setAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax); } - m_pairCache->refreshOverlappingPairs(); + m_broadphasePairCache->refreshOverlappingPairs(); btDispatcher* dispatcher = getDispatcher(); if (dispatcher) - dispatcher->dispatchAllCollisionPairs(m_pairCache,dispatchInfo); + dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache,dispatchInfo); } diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index c52e24d33..a5705b9bc 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -88,28 +88,30 @@ protected: btDispatcher* m_dispatcher1; - btOverlappingPairCache* m_pairCache; + btOverlappingPairCache* m_broadphasePairCache; + bool m_ownsDispatcher; + bool m_ownsBroadphasePairCache; public: - btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache) - :m_dispatcher1(dispatcher), - m_pairCache(pairCache) - { + //this constructor will create and own a dispatcher and paircache and delete it at destruction + btCollisionWorld(); + + //this constructor doesn't own the dispatcher and paircache/broadphase + btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache); - } virtual ~btCollisionWorld(); btBroadphaseInterface* getBroadphase() { - return m_pairCache; + return m_broadphasePairCache; } btOverlappingPairCache* getPairCache() { - return m_pairCache; + return m_broadphasePairCache; } diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 6d40fd14c..12722674c 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -36,10 +36,13 @@ subject to the following restrictions: #include btDiscreteDynamicsWorld::btDiscreteDynamicsWorld() -:btDynamicsWorld(new btCollisionDispatcher(),new btSimpleBroadphase()), +:btDynamicsWorld(), m_constraintSolver(new btSequentialImpulseConstraintSolver) { m_islandManager = new btSimulationIslandManager(); + m_ownsIslandManager = true; + m_ownsConstraintSolver = true; + } btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) @@ -47,20 +50,18 @@ btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOver m_constraintSolver(constraintSolver) { m_islandManager = new btSimulationIslandManager(); + m_ownsIslandManager = true; + m_ownsConstraintSolver = false; } btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() { - delete m_islandManager ; - - delete m_constraintSolver; - - //delete the dispatcher and paircache - delete m_dispatcher1; - m_dispatcher1 = 0; - delete m_pairCache; - m_pairCache = 0; + //only delete it when we created it + if (m_ownsIslandManager) + delete m_islandManager; + if (m_ownsConstraintSolver) + delete m_constraintSolver; } void btDiscreteDynamicsWorld::stepSimulation(float timeStep) @@ -291,7 +292,7 @@ void btDiscreteDynamicsWorld::updateAabbs() { btPoint3 minAabb,maxAabb; colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb); - btSimpleBroadphase* bp = (btSimpleBroadphase*)m_pairCache; + btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache; bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb); } } diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index d75a7425d..394a45eed 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -40,6 +40,9 @@ protected: std::vector m_constraints; + bool m_ownsIslandManager; + bool m_ownsConstraintSolver; + std::vector m_vehicles; void predictUnconstraintMotion(float timeStep); @@ -61,8 +64,10 @@ protected: public: + ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver); + ///this btDiscreteDynamicsWorld will create and own dispatcher, pairCache and constraintSolver, and deletes it in the destructor. btDiscreteDynamicsWorld(); virtual ~btDiscreteDynamicsWorld(); diff --git a/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 4c61fc13b..ab83a622a 100644 --- a/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -24,11 +24,15 @@ class btDynamicsWorld : public btCollisionWorld { public: + btDynamicsWorld() + { + } + btDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache) :btCollisionWorld(dispatcher,pairCache) { - } + virtual ~btDynamicsWorld() { } diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 4e3270417..1464547e5 100644 --- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -23,15 +23,15 @@ subject to the following restrictions: btSimpleDynamicsWorld::btSimpleDynamicsWorld() -:btDynamicsWorld(new btCollisionDispatcher(),new btSimpleBroadphase()), -m_constraintSolver(new btSequentialImpulseConstraintSolver) +:m_constraintSolver(new btSequentialImpulseConstraintSolver), +m_ownsConstraintSolver(true) { - } btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) :btDynamicsWorld(dispatcher,pairCache), -m_constraintSolver(constraintSolver) +m_constraintSolver(constraintSolver), +m_ownsConstraintSolver(false) { } @@ -39,13 +39,8 @@ m_constraintSolver(constraintSolver) btSimpleDynamicsWorld::~btSimpleDynamicsWorld() { - delete m_constraintSolver; - - //delete the dispatcher and paircache - delete m_dispatcher1; - m_dispatcher1 = 0; - delete m_pairCache; - m_pairCache = 0; + if (m_ownsConstraintSolver) + delete m_constraintSolver; } void btSimpleDynamicsWorld::stepSimulation(float timeStep) @@ -86,7 +81,7 @@ void btSimpleDynamicsWorld::updateAabbs() { btPoint3 minAabb,maxAabb; colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb); - btSimpleBroadphase* bp = (btSimpleBroadphase*)m_pairCache; + btBroadphaseInterface* bp = getBroadphase(); bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb); } } diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index 7eb8b893b..9aac55db8 100644 --- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -33,6 +33,8 @@ protected: btConstraintSolver* m_constraintSolver; + bool m_ownsConstraintSolver; + void predictUnconstraintMotion(float timeStep); void integrateTransforms(float timeStep); @@ -42,10 +44,12 @@ protected: public: - btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver); - + ///this btSimpleDynamicsWorld constructor creates and owns dispatcher, broadphase pairCache and constraintSolver btSimpleDynamicsWorld(); + ///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver + btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver); + virtual ~btSimpleDynamicsWorld(); virtual void stepSimulation( float timeStep);