From 701b9c21fd095716fef416bb10c90c51b378e48d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 18 Jul 2005 05:41:00 +0000 Subject: more preparations for bullet physics, and some eol issues with SCA_Joystick* --- .../CcdPhysics/CcdPhysicsEnvironment.cpp | 760 +++++++++++++++++++++ 1 file changed, 760 insertions(+) create mode 100644 extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp (limited to 'extern') diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp new file mode 100644 index 00000000000..2a89cd0cf4a --- /dev/null +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -0,0 +1,760 @@ +#include "CcdPhysicsEnvironment.h" +#include "CcdPhysicsController.h" + +#include +#include "SimdTransform.h" +#include "Dynamics/RigidBody.h" +#include "BroadphaseCollision/BroadPhaseInterface.h" +#include "BroadphaseCollision/SimpleBroadphase.h" + +#include "CollisionShapes/ConvexShape.h" +#include "BroadphaseCollision/CollisionDispatcher.h" +#include "NarrowPhaseCollision/PersistentManifold.h" +#include "CollisionShapes/TriangleMeshShape.h" +#include "ConstraintSolver/OdeConstraintSolver.h" +#include "ConstraintSolver/SimpleConstraintSolver.h" + + + +#include "CollisionDispatch/ToiContactDispatcher.h" + + +#include "CollisionDispatch/EmptyCollisionAlgorithm.h" +#include "CollisionDispatch/UnionFind.h" + +#include "NarrowPhaseCollision/RaycastCallback.h" + +bool useIslands = true; + +#include "ConstraintSolver/ConstraintSolver.h" +#include "ConstraintSolver/Point2PointConstraint.h" +//#include "BroadphaseCollision/QueryDispatcher.h" +//#include "BroadphaseCollision/QueryBox.h" +//todo: change this to allow dynamic registration of types! + +unsigned long gNumIterations = 10; + +#ifdef WIN32 +void DrawRasterizerLine(const float* from,const float* to,int color); +#endif + + +#include "ConstraintSolver/ContactConstraint.h" + + + +#include + + + +CcdPhysicsEnvironment::CcdPhysicsEnvironment(ToiContactDispatcher* dispatcher,BroadphaseInterface* bp) +:m_dispatcher(dispatcher), +m_broadphase(bp), +m_scalingPropagated(false) +{ + if (!m_dispatcher) + { + OdeConstraintSolver* solver = new OdeConstraintSolver(); + //SimpleConstraintSolver* solver= new SimpleConstraintSolver(); + m_dispatcher = new ToiContactDispatcher(solver); + } + if (!m_broadphase) + { + m_broadphase = new SimpleBroadphase(); + } + + m_debugDrawer = 0; + m_gravity = SimdVector3(0.f,-10.f,0.f); + + +} + +void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl) +{ + ctrl->GetRigidBody()->setGravity( m_gravity ); + m_controllers.push_back(ctrl); + + BroadphaseInterface* scene = m_broadphase; + + CollisionShape* shapeinterface = ctrl->GetCollisionShape(); + + assert(shapeinterface); + + const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform(); + + RigidBody* body = ctrl->GetRigidBody(); + + SimdPoint3 minAabb,maxAabb; + + shapeinterface->GetAabb(t,minAabb,maxAabb); + + float timeStep = 0.02f; + + + //extent it with the motion + + SimdVector3 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 = SimdVector3(minAabbx,minAabby,minAabbz); + maxAabb = SimdVector3(maxAabbx,maxAabby,maxAabbz); + + if (!ctrl->m_broadphaseHandle) + { + int type = shapeinterface->GetShapeType(); + ctrl->m_broadphaseHandle = scene->CreateProxy( + ctrl->GetRigidBody(), + type, + minAabb, + maxAabb); + } + + body->SetCollisionShape( shapeinterface ); + + + +} + +void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl) +{ + + //also remove constraint + + { + std::vector::iterator i; + + for (i=m_p2pConstraints.begin(); + !(i==m_p2pConstraints.end()); i++) + { + Point2PointConstraint* p2p = (*i); + if ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() || + (&p2p->GetRigidBodyB() == ctrl->GetRigidBody()))) + { + removeConstraint(int(p2p)); + //only 1 constraint per constroller + break; + } + } + } + + { + std::vector::iterator i; + + for (i=m_p2pConstraints.begin(); + !(i==m_p2pConstraints.end()); i++) + { + Point2PointConstraint* p2p = (*i); + if ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() || + (&p2p->GetRigidBodyB() == ctrl->GetRigidBody()))) + { + removeConstraint(int(p2p)); + //only 1 constraint per constroller + break; + } + } + } + + + + bool removeFromBroadphase = false; + + { + BroadphaseInterface* scene = m_broadphase; + BroadphaseProxy* bp = (BroadphaseProxy*)ctrl->m_broadphaseHandle; + + if (removeFromBroadphase) + { + } + // + // only clear the cached algorithms + // + scene->CleanProxyFromPairs(bp); + } + { + 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(); + } + } +} + +void CcdPhysicsEnvironment::UpdateActivationState() +{ + m_dispatcher->InitUnionFind(); + + // put the index into m_controllers into m_tag + { + std::vector::iterator i; + + int index = 0; + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + CcdPhysicsController* ctrl = (*i); + RigidBody* body = ctrl->GetRigidBody(); + body->m_islandTag1 = index; + body->m_hitFraction = 1.f; + index++; + + } + } + // do the union find + + m_dispatcher->FindUnions(); + + // put the islandId ('find' value) into m_tag + { + UnionFind& unionFind = m_dispatcher->GetUnionFind(); + + std::vector::iterator i; + + int index = 0; + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + CcdPhysicsController* ctrl = (*i); + RigidBody* body = ctrl->GetRigidBody(); + + + if (body->mergesSimulationIslands()) + { + body->m_islandTag1 = unionFind.find(index); + } else + { + body->m_islandTag1 = -1; + } + index++; + } + } + +} + +bool gPredictCollision = false;//true;//false; + + +/// Perform an integration step of duration 'timeStep'. +bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) +{ + + +// printf("CcdPhysicsEnvironment::proceedDeltaTime\n"); + + if (timeStep == 0.f) + return true; + + //clamp hardcoded for now + if (timeStep > 0.02) + timeStep = 0.02; + + //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 EXTRA_PHYSICS_PROFILE + cpuProfile.begin("integrate force"); +#endif //EXTRA_PHYSICS_PROFILE + + + + { +// std::vector::iterator i; + + + + int k; + for (k=0;kGetRigidBody(); + if (body->GetActivationState() != ISLAND_SLEEPING) + { + body->applyForces( timeStep); + body->integrateVelocities( timeStep); + } + + } + } +#ifdef EXTRA_PHYSICS_PROFILE + cpuProfile.end("integrate force"); +#endif //EXTRA_PHYSICS_PROFILE + BroadphaseInterface* scene = m_broadphase; + + + // + // collision detection (?) + // + + + + + + int numsubstep = gNumIterations; + + + DispatcherInfo dispatchInfo; + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; +#ifdef EXTRA_PHYSICS_PROFILE + cpuProfile.begin("cd"); +#endif //EXTRA_PHYSICS_PROFILE + + scene->DispatchAllCollisionPairs(*m_dispatcher,dispatchInfo);///numsubstep,g); + +#ifdef EXTRA_PHYSICS_PROFILE + cpuProfile.end("cd"); +#endif //EXTRA_PHYSICS_PROFILE + + + + +#ifdef EXTRA_PHYSICS_PROFILE + cpuProfile.begin("solver"); +#endif //EXTRA_PHYSICS_PROFILE + + int numRigidBodies = m_controllers.size(); + + UpdateActivationState(); + + //contacts + m_dispatcher->SolveConstraints(timeStep, gNumIterations ,numRigidBodies); + +#ifdef EXTRA_PHYSICS_PROFILE + cpuProfile.end("solver"); +#endif //EXTRA_PHYSICS_PROFILE + + for (int g=0;gBuildJacobian(); + p2p->SolveConstraint( timeStep ); + + } + /* + //vehicles + int numVehicles = m_vehicles.size(); + for (i=0;iUpdateVehicle( timeStep ); + } + */ + + + + } + + { + + + + { + + std::vector::iterator i; + + // + // update aabbs, only for moving objects (!) + // + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + CcdPhysicsController* ctrl = (*i); + RigidBody* body = ctrl->GetRigidBody(); + + + SimdPoint3 minAabb,maxAabb; + CollisionShape* shapeinterface = ctrl->GetCollisionShape(); + shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(), + body->getLinearVelocity(),body->getAngularVelocity(), + timeStep,minAabb,maxAabb); + + + BroadphaseProxy* bp = (BroadphaseProxy*) ctrl->m_broadphaseHandle; + if (bp) + { + +#ifdef WIN32 + SimdVector3 color (1,0,0); + if (m_debugDrawer) + m_debugDrawer->DrawLine(minAabb,maxAabb,color); +#endif + scene->SetAabb(bp,minAabb,maxAabb); + } + } + + float toi = 1.f; + + + + if (gPredictCollision) + { + DispatcherInfo dispatchInfo; + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS; + + scene->DispatchAllCollisionPairs( *m_dispatcher,dispatchInfo);///numsubstep,g); + toi = dispatchInfo.m_timeOfImpact; + } + + // + // integrating solution + // + + { + std::vector::iterator i; + + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + + CcdPhysicsController* ctrl = *i; + + SimdTransform predictedTrans; + RigidBody* body = ctrl->GetRigidBody(); + if (body->GetActivationState() != ISLAND_SLEEPING) + { + body->predictIntegratedTransform(timeStep* toi, predictedTrans); + body->proceedToTransform( predictedTrans); + + } + } + + } + + + + + + // + // disable sleeping physics objects + // + + std::vector m_sleepingControllers; + + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + CcdPhysicsController* ctrl = (*i); + RigidBody* body = ctrl->GetRigidBody(); + + if (ctrl->wantsSleeping()) + { + if (body->GetActivationState() == ACTIVE_TAG) + body->SetActivationState( WANTS_DEACTIVATION ); + } else + { + 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); + } + } + } + + + + + } + + SyncMotionStates(timeStep); + + } + return true; +} + +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 = SimdVector3(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 DASHDASJKHASDJK +class RaycastingQueryBox : public QueryBox +{ + + SimdVector3 m_aabbMin; + + SimdVector3 m_aabbMax; + + + +public: + + RaycastCallback m_raycastCallback; + + + RaycastingQueryBox(QueryBoxConstructionInfo& ci,const SimdVector3& from,const SimdVector3& to) + : QueryBox(ci), + m_raycastCallback(from,to) + { + for (int i=0;i<3;i++) + { + float fromI = from[i]; + float toI = to[i]; + if (fromI < toI) + { + m_aabbMin[i] = fromI; + m_aabbMax[i] = toI; + } else + { + m_aabbMin[i] = toI; + m_aabbMax[i] = fromI; + } + } + + } + virtual void AddCollider( BroadphaseProxy* proxy) + { + //perform raycast if wanted, and update the m_hitFraction + + if (proxy->GetClientObjectType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + //do it + RigidBody* body = (RigidBody*)proxy->m_clientObject; + TriangleMeshInterface* meshInterface = (TriangleMeshInterface*) + body->m_minkowski1; + + //if the hit is closer, record the proxy! + float curFraction = m_raycastCallback.m_hitFraction; + + meshInterface->ProcessAllTriangles(&m_raycastCallback,m_aabbMin,m_aabbMax); + + if (m_raycastCallback.m_hitFraction < curFraction) + { + m_raycastCallback.m_hitProxy = proxy; + } + + } + + } +}; + +struct InternalVehicleRaycaster : public VehicleRaycaster +{ + + CcdPhysicsEnvironment* m_env; + +public: + + InternalVehicleRaycaster(CcdPhysicsEnvironment* env) + : m_env(env) + { + + } + + virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& result) + { + + return 0; + } + +}; + +#endif +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; + + RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0; + RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0; + + ASSERT(rb0); + + SimdVector3 pivotInA(pivotX,pivotY,pivotZ); + SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA; + + switch (type) + { + case PHY_POINT2POINT_CONSTRAINT: + { + + Point2PointConstraint* p2p = 0; + + if (rb1) + { + p2p = new Point2PointConstraint(*rb0, + *rb1,pivotInA,pivotInB); + } else + { + p2p = new Point2PointConstraint(*rb0, + pivotInA); + } + + m_p2pConstraints.push_back(p2p); + return 0; + + break; + } + default: + { + } + }; + + //RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB + + return 0; + +} + +void CcdPhysicsEnvironment::removeConstraint(int constraintid) +{ + + Point2PointConstraint* p2p = (Point2PointConstraint*) constraintid; + + std::vector::iterator i = + std::find(m_p2pConstraints.begin(), m_p2pConstraints.end(), p2p); + + if (!(i == m_p2pConstraints.end()) ) + { + std::swap(*i, m_p2pConstraints.back()); + m_p2pConstraints.pop_back(); + } + +} +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) +{ + + +// m_broadphase->cast( + return 0; +} + + + +int CcdPhysicsEnvironment::getNumContactPoints() +{ + return 0; +} + +void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ) +{ + +} + + + + + +Dispatcher* CcdPhysicsEnvironment::GetDispatcher() +{ + return m_dispatcher; +} + +CcdPhysicsEnvironment::~CcdPhysicsEnvironment() +{ + + + m_vehicles.clear(); + + //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; + +} + + +int CcdPhysicsEnvironment::GetNumControllers() +{ + return m_controllers.size(); +} + + +CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index) +{ + return m_controllers[index]; +} + + +int CcdPhysicsEnvironment::GetNumManifolds() const +{ + return m_dispatcher->GetNumManifolds(); +} + +const PersistentManifold* CcdPhysicsEnvironment::GetManifold(int index) const +{ + return m_dispatcher->GetManifoldByIndexInternal(index); +} -- cgit v1.2.3