- fixed Bullet noResponse/ghost mode
authorErwin Coumans <blender@erwincoumans.com>
Fri, 28 Apr 2006 00:08:18 +0000 (00:08 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Fri, 28 Apr 2006 00:08:18 +0000 (00:08 +0000)
- added ccd option (future use, very basic and inefficient)
- some internal Bullet refactoring/improvements

26 files changed:
extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp
extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp
extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h
extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp
extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h
extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp
extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h
extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.cpp
extern/bullet/Bullet/CollisionShapes/TriangleCallback.h
extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp
extern/bullet/Bullet/NarrowPhaseCollision/ContinuousConvexCollision.cpp
extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp
extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp
extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h
extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp
extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.h
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h
extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h
extern/bullet/LinearMath/IDebugDraw.h
source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
source/gameengine/Physics/Bullet/CcdPhysicsController.h
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h

index 1a3f192e533f62962d89f7fe6a87f88009ce8626..5fe5157b420a0435c96d13f7eca0144094372dc6 100644 (file)
@@ -128,6 +128,7 @@ void CollisionDispatcher::BuildAndProcessIslands(int numBodies, IslandCallback*
                         CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
                         CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
 
+
                         
                         {
                                if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
@@ -145,7 +146,8 @@ void CollisionDispatcher::BuildAndProcessIslands(int numBodies, IslandCallback*
                                                allSleeping = false;
                                        }
 
-                                       islandmanifold.push_back(manifold);
+                                       if (NeedsResponse(*colObj0,*colObj1))
+                                               islandmanifold.push_back(manifold);
                                }
                         }
                }
@@ -196,7 +198,10 @@ void CollisionDispatcher::BuildAndProcessIslands(int numBodies, IslandCallback*
 
                        }
 
-                       callback->ProcessIsland(&islandmanifold[0],islandmanifold.size());
+                       if (islandmanifold.size())
+                       {
+                               callback->ProcessIsland(&islandmanifold[0],islandmanifold.size());
+                       }
 
                }
        }
index 5264662b51902a8c1372a9c397f42fa3c87e99c0..812ffa391dd2ecb8d8e721c8519f728942603ccb 100644 (file)
@@ -17,8 +17,14 @@ subject to the following restrictions:
 #include "CollisionDispatcher.h"
 #include "CollisionDispatch/CollisionObject.h"
 #include "CollisionShapes/CollisionShape.h"
+#include "CollisionShapes/SphereShape.h" //for raycasting
+#include "CollisionShapes/TriangleMeshShape.h" //for raycasting
+#include "NarrowPhaseCollision/RaycastCallback.h"
 
+#include "NarrowPhaseCollision/SubSimplexConvexCast.h"
 #include "BroadphaseCollision/BroadphaseInterface.h"
+#include "AabbUtil2.h"
+
 #include <algorithm>
 
 void   CollisionWorld::UpdateActivationState()
@@ -145,3 +151,147 @@ void      CollisionWorld::RemoveCollisionObject(CollisionObject* collisionObject)
                        m_collisionObjects.pop_back();
                }
 }
+
+
+
+void   CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback)
+{
+
+       
+       SimdTransform   rayFromTrans,rayToTrans;
+       rayFromTrans.setIdentity();
+       rayFromTrans.setOrigin(rayFromWorld);
+       rayToTrans.setIdentity();
+       
+       rayToTrans.setOrigin(rayToWorld);
+
+       //do culling based on aabb (rayFrom/rayTo)
+       SimdVector3 rayAabbMin = rayFromWorld;
+       SimdVector3 rayAabbMax = rayFromWorld;
+       rayAabbMin.setMin(rayToWorld);
+       rayAabbMax.setMax(rayToWorld);
+
+       SphereShape pointShape(0.0f);
+
+       /// brute force go over all objects. Once there is a broadphase, use that, or
+       /// add a raycast against aabb first.
+       
+       std::vector<CollisionObject*>::iterator iter;
+       
+       for (iter=m_collisionObjects.begin();
+       !(iter==m_collisionObjects.end()); iter++)
+       {
+               
+               CollisionObject*        collisionObject= (*iter);
+
+               //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+               SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+               collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);
+
+               //check aabb overlap
+
+               if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
+               {
+                       if (collisionObject->m_collisionShape->IsConvex())
+                       {
+                               ConvexCast::CastResult castResult;
+                               castResult.m_fraction = 1.f;//??
+
+                               ConvexShape* convexShape = (ConvexShape*) collisionObject->m_collisionShape;
+                               VoronoiSimplexSolver    simplexSolver;
+                               SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
+                               //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
+                               //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
+                               
+                               if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,collisionObject->m_worldTransform,collisionObject->m_worldTransform,castResult))
+                               {
+                                       //add hit
+                                       if (castResult.m_normal.length2() > 0.0001f)
+                                       {
+                                               castResult.m_normal.normalize();
+                                               if (castResult.m_fraction < resultCallback.m_closestHitFraction)
+                                               {
+                                                       
+
+                                                       CollisionWorld::LocalRayResult localRayResult
+                                                               (
+                                                                       collisionObject, 
+                                                                       0,
+                                                                       castResult.m_normal,
+                                                                       castResult.m_fraction
+                                                               );
+
+                                                       resultCallback.AddSingleResult(localRayResult);
+
+                                               }
+                                       }
+                               }
+                       }
+                       else
+                       {
+                               
+                               if (collisionObject->m_collisionShape->IsConcave())
+                                       {
+
+                                               TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionObject->m_collisionShape;
+                                               
+                                               SimdTransform worldTocollisionObject = collisionObject->m_worldTransform.inverse();
+
+                                               SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+                                               SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+                                               //ConvexCast::CastResult
+
+                                               struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback 
+                                               {
+                                                       RayResultCallback* m_resultCallback;
+                                                       CollisionObject*        m_collisionObject;
+                                                       TriangleMeshShape*      m_triangleMesh;
+
+                                                       BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to,
+                                                               RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape*  triangleMesh):
+                                                               TriangleRaycastCallback(from,to),
+                                                                       m_resultCallback(resultCallback),
+                                                                       m_collisionObject(collisionObject),
+                                                                       m_triangleMesh(triangleMesh)
+                                                               {
+                                                               }
+
+
+                                                       virtual void ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex )
+                                                       {
+                                                               LocalShapeInfo  shapeInfo;
+                                                               shapeInfo.m_shapePart = partId;
+                                                               shapeInfo.m_triangleIndex = triangleIndex;
+                                                               
+                                                               LocalRayResult rayResult
+                                                               (m_collisionObject, 
+                                                                       &shapeInfo,
+                                                                       hitNormalLocal,
+                                                                       hitFraction);
+                                                               
+                                                               m_resultCallback->AddSingleResult(rayResult);
+                                                               
+                                                       }
+       
+                                               };
+
+
+                                               BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
+                                               rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+
+                                               SimdVector3 rayAabbMinLocal = rayFromLocal;
+                                               rayAabbMinLocal.setMin(rayToLocal);
+                                               SimdVector3 rayAabbMaxLocal = rayFromLocal;
+                                               rayAabbMaxLocal.setMax(rayToLocal);
+
+                                               triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
+                                                                                       
+                                       }
+                                       
+
+                       }
+               }
+       }
+
+}
\ No newline at end of file
index 0e7cb80bf209a1c10f1ff68665c42b6b93897863..c83192d9d2fee35e0383f0535d7d78204c50fb23 100644 (file)
@@ -16,9 +16,13 @@ subject to the following restrictions:
 #ifndef COLLISION_WORLD_H
 #define COLLISION_WORLD_H
 
-struct CollisionObject;
+
+class CollisionShape;
 class CollisionDispatcher;
 class BroadphaseInterface;
+#include "SimdVector3.h"
+#include "SimdTransform.h"
+#include "CollisionObject.h"
 
 #include <vector>
 
@@ -26,13 +30,15 @@ class BroadphaseInterface;
 class CollisionWorld
 {
 
+       
+
        std::vector<CollisionObject*>   m_collisionObjects;
        
        CollisionDispatcher*    m_dispatcher;
 
        BroadphaseInterface*    m_broadphase;
 
-       public:
+public:
 
        CollisionWorld(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase)
                :m_dispatcher(dispatcher),
@@ -54,11 +60,93 @@ class CollisionWorld
                return m_dispatcher;
        }
 
+       ///LocalShapeInfo gives extra information for complex shapes
+       ///Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart
+       struct  LocalShapeInfo
+       {
+               int     m_shapePart;
+               int     m_triangleIndex;
+               
+               //const CollisionShape* m_shapeTemp;
+               //const SimdTransform*  m_shapeLocalTransform;
+       };
+
+       struct  LocalRayResult
+       {
+               LocalRayResult(const CollisionObject*   collisionObject, 
+                       LocalShapeInfo* localShapeInfo,
+                       const SimdVector3&              hitNormalLocal,
+                       float hitFraction)
+               :m_collisionObject(collisionObject),
+               m_localShapeInfo(m_localShapeInfo),
+               m_hitNormalLocal(hitNormalLocal),
+               m_hitFraction(hitFraction)
+               {
+               }
+
+               const CollisionObject*  m_collisionObject;
+               LocalShapeInfo*                 m_localShapeInfo;
+               const SimdVector3&              m_hitNormalLocal;
+               float                                   m_hitFraction;
+
+       };
+
+       ///RayResultCallback is used to report new raycast results
+       struct  RayResultCallback
+       {
+               float   m_closestHitFraction;
+               bool    HasHit()
+               {
+                       return (m_closestHitFraction < 1.f);
+               }
+
+               RayResultCallback()
+                       :m_closestHitFraction(1.f)
+               {
+               }
+               virtual float   AddSingleResult(const LocalRayResult& rayResult) = 0;
+       };
+
+       struct  ClosestRayResultCallback : public RayResultCallback
+       {
+               ClosestRayResultCallback(SimdVector3    rayFromWorld,SimdVector3        rayToWorld)
+               :m_rayFromWorld(rayFromWorld),
+               m_rayToWorld(rayToWorld),
+               m_collisionObject(0)
+               {
+               }
+
+               SimdVector3     m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
+               SimdVector3     m_rayToWorld;
+
+               SimdVector3     m_hitNormalWorld;
+               SimdVector3     m_hitPointWorld;
+               const CollisionObject*  m_collisionObject;
+               
+               virtual float   AddSingleResult(const LocalRayResult& rayResult)
+               {
+                       //caller already does the filter on the m_closestHitFraction
+                       assert(rayResult.m_hitFraction < m_closestHitFraction);
+
+                       m_closestHitFraction = rayResult.m_hitFraction;
+                       m_collisionObject = rayResult.m_collisionObject;
+                       m_hitNormalWorld = m_collisionObject->m_worldTransform.getBasis()*rayResult.m_hitNormalLocal;
+                       m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
+                       return rayResult.m_hitFraction;
+               }
+       };
+
+
+       
+
        int     GetNumCollisionObjects() const
        {
                return m_collisionObjects.size();
        }
 
+       void    RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback);
+
+
        void    AddCollisionObject(CollisionObject* collisionObject);
 
        void    RemoveCollisionObject(CollisionObject* collisionObject);
index d600c061638e46e2ab6466e9b0661309301f1c77..292b0d72de1d710a2bd17cd5ccc8f4b817354877 100644 (file)
@@ -68,7 +68,7 @@ void  BoxTriangleCallback::ClearCache()
 
 
 
-void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle)
+void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex)
 {
  
        //just for debugging purposes
@@ -178,17 +178,30 @@ void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,Broadp
 float ConvexConcaveCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo)
 {
 
-       //quick approximation using raycast, todo: use proper continuou collision detection
+       //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the ConvexCast)
        CollisionObject* convexbody = (CollisionObject* )m_convex.m_clientObject;
        CollisionObject* triBody = static_cast<CollisionObject* >(m_concave.m_clientObject);
 
        const SimdVector3& from = convexbody->m_worldTransform.getOrigin();
        
        SimdVector3 to = convexbody->m_nextPredictedWorldTransform.getOrigin();
-       //only do if the motion exceeds the 'radius'
+       //todo: only do if the motion exceeds the 'radius'
+
+       struct LocalTriangleRaycastCallback     : public TriangleRaycastCallback
+       {
+               LocalTriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to)
+                       :TriangleRaycastCallback(from,to)
+               {
+               }
+               
+               virtual void ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex )
+               {
+
+               }
+       };
 
 
-       RaycastCallback raycastCallback(from,to);
+       LocalTriangleRaycastCallback raycastCallback(from,to);
 
        raycastCallback.m_hitFraction = convexbody->m_hitFraction;
 
index 7569f4ed32e9023f81611eb19f6f893f034b8f1c..18652334b4ed4db89a085bbc7a896c63f66483e9 100644 (file)
@@ -49,7 +49,7 @@ int   m_triangleCount;
 
        virtual ~BoxTriangleCallback();
 
-       virtual void ProcessTriangle(SimdVector3* triangle);
+       virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex);
        
        void ClearCache();
 
index 2bb3016acdb516eb88a1d9d1e91232ab8a951d19..adb310c3f9991b5e00d44ad0c3045ffbcd0997fa 100644 (file)
@@ -341,6 +341,11 @@ float      ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,Broad
 
        CheckPenetrationDepthSolver();
 
+       //An adhoc way of testing the Continuous Collision Detection algorithms
+       //One object is approximated as a point, to simplify things
+       //Starting in penetration should report no time of impact
+       //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
+       //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
 
        bool needsCollision = m_dispatcher->NeedsCollision(m_box0,m_box1);
 
@@ -351,21 +356,25 @@ float     ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,Broad
        CollisionObject* col0 = static_cast<CollisionObject*>(m_box0.m_clientObject);
        CollisionObject* col1 = static_cast<CollisionObject*>(m_box1.m_clientObject);
        
+       SphereShape     sphere(0.f);
+
        ConvexShape* min0 = static_cast<ConvexShape*>(col0->m_collisionShape);
        ConvexShape* min1 = static_cast<ConvexShape*>(col1->m_collisionShape);
        
        ConvexCast::CastResult result;
 
+
        VoronoiSimplexSolver voronoiSimplex;
-       //SubsimplexConvexCast ccd(&voronoiSimplex);
-       //GjkConvexCast ccd(&voronoiSimplex);
-       
-       ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,m_penetrationDepthSolver);
+       SubsimplexConvexCast ccd0(&sphere,min1,&voronoiSimplex);
+
+       ///Simplification, one object is simplified as a sphere
+       GjkConvexCast ccd1(&sphere,min0,&voronoiSimplex);
+       //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
 
        if (disableCcd)
                return 1.f;
 
-       if (ccd.calcTimeOfImpact(col0->m_worldTransform,col0->m_nextPredictedWorldTransform,
+       if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_nextPredictedWorldTransform,
                col1->m_worldTransform,col1->m_nextPredictedWorldTransform,result))
        {
        
@@ -381,7 +390,9 @@ float       ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,Broad
        }
 
 
-       return 1.f;
+       
 
 
+       return 1.f;
+
 }
index c96d03e3989084a3cc182f66d5d52253a14cefa3..c8ace81dda16e2467071a62b85755046fce9245c 100644 (file)
@@ -27,7 +27,7 @@ class ConvexPenetrationDepthSolver;
 ///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
 class ConvexConvexAlgorithm : public CollisionAlgorithm
 {
-       ConvexPenetrationDepthSolver*   m_penetrationDepthSolver;
+       //ConvexPenetrationDepthSolver* m_penetrationDepthSolver;
        VoronoiSimplexSolver    m_simplexSolver;
        GjkPairDetector m_gjkPairDetector;
        bool    m_useEpa;
index c054d689e75af5800fe2289f678642d046422452..d2624dcdfe1cc9077fd52104e3ba0e9391ea39b7 100644 (file)
@@ -108,7 +108,7 @@ void        BvhTriangleMeshShape::ProcessAllTriangles(TriangleCallback* callback,const
 #endif //DEBUG_TRIANGLE_MESH
                        }
 
-                       m_callback->ProcessTriangle(m_triangle);
+                       m_callback->ProcessTriangle(m_triangle,node->m_subPart,node->m_triangleIndex);
                        m_meshInterface->unLockReadOnlyVertexBase(node->m_subPart);
                }
 
index 80519160c9c45c0cdbef6ee07ea08a99e3b18a11..810d4e62644ac588ff01f80ce53ba68f81ce08da 100644 (file)
@@ -24,7 +24,7 @@ class TriangleCallback
 public:
 
        virtual ~TriangleCallback();
-       virtual void ProcessTriangle(SimdVector3* triangle) = 0;
+       virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex) = 0;
 };
 
 class InternalTriangleIndexCallback
index 486b41fbc000221c6baf735bdda0a50f822a9532..73b53cd02666e2f88efcc57f02142e9db2364240 100644 (file)
@@ -92,7 +92,7 @@ public:
                m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis();
        }
 
-       virtual void ProcessTriangle( SimdVector3* triangle)
+       virtual void ProcessTriangle( SimdVector3* triangle,int partId, int triangleIndex)
        {
                for (int i=0;i<3;i++)
                {
@@ -158,7 +158,7 @@ void        TriangleMeshShape::ProcessAllTriangles(TriangleCallback* callback,const Sim
                        if (TestTriangleAgainstAabb2(&triangle[0],m_aabbMin,m_aabbMax))
                        {
                                //check aabb in triangle-space, before doing this
-                               m_callback->ProcessTriangle(triangle);
+                               m_callback->ProcessTriangle(triangle,partId,triangleIndex);
                        }
                        
                }
index f5762ee248cb1eb8f93d85d56f8a26e8c81a2a5c..710208a916e717cd49ba8e12a9cf830cb79e582b 100644 (file)
@@ -91,6 +91,10 @@ bool ContinuousConvexCollision::calcTimeOfImpact(
                
                GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);              
                GjkPairDetector::ClosestPointInput input;
+       
+               //we don't use margins during CCD
+               gjk.SetIgnoreMargin(true);
+
                input.m_transformA = fromA;
                input.m_transformB = fromB;
                gjk.GetClosestPoints(input,pointCollector1,0);
index 344a4555bcf18fde45b8ddf1a459688c66c46909..f87e9ced8a74bad41be411fe9a26337d526cec44 100644 (file)
@@ -103,11 +103,17 @@ bool      GjkConvexCast::calcTimeOfImpact(
                n = pointCollector1.m_normalOnBInWorld;
        }
 
+       
+
        if (hasResult)
        {
                SimdScalar dist;
                dist = (c-x).length();
-               
+               if (dist < radius)
+               {
+                       //penetration
+                       lastLambda = 1.f;
+               }
 
                //not close enough
                while (dist > radius)
@@ -153,9 +159,13 @@ bool       GjkConvexCast::calcTimeOfImpact(
 
                }
 
-               result.m_fraction = lambda;
-               result.m_normal = n;
-               return true;
+               if (lastLambda < 1.f)
+               {
+               
+                       result.m_fraction = lastLambda;
+                       result.m_normal = n;
+                       return true;
+               }
        }
 
        return false;
index 1390667a58ad6d08b1e3a184f2b698f1482b3734..f497a77d7e53fd5ae733823c1945b67c896f4f32 100644 (file)
@@ -29,7 +29,8 @@ GjkPairDetector::GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,Simpl
 m_penetrationDepthSolver(penetrationDepthSolver),
 m_simplexSolver(simplexSolver),
 m_minkowskiA(objectA),
-m_minkowskiB(objectB)
+m_minkowskiB(objectB),
+m_ignoreMargin(false)
 {
 }
 
@@ -42,6 +43,13 @@ void GjkPairDetector::GetClosestPoints(const ClosestPointInput& input,Result& ou
        float marginA = m_minkowskiA->GetMargin();
        float marginB = m_minkowskiB->GetMargin();
 
+       //for CCD we don't use margins
+       if (m_ignoreMargin)
+       {
+               marginA = 0.f;
+               marginB = 0.f;
+       }
+
        bool isValid = false;
        bool checkSimplex = false;
        bool checkPenetration = true;
index e18fd20697c01b6041b948db095f2a0a422d1051..c550728f845571188f54d5b46fc1ce06489dab9a 100644 (file)
@@ -38,6 +38,7 @@ class GjkPairDetector : public DiscreteCollisionDetectorInterface
        SimplexSolverInterface* m_simplexSolver;
        ConvexShape* m_minkowskiA;
        ConvexShape* m_minkowskiB;
+       bool            m_ignoreMargin;
 
 public:
 
@@ -64,6 +65,12 @@ public:
        {
                m_penetrationDepthSolver = penetrationDepthSolver;
        }
+
+       void    SetIgnoreMargin(bool ignoreMargin)
+       {
+               m_ignoreMargin = ignoreMargin;
+       }
+
 };
 
 #endif //GJK_PAIR_DETECTOR_H
index 9401aa056999c25d2081f8be170021323802d4eb..fc4a533b22529b9121b4bf5f6989b9b2845ee443 100644 (file)
@@ -16,20 +16,18 @@ subject to the following restrictions:
 
 #include "RaycastCallback.h"
 
-RaycastCallback::RaycastCallback(const SimdVector3& from,const SimdVector3& to)
+TriangleRaycastCallback::TriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to)
        :
        m_from(from),
        m_to(to),
-       m_hitFraction(1.f),
-       m_hitProxy(0),
-       m_hitFound(false)
+       m_hitFraction(1.f)
 {
 
 }
 
 
 
-void RaycastCallback::ProcessTriangle(SimdVector3* triangle)
+void TriangleRaycastCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex)
 {
        
 
@@ -86,17 +84,15 @@ void RaycastCallback::ProcessTriangle(SimdVector3* triangle)
                                        
                                        if ( (float)(cp2.dot(triangleNormal)) >=edge_tolerance) 
                                        {
-                                               m_hitFraction = distance;
+
                                                if ( dist_a > 0 )
                                                {
-                                                       m_hitNormalLocal = triangleNormal;
+                                                       ReportHit(triangleNormal,distance,partId,triangleIndex);
                                                }
                                                else
                                                {
-                                                       m_hitNormalLocal = -triangleNormal;
+                                                       ReportHit(-triangleNormal,distance,partId,triangleIndex);
                                                }
-                                               
-                                               m_hitFound= true;
                                        }
                                }
                        }
index dc31eca76fe186ed2a15ce55bc35a4c0ecefb170..c58fa202fc89d5d164c2c843e9f3cfb62128416e 100644 (file)
@@ -20,26 +20,22 @@ subject to the following restrictions:
 struct BroadphaseProxy;
 
 
-class  RaycastCallback: public TriangleCallback
+class  TriangleRaycastCallback: public TriangleCallback
 {
 public:
 
                //input
        SimdVector3 m_from;
        SimdVector3 m_to;
-       //input / output
-       SimdScalar                      m_hitFraction;
-       BroadphaseProxy*        m_hitProxy;
 
-       //output
-       SimdVector3                     m_hitNormalLocal;
-       bool                            m_hitFound;
+       float   m_hitFraction;
 
+       TriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to);
+       
+       virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex);
 
-       RaycastCallback(const SimdVector3& from,const SimdVector3& to);
-
+       virtual void ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) = 0;
        
-       virtual void ProcessTriangle(SimdVector3* triangle);
 };
 
 #endif //RAYCAST_TRI_CALLBACK_H
index 68010b851807a144e2f49c0bff2ad25115f1e6d4..79b311bd691f69fea0395e6ce2c6af20cf6d7434 100644 (file)
@@ -1,3 +1,18 @@
+/*
+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 "Dynamics/RigidBody.h"
index e670755b1f2667bb6840804a4aac48ae3c31d442..f1933d5513764c8c055c30fe1ab16da0e5963453 100644 (file)
@@ -1,3 +1,18 @@
+/*
+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
index 5cb2859046c7d52f47618be49367fa1ca3306659..d2559e88e10207f83e5257b8ef6b9f993031d725 100644 (file)
@@ -1,3 +1,21 @@
+/*
+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 "ConstraintSolver/OdeConstraintSolver.h"
 #include "ConstraintSolver/SimpleConstraintSolver.h"
 
-#ifdef USE_PROFILE
+
 //profiling/timings
 #include "quickprof.h"
-#endif //USE_PROFILE
+
 
 #include "IDebugDraw.h"
 
@@ -36,7 +54,7 @@
 #include "CollisionDispatch/EmptyCollisionAlgorithm.h"
 #include "CollisionDispatch/UnionFind.h"
 
-#include "NarrowPhaseCollision/RaycastCallback.h"
+
 #include "CollisionShapes/SphereShape.h"
 
 bool useIslands = true;
@@ -77,7 +95,7 @@ class WrapperVehicle : public PHY_IVehicle
 
        RaycastVehicle* m_vehicle;
        PHY_IPhysicsController* m_chassis;
-               
+
 public:
 
        WrapperVehicle(RaycastVehicle* vehicle,PHY_IPhysicsController* chassis)
@@ -97,24 +115,24 @@ public:
        }
 
        virtual void    AddWheel(
-                       PHY_IMotionState*       motionState,
-                       PHY__Vector3    connectionPoint,
-                       PHY__Vector3    downDirection,
-                       PHY__Vector3    axleDirection,
-                       float   suspensionRestLength,
-                       float wheelRadius,
-                       bool hasSteering
+               PHY_IMotionState*       motionState,
+               PHY__Vector3    connectionPoint,
+               PHY__Vector3    downDirection,
+               PHY__Vector3    axleDirection,
+               float   suspensionRestLength,
+               float wheelRadius,
+               bool hasSteering
                )
        {
                SimdVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]);
                SimdVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]);
                SimdVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]);
-               
+
 
                WheelInfo& info = m_vehicle->AddWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle,
                        suspensionRestLength,wheelRadius,gTuning,hasSteering);
                info.m_clientInfo = motionState;
-               
+
        }
 
        void    SyncWheels()
@@ -139,7 +157,7 @@ public:
        {
                return m_vehicle->GetNumWheels();
        }
-       
+
        virtual void    GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
        {
                SimdTransform   trans = m_vehicle->GetWheelTransformWS(wheelIndex);
@@ -157,7 +175,7 @@ public:
                quatY = trans.getRotation().y();
                quatZ = trans.getRotation().z();
                quatW = trans.getRotation()[3];
-               
+
 
                //printf("test");
 
@@ -178,7 +196,7 @@ public:
        }
 
 
-       
+
        virtual int     GetUserConstraintId() const
        {
                return m_vehicle->GetUserConstraintId();
@@ -217,7 +235,7 @@ public:
                }
 
        }
-       
+
        virtual void    SetSuspensionStiffness(float suspensionStiffness,int wheelIndex)
        {
                if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
@@ -247,7 +265,7 @@ public:
        }
 
 
-       
+
        virtual void    SetRollInfluence(float rollInfluence,int wheelIndex)
        {
                if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
@@ -278,13 +296,13 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV
                        pa = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],              
                                edgecoord[2]*halfExtents[2]);
                        pa+=center;
-                       
+
                        int othercoord = j%3;
                        edgecoord[othercoord]*=-1.f;
                        pb = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],      
                                edgecoord[2]*halfExtents[2]);
                        pb+=center;
-                       
+
                        debugDrawer->DrawLine(pa,pb,color);
                }
                edgecoord = SimdVector3(-1.f,-1.f,-1.f);
@@ -316,7 +334,7 @@ m_enableSatCollisionDetection(false)
        if (!dispatcher)
                dispatcher = new CollisionDispatcher();
 
-       
+
        if(!broadphase)
        {
 
@@ -324,57 +342,57 @@ m_enableSatCollisionDetection(false)
                SimdVector3 worldMin(-10000,-10000,-10000);
                SimdVector3 worldMax(10000,10000,10000);
 
-               broadphase = new AxisSweep3(worldMin,worldMax);
+               //broadphase = new AxisSweep3(worldMin,worldMax);
 
-               //broadphase = new SimpleBroadphase();
+               broadphase = new SimpleBroadphase();
        }
-               
+
 
        setSolverType(1);
-       
+
        m_collisionWorld = new CollisionWorld(dispatcher,broadphase);
-       
+
        m_debugDrawer = 0;
        m_gravity = SimdVector3(0.f,-10.f,0.f);
-       
+
 
 }
 
 void   CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
 {
        RigidBody* body = ctrl->GetRigidBody();
-       
+
        //this m_userPointer is just used for triggers, see CallbackTriggers
        body->m_userPointer = ctrl;
 
        body->setGravity( m_gravity );
        m_controllers.push_back(ctrl);
-       
+
        m_collisionWorld->AddCollisionObject(body);
 
        assert(body->m_broadphaseHandle);
 
        BroadphaseInterface* scene =  GetBroadphase();
 
-       
+
        CollisionShape* shapeinterface = ctrl->GetCollisionShape();
-       
+
        assert(shapeinterface);
-       
+
        const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
-       
-       
+
+
        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();
@@ -394,43 +412,43 @@ void      CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
                maxAabbz += linMotion.z(); 
        else
                minAabbz += linMotion.z();
-       
+
 
        minAabb = SimdVector3(minAabbx,minAabby,minAabbz);
        maxAabb = SimdVector3(maxAabbx,maxAabby,maxAabbz);
-       
-       
-       
-       
+
+
+
+
 }
 
 void   CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
 {
-       
+
        //also remove constraint
-       
+
        {
                std::vector<TypedConstraint*>::iterator i;
-               
+
                for (i=m_constraints.begin();
-               !(i==m_constraints.end()); i++)
+                       !(i==m_constraints.end()); i++)
                {
                        TypedConstraint* constraint = (*i);
                        if  ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() ||
                                (&constraint->GetRigidBodyB() == ctrl->GetRigidBody())))
                        {
-                                removeConstraint(constraint->GetUserConstraintId());
+                               removeConstraint(constraint->GetUserConstraintId());
                                //only 1 constraint per constroller
                                break;
                        }
                }
        }
-       
+
        {
                std::vector<TypedConstraint*>::iterator i;
-               
+
                for (i=m_constraints.begin();
-               !(i==m_constraints.end()); i++)
+                       !(i==m_constraints.end()); i++)
                {
                        TypedConstraint* constraint = (*i);
                        if  ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() ||
@@ -442,11 +460,11 @@ void      CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr
                        }
                }
        }
-       
-       
+
+
        m_collisionWorld->RemoveCollisionObject(ctrl->GetRigidBody());
 
-       
+
        {
                std::vector<CcdPhysicsController*>::iterator i =
                        std::find(m_controllers.begin(), m_controllers.end(), ctrl);
@@ -467,7 +485,7 @@ void        CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr
                        m_triggerControllers.pop_back();
                }
        }
-       
+
 
 }
 
@@ -481,7 +499,7 @@ void        CcdPhysicsEnvironment::beginFrame()
 bool   CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 {
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        //toggle Profiler
        if ( m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_ProfileTimings)
        {
@@ -504,15 +522,15 @@ bool      CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
                        Profiler::destroy();
                }
        }
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
 
 
 
        if (!SimdFuzzyZero(timeStep))
        {
 
-// define this in blender, the stepsize is 30 hertz, 60 hertz works much better 
- #define SPLIT_TIMESTEP 1
+               // define this in blender, the stepsize is 30 hertz, 60 hertz works much better 
+#define SPLIT_TIMESTEP 1
 
 #ifdef SPLIT_TIMESTEP
                proceedDeltaTimeOneStep(0.5f*timeStep);
@@ -527,13 +545,26 @@ bool      CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 
        return true;
 }
+
+
+
+
+
+
+
+
+
+
+
+
+
 /// Perform an integration step of duration 'timeStep'.
 bool   CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 {
-       
-       
-//     printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
-       
+
+
+       //      printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
+
        if (SimdFuzzyZero(timeStep))
                return true;
 
@@ -543,11 +574,11 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
        }
 
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::beginBlock("SyncMotionStates");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
+
 
-       
        //this is needed because scaling is not known in advance, and scaling has to propagate to the shape
        if (!m_scalingPropagated)
        {
@@ -555,17 +586,17 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
                m_scalingPropagated = true;
        }
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::endBlock("SyncMotionStates");
 
        Profiler::beginBlock("predictIntegratedTransform");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
 
        {
-//             std::vector<CcdPhysicsController*>::iterator i;
-               
-               
-               
+               //              std::vector<CcdPhysicsController*>::iterator i;
+
+
+
                int k;
                for (k=0;k<GetNumControllers();k++)
                {
@@ -578,30 +609,30 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
                                body->integrateVelocities( timeStep);
                                body->predictIntegratedTransform(timeStep,body->m_nextPredictedWorldTransform);
                        }
-                       
+
                }
        }
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::endBlock("predictIntegratedTransform");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
 
        BroadphaseInterface*    scene = GetBroadphase();
-       
-       
+
+
        //
        // collision detection (?)
        //
-       
-       
-#ifdef USE_PROFILE
+
+
+#ifdef USE_QUICKPROF
        Profiler::beginBlock("DispatchAllCollisionPairs");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
+
 
-       
        int numsubstep = m_numIterations;
-       
-       
+
+
        DispatcherInfo dispatchInfo;
        dispatchInfo.m_timeStep = timeStep;
        dispatchInfo.m_stepCount = 0;
@@ -610,23 +641,23 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
        scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g);
 
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::endBlock("DispatchAllCollisionPairs");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
+
 
-               
        int numRigidBodies = m_controllers.size();
-       
+
        m_collisionWorld->UpdateActivationState();
 
-       
+
 
        //contacts
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::beginBlock("SolveConstraint");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
+
 
-       
        //solve the regular constraints (point 2 point, hinge, etc)
 
        for (int g=0;g<numsubstep;g++)
@@ -634,49 +665,49 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
                //
                // constraint solving
                //
-                               
-               
+
+
                int i;
                int numConstraints = m_constraints.size();
-               
+
                //point to point constraints
                for (i=0;i< numConstraints ; i++ )
                {
                        TypedConstraint* constraint = m_constraints[i];
-                       
+
                        constraint->BuildJacobian();
                        constraint->SolveConstraint( timeStep );
-                       
+
                }
-               
-               
+
+
        }
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::endBlock("SolveConstraint");
-#endif //USE_PROFILE
-       
+#endif //USE_QUICKPROF
+
        //solve the vehicles
 
-       #ifdef NEW_BULLET_VEHICLE_SUPPORT
-               //vehicles
-               int numVehicles = m_wrapperVehicles.size();
-               for (int i=0;i<numVehicles;i++)
-               {
-                       WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
-                       RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
-                       vehicle->UpdateVehicle( timeStep);
-               }
+#ifdef NEW_BULLET_VEHICLE_SUPPORT
+       //vehicles
+       int numVehicles = m_wrapperVehicles.size();
+       for (int i=0;i<numVehicles;i++)
+       {
+               WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
+               RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
+               vehicle->UpdateVehicle( timeStep);
+       }
 #endif //NEW_BULLET_VEHICLE_SUPPORT
 
-       
-               struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback
+
+       struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback
        {
 
                ContactSolverInfo& m_solverInfo;
                ConstraintSolver*       m_solver;
                IDebugDraw*     m_debugDrawer;
-               
+
                InplaceSolverIslandCallback(
                        ContactSolverInfo& solverInfo,
                        ConstraintSolver*       solver,
@@ -685,14 +716,14 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
                        m_solver(solver),
                        m_debugDrawer(debugDrawer)
                {
-                       
+
                }
-               
+
                virtual void    ProcessIsland(PersistentManifold**      manifolds,int numManifolds)
                {
                        m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
                }
-               
+
        };
 
 
@@ -702,141 +733,75 @@ bool     CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
        m_solverInfo.m_restitution = 0.f;//m_restitution;
 
        InplaceSolverIslandCallback     solverCallback(
-                       m_solverInfo,
-                       m_solver,
-                       m_debugDrawer);
+               m_solverInfo,
+               m_solver,
+               m_debugDrawer);
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::beginBlock("BuildAndProcessIslands");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
 
        /// solve all the contact points and contact friction
        GetDispatcher()->BuildAndProcessIslands(numRigidBodies,&solverCallback);
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::endBlock("BuildAndProcessIslands");
 
        Profiler::beginBlock("CallbackTriggers");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
 
        CallbackTriggers();
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::endBlock("CallbackTriggers");
 
 
        Profiler::beginBlock("proceedToTransform");
 
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
        {
-               
-               
-               
-               {
-                       
-                       std::vector<CcdPhysicsController*>::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);
-
-                               shapeinterface->GetAabb(body->getCenterOfMassTransform(),
-                                       minAabb,maxAabb);
-
-                               SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
-                               minAabb -= manifoldExtraExtents;
-                               maxAabb += manifoldExtraExtents;
-                               
-                               BroadphaseProxy* bp = body->m_broadphaseHandle;
-                               if (bp)
-                               {
-                                       
-#ifdef WIN32
-                                       SimdVector3 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() & IDebugDraw::DBG_DrawAabb)
-                                               {
-                                                       DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
-                                               }
-                                       }
-#endif
+               {
 
-                                       scene->SetAabb(bp,minAabb,maxAabb);
+                       
+                       
+                       UpdateAabbs(timeStep);
 
 
-                                       
-                               }
-                       }
-                       
                        float toi = 1.f;
 
 
-                       
+
                        if (m_ccdMode == 3)
                        {
                                DispatcherInfo dispatchInfo;
                                dispatchInfo.m_timeStep = timeStep;
                                dispatchInfo.m_stepCount = 0;
                                dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
-                               
+
                                scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g);
                                toi = dispatchInfo.m_timeOfImpact;
+
                        }
+
                        
+
                        //
                        // integrating solution
                        //
-                       
+
                        {
-                               std::vector<CcdPhysicsController*>::iterator i;
                                
+                               std::vector<CcdPhysicsController*>::iterator i;
+
                                for (i=m_controllers.begin();
-                               !(i==m_controllers.end()); i++)
+                                       !(i==m_controllers.end()); i++)
                                {
-                                       
+
                                        CcdPhysicsController* ctrl = *i;
-                                       
+
                                        SimdTransform predictedTrans;
                                        RigidBody* body = ctrl->GetRigidBody();
                                        if (body->GetActivationState() != ISLAND_SLEEPING)
@@ -844,31 +809,33 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 
                                                body->predictIntegratedTransform(timeStep*      toi, predictedTrans);
                                                body->proceedToTransform( predictedTrans);
-                       
+
                                        }
                                }
-                               
+
                        }
-                       
-                       
-                       
-                       
-                       
+
+
+
+
+
                        //
                        // disable sleeping physics objects
                        //
-                       
+
                        std::vector<CcdPhysicsController*> m_sleepingControllers;
-                       
+
+                       std::vector<CcdPhysicsController*>::iterator i;
+
                        for (i=m_controllers.begin();
-                       !(i==m_controllers.end()); i++)
+                               !(i==m_controllers.end()); i++)
                        {
                                CcdPhysicsController* ctrl = (*i);
                                RigidBody* body = ctrl->GetRigidBody();
 
                                ctrl->UpdateDeactivation(timeStep);
 
-                               
+
                                if (ctrl->wantsSleeping())
                                {
                                        if (body->GetActivationState() == ACTIVE_TAG)
@@ -893,35 +860,35 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
                                        }
                                }
                        }
-                       
-       
-                       
-                       
-       }
-       
 
-#ifdef USE_PROFILE
-       Profiler::endBlock("proceedToTransform");
 
-       Profiler::beginBlock("SyncMotionStates");
-#endif //USE_PROFILE
 
-       SyncMotionStates(timeStep);
 
-#ifdef USE_PROFILE
-       Profiler::endBlock("SyncMotionStates");
+               }
+
+
+#ifdef USE_QUICKPROF
+               Profiler::endBlock("proceedToTransform");
+
+               Profiler::beginBlock("SyncMotionStates");
+#endif //USE_QUICKPROF
+
+               SyncMotionStates(timeStep);
+
+#ifdef USE_QUICKPROF
+               Profiler::endBlock("SyncMotionStates");
+
+               Profiler::endProfilingCycle();
+#endif //USE_QUICKPROF
 
-       Profiler::endProfilingCycle();
-#endif //USE_PROFILE
 
-       
 #ifdef NEW_BULLET_VEHICLE_SUPPORT
                //sync wheels for vehicles
                int numVehicles = m_wrapperVehicles.size();
                for (int i=0;i<numVehicles;i++)
                {
                        WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
-               
+
                        wrapperVehicle->SyncWheels();
                }
 #endif //NEW_BULLET_VEHICLE_SUPPORT
@@ -955,7 +922,7 @@ void                CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh)
 void           CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
 {
        gContactBreakingTreshold = contactBreakingTreshold;
-       
+
 }
 
 
@@ -999,24 +966,24 @@ void              CcdPhysicsEnvironment::setSolverType(int solverType)
                {
                        if (m_solverType != solverType)
                        {
-                               
+
                                m_solver = new SimpleConstraintSolver();
-                               
+
                                break;
                        }
                }
-       
+
        case 0:
        default:
-                       if (m_solverType != solverType)
-                       {
-                               m_solver = new OdeConstraintSolver();
-               
-                               break;
-                       }
+               if (m_solverType != solverType)
+               {
+                       m_solver = new OdeConstraintSolver();
+
+                       break;
+               }
 
        };
-       
+
        m_solverType = solverType ;
 }
 
@@ -1033,11 +1000,11 @@ void    CcdPhysicsEnvironment::SyncMotionStates(float timeStep)
        //
 
        for (i=m_controllers.begin();
-       !(i==m_controllers.end()); i++)
+               !(i==m_controllers.end()); i++)
        {
                CcdPhysicsController* ctrl = (*i);
                ctrl->SynchronizeMotionStates(timeStep);
-               
+
        }
 
 }
@@ -1049,7 +1016,7 @@ void              CcdPhysicsEnvironment::setGravity(float x,float y,float z)
 
        //todo: review this gravity stuff
        for (i=m_controllers.begin();
-       !(i==m_controllers.end()); i++)
+               !(i==m_controllers.end()); i++)
        {
 
                CcdPhysicsController* ctrl = (*i);
@@ -1068,83 +1035,83 @@ class DefaultVehicleRaycaster : public VehicleRaycaster
 
 public:
        DefaultVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis):
-       m_physEnv(physEnv),
-               m_chassis(chassis)
-       {
-       }
-
-       /*      struct VehicleRaycasterResult
-       {
-               VehicleRaycasterResult() :m_distFraction(-1.f){};
-               SimdVector3     m_hitPointInWorld;
-               SimdVector3     m_hitNormalInWorld;
-               SimdScalar      m_distFraction;
-       };
-*/
-       virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& 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;
-       }
+         m_physEnv(physEnv),
+                 m_chassis(chassis)
+         {
+         }
+
+         /*    struct VehicleRaycasterResult
+         {
+         VehicleRaycasterResult() :m_distFraction(-1.f){};
+         SimdVector3   m_hitPointInWorld;
+         SimdVector3   m_hitNormalInWorld;
+         SimdScalar    m_distFraction;
+         };
+         */
+         virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& 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)
+                                                                                                       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;
        SimdVector3 axisInA(axisX,axisY,axisZ);
@@ -1152,15 +1119,15 @@ int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
                (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : 
        rb0->getCenterOfMassTransform().getBasis() * -axisInA;
 
-       bool hingeAngularOnly = false;
+       bool angularOnly = false;
 
        switch (type)
        {
        case PHY_POINT2POINT_CONSTRAINT:
                {
-                       
+
                        Point2PointConstraint* p2p = 0;
-                       
+
                        if (rb1)
                        {
                                p2p = new Point2PointConstraint(*rb0,
@@ -1170,22 +1137,23 @@ int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
                                p2p = new Point2PointConstraint(*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_ANGULAR_CONSTRAINT:
-                       hingeAngularOnly = true;
+               angularOnly = true;
+
        case PHY_LINEHINGE_CONSTRAINT:
                {
                        HingeConstraint* hinge = 0;
-                       
+
                        if (rb1)
                        {
                                hinge = new HingeConstraint(
@@ -1199,7 +1167,8 @@ int                       CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
                                        pivotInA,axisInA);
 
                        }
-                       hinge->setAngularOnly( hingeAngularOnly );
+                       hinge->setAngularOnly(angularOnly);
+
                        m_constraints.push_back(hinge);
                        hinge->SetUserConstraintId(gConstraintUid++);
                        hinge->SetUserConstraintType(type);
@@ -1229,152 +1198,86 @@ int                    CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
                {
                }
        };
-       
+
        //RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB
-       
+
        return 0;
-       
+
 }
 
 void           CcdPhysicsEnvironment::removeConstraint(int     constraintId)
 {
        std::vector<TypedConstraint*>::iterator i;
-       
-               for (i=m_constraints.begin();
+
+       for (i=m_constraints.begin();
                !(i==m_constraints.end()); i++)
+       {
+               TypedConstraint* constraint = (*i);
+               if (constraint->GetUserConstraintId() == constraintId)
                {
-                       TypedConstraint* constraint = (*i);
-                       if (constraint->GetUserConstraintId() == constraintId)
-                       {
-                               std::swap(*i, m_constraints.back());
-                               m_constraints.pop_back();
-                               break;
-                       }
+                       std::swap(*i, m_constraints.back());
+                       m_constraints.pop_back();
+                       break;
                }
-       
+       }
+
 }
 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)
+                                                                                                          float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
 {
 
-       float minFraction = 1.f;
 
-       SimdTransform   rayFromTrans,rayToTrans;
-       rayFromTrans.setIdentity();
+       float minFraction = 1.f;
 
        SimdVector3 rayFrom(fromX,fromY,fromZ);
-
-       rayFromTrans.setOrigin(rayFrom);
-       rayToTrans.setIdentity();
        SimdVector3 rayTo(toX,toY,toZ);
-       rayToTrans.setOrigin(rayTo);
 
-       //do culling based on aabb (rayFrom/rayTo)
-       SimdVector3 rayAabbMin = rayFrom;
-       SimdVector3 rayAabbMax = rayFrom;
-       rayAabbMin.setMin(rayTo);
-       rayAabbMax.setMax(rayTo);
+       SimdVector3     hitPointWorld,normalWorld;
 
-       
-       CcdPhysicsController* nearestHit = 0;
-       
-       std::vector<CcdPhysicsController*>::iterator i;
-       SphereShape pointShape(0.0f);
+       CollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
 
-       /// brute force go over all objects. Once there is a broadphase, use that, or
-       /// add a raycast against aabb first.
-       for (i=m_controllers.begin();
-       !(i==m_controllers.end()); i++)
-       {
-               CcdPhysicsController* ctrl = (*i);
-               if (ctrl == ignoreClient)
-                       continue;
-               RigidBody* body = ctrl->GetRigidBody();
-               SimdVector3 bodyAabbMin,bodyAabbMax;
-               body->getAabb(bodyAabbMin,bodyAabbMax);
 
-               //check aabb overlap
+       struct  FilterClosestRayResultCallback : CollisionWorld::ClosestRayResultCallback
+       {
+               PHY_IPhysicsController* m_ignoreClient;
 
-               if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,bodyAabbMin,bodyAabbMax))
+               FilterClosestRayResultCallback (PHY_IPhysicsController* ignoreClient,const SimdVector3& rayFrom,const SimdVector3& rayTo)
+                       : CollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
+                       m_ignoreClient(ignoreClient)
                {
-                       if (body->GetCollisionShape()->IsConvex())
-                       {
-                               ConvexCast::CastResult rayResult;
-                               rayResult.m_fraction = 1.f;
-
-                               ConvexShape* convexShape = (ConvexShape*) body->GetCollisionShape();
-                               VoronoiSimplexSolver    simplexSolver;
-                               SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
-                               //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
-                               //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
-                               
-                               if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,body->getCenterOfMassTransform(),body->getCenterOfMassTransform(),rayResult))
-                               {
-                                       //add hit
-                                       if (rayResult.m_normal.length2() > 0.0001f)
-                                       {
-                                               rayResult.m_normal.normalize();
-                                               if (rayResult.m_fraction < minFraction)
-                                               {
 
+               }
 
-                                                       minFraction = rayResult.m_fraction;
-                                                       nearestHit = ctrl;
-                                                       normalX = rayResult.m_normal.getX();
-                                                       normalY = rayResult.m_normal.getY();
-                                                       normalZ = rayResult.m_normal.getZ();
-                                                       SimdVector3 hitWorld;
-                                                       hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rayResult.m_fraction);
-                                                       hitX = hitWorld.getX();
-                                                       hitY = hitWorld.getY();
-                                                       hitZ = hitWorld.getZ();
-
-                                               }
-                                       }
-                               }
+               virtual float   AddSingleResult(const CollisionWorld::LocalRayResult& rayResult)
+               {
+                       CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->m_userPointer);
+                       //ignore client...
+                       if (curHit != m_ignoreClient)
+                       {               
+                               //if valid
+                               return CollisionWorld::ClosestRayResultCallback::AddSingleResult(rayResult);
                        }
-                       else
-                       {
-                                       if (body->GetCollisionShape()->IsConcave())
-                                       {
+               }
 
-                                               TriangleMeshShape* triangleMesh = (TriangleMeshShape*)body->GetCollisionShape();
-                                               
-                                               SimdTransform worldToBody = body->getCenterOfMassTransform().inverse();
+       };
 
-                                               SimdVector3 rayFromLocal = worldToBody * rayFromTrans.getOrigin();
-                                               SimdVector3 rayToLocal = worldToBody * rayToTrans.getOrigin();
 
-                                               RaycastCallback rcb(rayFromLocal,rayToLocal);
-                                               rcb.m_hitFraction = minFraction;
+       PHY_IPhysicsController* nearestHit = 0;
 
-                                               SimdVector3 rayAabbMinLocal = rayFromLocal;
-                                               rayAabbMinLocal.setMin(rayToLocal);
-                                               SimdVector3 rayAabbMaxLocal = rayFromLocal;
-                                               rayAabbMaxLocal.setMax(rayToLocal);
+       m_collisionWorld->RayTest(rayFrom,rayTo,rayCallback);
+       if (rayCallback.HasHit())
+       {
+               nearestHit = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->m_userPointer);
+               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();
+
+       }       
 
-                                               triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
-                                               if (rcb.m_hitFound)
-                                               {
-                                                       nearestHit = ctrl;
-                                                       minFraction = rcb.m_hitFraction;
-                                                       SimdVector3 hitNormalWorld = body->getCenterOfMassTransform().getBasis()*rcb.m_hitNormalLocal;
-                                                       hitNormalWorld.normalize();
-
-                                                       normalX = hitNormalWorld.getX();
-                                                       normalY = hitNormalWorld.getY();
-                                                       normalZ = hitNormalWorld.getZ();
-                                                       SimdVector3 hitWorld;
-                                                       hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rcb.m_hitFraction);
-                                                       hitX = hitWorld.getX();
-                                                       hitY = hitWorld.getY();
-                                                       hitZ = hitWorld.getZ();
-                                                       
-                                               }
-                                       }
-                       }
-               }
-       }
 
        return nearestHit;
 }
@@ -1388,7 +1291,7 @@ int       CcdPhysicsEnvironment::getNumContactPoints()
 
 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
 {
-       
+
 }
 
 
@@ -1413,18 +1316,18 @@ CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher()
 
 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;
-       
+
 }
 
 
@@ -1476,31 +1379,31 @@ void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
 }
 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
 {
-/*     printf("addTouchCallback\n(response class = %i)\n",response_class);
+       /*      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;
+       printf("PHY_FH_RESPONSE\n");
+       break;
        case PHY_SENSOR_RESPONSE:
-               printf("PHY_SENSOR_RESPONSE\n");
-               break;
+       printf("PHY_SENSOR_RESPONSE\n");
+       break;
        case PHY_CAMERA_RESPONSE:
-               printf("PHY_CAMERA_RESPONSE\n");
-               break;
+       printf("PHY_CAMERA_RESPONSE\n");
+       break;
        case PHY_OBJECT_RESPONSE:
-               printf("PHY_OBJECT_RESPONSE\n");
-               break;
+       printf("PHY_OBJECT_RESPONSE\n");
+       break;
        case PHY_STATIC_RESPONSE:
-               printf("PHY_STATIC_RESPONSE\n");
-               break;
+       printf("PHY_STATIC_RESPONSE\n");
+       break;
        default:
-               assert(0);
-               return;
+       assert(0);
+       return;
        }
-*/
+       */
 
        m_triggerCallbacks[response_class] = callback;
        m_triggerCallbacksUserPtrs[response_class] = user;
@@ -1531,13 +1434,13 @@ void    CcdPhysicsEnvironment::CallbackTriggers()
                        {
                                RigidBody* obj0 = static_cast<RigidBody* >(manifold->GetBody0());
                                RigidBody* obj1 = static_cast<RigidBody* >(manifold->GetBody1());
-                               
+
                                //m_userPointer is set in 'addPhysicsController
                                CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->m_userPointer);
                                CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->m_userPointer);
 
                                std::vector<CcdPhysicsController*>::iterator i =
-                               std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
+                                       std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
                                if (i == m_triggerControllers.end())
                                {
                                        i = std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl1);
@@ -1551,7 +1454,7 @@ void      CcdPhysicsEnvironment::CallbackTriggers()
                        }
                }
 
-               
+
 
        }
 
@@ -1582,3 +1485,81 @@ PHY_IVehicle*    CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
 
 #endif //NEW_BULLET_VEHICLE_SUPPORT
 
+
+
+void   CcdPhysicsEnvironment::UpdateAabbs(float        timeStep)
+{
+       std::vector<CcdPhysicsController*>::iterator i;
+       BroadphaseInterface* scene =  GetBroadphase();
+
+       //
+                       // 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);
+
+
+                               SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
+                               minAabb -= manifoldExtraExtents;
+                               maxAabb += manifoldExtraExtents;
+
+                               BroadphaseProxy* bp = body->m_broadphaseHandle;
+                               if (bp)
+                               {
+
+#ifdef WIN32
+                                       SimdVector3 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() & IDebugDraw::DBG_DrawAabb)
+                                               {
+                                                       DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
+                                               }
+                                       }
+#endif
+
+                                       scene->SetAabb(bp,minAabb,maxAabb);
+
+
+
+                               }
+                       }
+}
\ No newline at end of file
index 2a4396d8c7892fd4536371be21599d37f1fd33af..08dff62e236e5e90a87a0e97d359f6179a9206a4 100644 (file)
@@ -1,3 +1,18 @@
+/*
+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
 
@@ -24,6 +39,7 @@ class PersistentManifold;
 class BroadphaseInterface;
 class IDebugDraw;
 
+/// CcdPhysicsEnvironment is 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
@@ -147,6 +163,8 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
                        m_enableSatCollisionDetection = enableSat;
                }
 
+               void    UpdateAabbs(float       timeStep);
+
                int     GetNumControllers();
 
                CcdPhysicsController* GetPhysicsController( int index);
index 8e60fdb126ee75a02d172d116c9b71aac19b81c0..79cd57756b0c55f2d4f1aad407a8cc3877246cd8 100644 (file)
@@ -79,8 +79,10 @@ typedef enum PHY_PhysicsType {
 /// PHY_ConstraintType enumerates all supported Constraint Types
 typedef enum PHY_ConstraintType {
        PHY_POINT2POINT_CONSTRAINT=1,
-       PHY_LINEHINGE_CONSTRAINT,
-       PHY_VEHICLE_CONSTRAINT
+       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_ConstraintType;
 
index 5ef4d8d126bdbcaafeaff43aea5bcdd290f86bd7..be6c3fd4fbb5e6cf56518b21686916de942921c7 100644 (file)
@@ -48,6 +48,7 @@ class IDebugDraw
                DBG_ProfileTimings = 128,
                DBG_EnableSatComparison = 256,
                DBG_DisableBulletLCP = 512,
+               DBG_EnableCCD = 1024,
                DBG_MAX_DEBUG_DRAW_MODE
        };
 
index 68010b851807a144e2f49c0bff2ad25115f1e6d4..79b311bd691f69fea0395e6ce2c6af20cf6d7434 100644 (file)
@@ -1,3 +1,18 @@
+/*
+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 "Dynamics/RigidBody.h"
index e670755b1f2667bb6840804a4aac48ae3c31d442..f1933d5513764c8c055c30fe1ab16da0e5963453 100644 (file)
@@ -1,3 +1,18 @@
+/*
+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
index 5cb2859046c7d52f47618be49367fa1ca3306659..d2559e88e10207f83e5257b8ef6b9f993031d725 100644 (file)
@@ -1,3 +1,21 @@
+/*
+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 "ConstraintSolver/OdeConstraintSolver.h"
 #include "ConstraintSolver/SimpleConstraintSolver.h"
 
-#ifdef USE_PROFILE
+
 //profiling/timings
 #include "quickprof.h"
-#endif //USE_PROFILE
+
 
 #include "IDebugDraw.h"
 
@@ -36,7 +54,7 @@
 #include "CollisionDispatch/EmptyCollisionAlgorithm.h"
 #include "CollisionDispatch/UnionFind.h"
 
-#include "NarrowPhaseCollision/RaycastCallback.h"
+
 #include "CollisionShapes/SphereShape.h"
 
 bool useIslands = true;
@@ -77,7 +95,7 @@ class WrapperVehicle : public PHY_IVehicle
 
        RaycastVehicle* m_vehicle;
        PHY_IPhysicsController* m_chassis;
-               
+
 public:
 
        WrapperVehicle(RaycastVehicle* vehicle,PHY_IPhysicsController* chassis)
@@ -97,24 +115,24 @@ public:
        }
 
        virtual void    AddWheel(
-                       PHY_IMotionState*       motionState,
-                       PHY__Vector3    connectionPoint,
-                       PHY__Vector3    downDirection,
-                       PHY__Vector3    axleDirection,
-                       float   suspensionRestLength,
-                       float wheelRadius,
-                       bool hasSteering
+               PHY_IMotionState*       motionState,
+               PHY__Vector3    connectionPoint,
+               PHY__Vector3    downDirection,
+               PHY__Vector3    axleDirection,
+               float   suspensionRestLength,
+               float wheelRadius,
+               bool hasSteering
                )
        {
                SimdVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]);
                SimdVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]);
                SimdVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]);
-               
+
 
                WheelInfo& info = m_vehicle->AddWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle,
                        suspensionRestLength,wheelRadius,gTuning,hasSteering);
                info.m_clientInfo = motionState;
-               
+
        }
 
        void    SyncWheels()
@@ -139,7 +157,7 @@ public:
        {
                return m_vehicle->GetNumWheels();
        }
-       
+
        virtual void    GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
        {
                SimdTransform   trans = m_vehicle->GetWheelTransformWS(wheelIndex);
@@ -157,7 +175,7 @@ public:
                quatY = trans.getRotation().y();
                quatZ = trans.getRotation().z();
                quatW = trans.getRotation()[3];
-               
+
 
                //printf("test");
 
@@ -178,7 +196,7 @@ public:
        }
 
 
-       
+
        virtual int     GetUserConstraintId() const
        {
                return m_vehicle->GetUserConstraintId();
@@ -217,7 +235,7 @@ public:
                }
 
        }
-       
+
        virtual void    SetSuspensionStiffness(float suspensionStiffness,int wheelIndex)
        {
                if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
@@ -247,7 +265,7 @@ public:
        }
 
 
-       
+
        virtual void    SetRollInfluence(float rollInfluence,int wheelIndex)
        {
                if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
@@ -278,13 +296,13 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV
                        pa = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],              
                                edgecoord[2]*halfExtents[2]);
                        pa+=center;
-                       
+
                        int othercoord = j%3;
                        edgecoord[othercoord]*=-1.f;
                        pb = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],      
                                edgecoord[2]*halfExtents[2]);
                        pb+=center;
-                       
+
                        debugDrawer->DrawLine(pa,pb,color);
                }
                edgecoord = SimdVector3(-1.f,-1.f,-1.f);
@@ -316,7 +334,7 @@ m_enableSatCollisionDetection(false)
        if (!dispatcher)
                dispatcher = new CollisionDispatcher();
 
-       
+
        if(!broadphase)
        {
 
@@ -324,57 +342,57 @@ m_enableSatCollisionDetection(false)
                SimdVector3 worldMin(-10000,-10000,-10000);
                SimdVector3 worldMax(10000,10000,10000);
 
-               broadphase = new AxisSweep3(worldMin,worldMax);
+               //broadphase = new AxisSweep3(worldMin,worldMax);
 
-               //broadphase = new SimpleBroadphase();
+               broadphase = new SimpleBroadphase();
        }
-               
+
 
        setSolverType(1);
-       
+
        m_collisionWorld = new CollisionWorld(dispatcher,broadphase);
-       
+
        m_debugDrawer = 0;
        m_gravity = SimdVector3(0.f,-10.f,0.f);
-       
+
 
 }
 
 void   CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
 {
        RigidBody* body = ctrl->GetRigidBody();
-       
+
        //this m_userPointer is just used for triggers, see CallbackTriggers
        body->m_userPointer = ctrl;
 
        body->setGravity( m_gravity );
        m_controllers.push_back(ctrl);
-       
+
        m_collisionWorld->AddCollisionObject(body);
 
        assert(body->m_broadphaseHandle);
 
        BroadphaseInterface* scene =  GetBroadphase();
 
-       
+
        CollisionShape* shapeinterface = ctrl->GetCollisionShape();
-       
+
        assert(shapeinterface);
-       
+
        const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
-       
-       
+
+
        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();
@@ -394,43 +412,43 @@ void      CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
                maxAabbz += linMotion.z(); 
        else
                minAabbz += linMotion.z();
-       
+
 
        minAabb = SimdVector3(minAabbx,minAabby,minAabbz);
        maxAabb = SimdVector3(maxAabbx,maxAabby,maxAabbz);
-       
-       
-       
-       
+
+
+
+
 }
 
 void   CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
 {
-       
+
        //also remove constraint
-       
+
        {
                std::vector<TypedConstraint*>::iterator i;
-               
+
                for (i=m_constraints.begin();
-               !(i==m_constraints.end()); i++)
+                       !(i==m_constraints.end()); i++)
                {
                        TypedConstraint* constraint = (*i);
                        if  ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() ||
                                (&constraint->GetRigidBodyB() == ctrl->GetRigidBody())))
                        {
-                                removeConstraint(constraint->GetUserConstraintId());
+                               removeConstraint(constraint->GetUserConstraintId());
                                //only 1 constraint per constroller
                                break;
                        }
                }
        }
-       
+
        {
                std::vector<TypedConstraint*>::iterator i;
-               
+
                for (i=m_constraints.begin();
-               !(i==m_constraints.end()); i++)
+                       !(i==m_constraints.end()); i++)
                {
                        TypedConstraint* constraint = (*i);
                        if  ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() ||
@@ -442,11 +460,11 @@ void      CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr
                        }
                }
        }
-       
-       
+
+
        m_collisionWorld->RemoveCollisionObject(ctrl->GetRigidBody());
 
-       
+
        {
                std::vector<CcdPhysicsController*>::iterator i =
                        std::find(m_controllers.begin(), m_controllers.end(), ctrl);
@@ -467,7 +485,7 @@ void        CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr
                        m_triggerControllers.pop_back();
                }
        }
-       
+
 
 }
 
@@ -481,7 +499,7 @@ void        CcdPhysicsEnvironment::beginFrame()
 bool   CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 {
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        //toggle Profiler
        if ( m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_ProfileTimings)
        {
@@ -504,15 +522,15 @@ bool      CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
                        Profiler::destroy();
                }
        }
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
 
 
 
        if (!SimdFuzzyZero(timeStep))
        {
 
-// define this in blender, the stepsize is 30 hertz, 60 hertz works much better 
- #define SPLIT_TIMESTEP 1
+               // define this in blender, the stepsize is 30 hertz, 60 hertz works much better 
+#define SPLIT_TIMESTEP 1
 
 #ifdef SPLIT_TIMESTEP
                proceedDeltaTimeOneStep(0.5f*timeStep);
@@ -527,13 +545,26 @@ bool      CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 
        return true;
 }
+
+
+
+
+
+
+
+
+
+
+
+
+
 /// Perform an integration step of duration 'timeStep'.
 bool   CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 {
-       
-       
-//     printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
-       
+
+
+       //      printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
+
        if (SimdFuzzyZero(timeStep))
                return true;
 
@@ -543,11 +574,11 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
        }
 
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::beginBlock("SyncMotionStates");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
+
 
-       
        //this is needed because scaling is not known in advance, and scaling has to propagate to the shape
        if (!m_scalingPropagated)
        {
@@ -555,17 +586,17 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
                m_scalingPropagated = true;
        }
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::endBlock("SyncMotionStates");
 
        Profiler::beginBlock("predictIntegratedTransform");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
 
        {
-//             std::vector<CcdPhysicsController*>::iterator i;
-               
-               
-               
+               //              std::vector<CcdPhysicsController*>::iterator i;
+
+
+
                int k;
                for (k=0;k<GetNumControllers();k++)
                {
@@ -578,30 +609,30 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
                                body->integrateVelocities( timeStep);
                                body->predictIntegratedTransform(timeStep,body->m_nextPredictedWorldTransform);
                        }
-                       
+
                }
        }
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::endBlock("predictIntegratedTransform");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
 
        BroadphaseInterface*    scene = GetBroadphase();
-       
-       
+
+
        //
        // collision detection (?)
        //
-       
-       
-#ifdef USE_PROFILE
+
+
+#ifdef USE_QUICKPROF
        Profiler::beginBlock("DispatchAllCollisionPairs");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
+
 
-       
        int numsubstep = m_numIterations;
-       
-       
+
+
        DispatcherInfo dispatchInfo;
        dispatchInfo.m_timeStep = timeStep;
        dispatchInfo.m_stepCount = 0;
@@ -610,23 +641,23 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
        scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g);
 
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::endBlock("DispatchAllCollisionPairs");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
+
 
-               
        int numRigidBodies = m_controllers.size();
-       
+
        m_collisionWorld->UpdateActivationState();
 
-       
+
 
        //contacts
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::beginBlock("SolveConstraint");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
+
 
-       
        //solve the regular constraints (point 2 point, hinge, etc)
 
        for (int g=0;g<numsubstep;g++)
@@ -634,49 +665,49 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
                //
                // constraint solving
                //
-                               
-               
+
+
                int i;
                int numConstraints = m_constraints.size();
-               
+
                //point to point constraints
                for (i=0;i< numConstraints ; i++ )
                {
                        TypedConstraint* constraint = m_constraints[i];
-                       
+
                        constraint->BuildJacobian();
                        constraint->SolveConstraint( timeStep );
-                       
+
                }
-               
-               
+
+
        }
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::endBlock("SolveConstraint");
-#endif //USE_PROFILE
-       
+#endif //USE_QUICKPROF
+
        //solve the vehicles
 
-       #ifdef NEW_BULLET_VEHICLE_SUPPORT
-               //vehicles
-               int numVehicles = m_wrapperVehicles.size();
-               for (int i=0;i<numVehicles;i++)
-               {
-                       WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
-                       RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
-                       vehicle->UpdateVehicle( timeStep);
-               }
+#ifdef NEW_BULLET_VEHICLE_SUPPORT
+       //vehicles
+       int numVehicles = m_wrapperVehicles.size();
+       for (int i=0;i<numVehicles;i++)
+       {
+               WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
+               RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
+               vehicle->UpdateVehicle( timeStep);
+       }
 #endif //NEW_BULLET_VEHICLE_SUPPORT
 
-       
-               struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback
+
+       struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback
        {
 
                ContactSolverInfo& m_solverInfo;
                ConstraintSolver*       m_solver;
                IDebugDraw*     m_debugDrawer;
-               
+
                InplaceSolverIslandCallback(
                        ContactSolverInfo& solverInfo,
                        ConstraintSolver*       solver,
@@ -685,14 +716,14 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
                        m_solver(solver),
                        m_debugDrawer(debugDrawer)
                {
-                       
+
                }
-               
+
                virtual void    ProcessIsland(PersistentManifold**      manifolds,int numManifolds)
                {
                        m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
                }
-               
+
        };
 
 
@@ -702,141 +733,75 @@ bool     CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
        m_solverInfo.m_restitution = 0.f;//m_restitution;
 
        InplaceSolverIslandCallback     solverCallback(
-                       m_solverInfo,
-                       m_solver,
-                       m_debugDrawer);
+               m_solverInfo,
+               m_solver,
+               m_debugDrawer);
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::beginBlock("BuildAndProcessIslands");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
 
        /// solve all the contact points and contact friction
        GetDispatcher()->BuildAndProcessIslands(numRigidBodies,&solverCallback);
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::endBlock("BuildAndProcessIslands");
 
        Profiler::beginBlock("CallbackTriggers");
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
 
        CallbackTriggers();
 
-#ifdef USE_PROFILE
+#ifdef USE_QUICKPROF
        Profiler::endBlock("CallbackTriggers");
 
 
        Profiler::beginBlock("proceedToTransform");
 
-#endif //USE_PROFILE
+#endif //USE_QUICKPROF
        {
-               
-               
-               
-               {
-                       
-                       std::vector<CcdPhysicsController*>::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);
-
-                               shapeinterface->GetAabb(body->getCenterOfMassTransform(),
-                                       minAabb,maxAabb);
-
-                               SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
-                               minAabb -= manifoldExtraExtents;
-                               maxAabb += manifoldExtraExtents;
-                               
-                               BroadphaseProxy* bp = body->m_broadphaseHandle;
-                               if (bp)
-                               {
-                                       
-#ifdef WIN32
-                                       SimdVector3 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() & IDebugDraw::DBG_DrawAabb)
-                                               {
-                                                       DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
-                                               }
-                                       }
-#endif
+               {
 
-                                       scene->SetAabb(bp,minAabb,maxAabb);
+                       
+                       
+                       UpdateAabbs(timeStep);
 
 
-                                       
-                               }
-                       }
-                       
                        float toi = 1.f;
 
 
-                       
+
                        if (m_ccdMode == 3)
                        {
                                DispatcherInfo dispatchInfo;
                                dispatchInfo.m_timeStep = timeStep;
                                dispatchInfo.m_stepCount = 0;
                                dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
-                               
+
                                scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g);
                                toi = dispatchInfo.m_timeOfImpact;
+
                        }
+
                        
+
                        //
                        // integrating solution
                        //
-                       
+
                        {
-                               std::vector<CcdPhysicsController*>::iterator i;
                                
+                               std::vector<CcdPhysicsController*>::iterator i;
+
                                for (i=m_controllers.begin();
-                               !(i==m_controllers.end()); i++)
+                                       !(i==m_controllers.end()); i++)
                                {
-                                       
+
                                        CcdPhysicsController* ctrl = *i;
-                                       
+
                                        SimdTransform predictedTrans;
                                        RigidBody* body = ctrl->GetRigidBody();
                                        if (body->GetActivationState() != ISLAND_SLEEPING)
@@ -844,31 +809,33 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 
                                                body->predictIntegratedTransform(timeStep*      toi, predictedTrans);
                                                body->proceedToTransform( predictedTrans);
-                       
+
                                        }
                                }
-                               
+
                        }
-                       
-                       
-                       
-                       
-                       
+
+
+
+
+
                        //
                        // disable sleeping physics objects
                        //
-                       
+
                        std::vector<CcdPhysicsController*> m_sleepingControllers;
-                       
+
+                       std::vector<CcdPhysicsController*>::iterator i;
+
                        for (i=m_controllers.begin();
-                       !(i==m_controllers.end()); i++)
+                               !(i==m_controllers.end()); i++)
                        {
                                CcdPhysicsController* ctrl = (*i);
                                RigidBody* body = ctrl->GetRigidBody();
 
                                ctrl->UpdateDeactivation(timeStep);
 
-                               
+
                                if (ctrl->wantsSleeping())
                                {
                                        if (body->GetActivationState() == ACTIVE_TAG)
@@ -893,35 +860,35 @@ bool      CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
                                        }
                                }
                        }
-                       
-       
-                       
-                       
-       }
-       
 
-#ifdef USE_PROFILE
-       Profiler::endBlock("proceedToTransform");
 
-       Profiler::beginBlock("SyncMotionStates");
-#endif //USE_PROFILE
 
-       SyncMotionStates(timeStep);
 
-#ifdef USE_PROFILE
-       Profiler::endBlock("SyncMotionStates");
+               }
+
+
+#ifdef USE_QUICKPROF
+               Profiler::endBlock("proceedToTransform");
+
+               Profiler::beginBlock("SyncMotionStates");
+#endif //USE_QUICKPROF
+
+               SyncMotionStates(timeStep);
+
+#ifdef USE_QUICKPROF
+               Profiler::endBlock("SyncMotionStates");
+
+               Profiler::endProfilingCycle();
+#endif //USE_QUICKPROF
 
-       Profiler::endProfilingCycle();
-#endif //USE_PROFILE
 
-       
 #ifdef NEW_BULLET_VEHICLE_SUPPORT
                //sync wheels for vehicles
                int numVehicles = m_wrapperVehicles.size();
                for (int i=0;i<numVehicles;i++)
                {
                        WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
-               
+
                        wrapperVehicle->SyncWheels();
                }
 #endif //NEW_BULLET_VEHICLE_SUPPORT
@@ -955,7 +922,7 @@ void                CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh)
 void           CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
 {
        gContactBreakingTreshold = contactBreakingTreshold;
-       
+
 }
 
 
@@ -999,24 +966,24 @@ void              CcdPhysicsEnvironment::setSolverType(int solverType)
                {
                        if (m_solverType != solverType)
                        {
-                               
+
                                m_solver = new SimpleConstraintSolver();
-                               
+
                                break;
                        }
                }
-       
+
        case 0:
        default:
-                       if (m_solverType != solverType)
-                       {
-                               m_solver = new OdeConstraintSolver();
-               
-                               break;
-                       }
+               if (m_solverType != solverType)
+               {
+                       m_solver = new OdeConstraintSolver();
+
+                       break;
+               }
 
        };
-       
+
        m_solverType = solverType ;
 }
 
@@ -1033,11 +1000,11 @@ void    CcdPhysicsEnvironment::SyncMotionStates(float timeStep)
        //
 
        for (i=m_controllers.begin();
-       !(i==m_controllers.end()); i++)
+               !(i==m_controllers.end()); i++)
        {
                CcdPhysicsController* ctrl = (*i);
                ctrl->SynchronizeMotionStates(timeStep);
-               
+
        }
 
 }
@@ -1049,7 +1016,7 @@ void              CcdPhysicsEnvironment::setGravity(float x,float y,float z)
 
        //todo: review this gravity stuff
        for (i=m_controllers.begin();
-       !(i==m_controllers.end()); i++)
+               !(i==m_controllers.end()); i++)
        {
 
                CcdPhysicsController* ctrl = (*i);
@@ -1068,83 +1035,83 @@ class DefaultVehicleRaycaster : public VehicleRaycaster
 
 public:
        DefaultVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis):
-       m_physEnv(physEnv),
-               m_chassis(chassis)
-       {
-       }
-
-       /*      struct VehicleRaycasterResult
-       {
-               VehicleRaycasterResult() :m_distFraction(-1.f){};
-               SimdVector3     m_hitPointInWorld;
-               SimdVector3     m_hitNormalInWorld;
-               SimdScalar      m_distFraction;
-       };
-*/
-       virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& 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;
-       }
+         m_physEnv(physEnv),
+                 m_chassis(chassis)
+         {
+         }
+
+         /*    struct VehicleRaycasterResult
+         {
+         VehicleRaycasterResult() :m_distFraction(-1.f){};
+         SimdVector3   m_hitPointInWorld;
+         SimdVector3   m_hitNormalInWorld;
+         SimdScalar    m_distFraction;
+         };
+         */
+         virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& 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)
+                                                                                                       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;
        SimdVector3 axisInA(axisX,axisY,axisZ);
@@ -1152,15 +1119,15 @@ int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
                (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : 
        rb0->getCenterOfMassTransform().getBasis() * -axisInA;
 
-       bool hingeAngularOnly = false;
+       bool angularOnly = false;
 
        switch (type)
        {
        case PHY_POINT2POINT_CONSTRAINT:
                {
-                       
+
                        Point2PointConstraint* p2p = 0;
-                       
+
                        if (rb1)
                        {
                                p2p = new Point2PointConstraint(*rb0,
@@ -1170,22 +1137,23 @@ int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
                                p2p = new Point2PointConstraint(*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_ANGULAR_CONSTRAINT:
-                       hingeAngularOnly = true;
+               angularOnly = true;
+
        case PHY_LINEHINGE_CONSTRAINT:
                {
                        HingeConstraint* hinge = 0;
-                       
+
                        if (rb1)
                        {
                                hinge = new HingeConstraint(
@@ -1199,7 +1167,8 @@ int                       CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
                                        pivotInA,axisInA);
 
                        }
-                       hinge->setAngularOnly( hingeAngularOnly );
+                       hinge->setAngularOnly(angularOnly);
+
                        m_constraints.push_back(hinge);
                        hinge->SetUserConstraintId(gConstraintUid++);
                        hinge->SetUserConstraintType(type);
@@ -1229,152 +1198,86 @@ int                    CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
                {
                }
        };
-       
+
        //RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB
-       
+
        return 0;
-       
+
 }
 
 void           CcdPhysicsEnvironment::removeConstraint(int     constraintId)
 {
        std::vector<TypedConstraint*>::iterator i;
-       
-               for (i=m_constraints.begin();
+
+       for (i=m_constraints.begin();
                !(i==m_constraints.end()); i++)
+       {
+               TypedConstraint* constraint = (*i);
+               if (constraint->GetUserConstraintId() == constraintId)
                {
-                       TypedConstraint* constraint = (*i);
-                       if (constraint->GetUserConstraintId() == constraintId)
-                       {
-                               std::swap(*i, m_constraints.back());
-                               m_constraints.pop_back();
-                               break;
-                       }
+                       std::swap(*i, m_constraints.back());
+                       m_constraints.pop_back();
+                       break;
                }
-       
+       }
+
 }
 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)
+                                                                                                          float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
 {
 
-       float minFraction = 1.f;
 
-       SimdTransform   rayFromTrans,rayToTrans;
-       rayFromTrans.setIdentity();
+       float minFraction = 1.f;
 
        SimdVector3 rayFrom(fromX,fromY,fromZ);
-
-       rayFromTrans.setOrigin(rayFrom);
-       rayToTrans.setIdentity();
        SimdVector3 rayTo(toX,toY,toZ);
-       rayToTrans.setOrigin(rayTo);
 
-       //do culling based on aabb (rayFrom/rayTo)
-       SimdVector3 rayAabbMin = rayFrom;
-       SimdVector3 rayAabbMax = rayFrom;
-       rayAabbMin.setMin(rayTo);
-       rayAabbMax.setMax(rayTo);
+       SimdVector3     hitPointWorld,normalWorld;
 
-       
-       CcdPhysicsController* nearestHit = 0;
-       
-       std::vector<CcdPhysicsController*>::iterator i;
-       SphereShape pointShape(0.0f);
+       CollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
 
-       /// brute force go over all objects. Once there is a broadphase, use that, or
-       /// add a raycast against aabb first.
-       for (i=m_controllers.begin();
-       !(i==m_controllers.end()); i++)
-       {
-               CcdPhysicsController* ctrl = (*i);
-               if (ctrl == ignoreClient)
-                       continue;
-               RigidBody* body = ctrl->GetRigidBody();
-               SimdVector3 bodyAabbMin,bodyAabbMax;
-               body->getAabb(bodyAabbMin,bodyAabbMax);
 
-               //check aabb overlap
+       struct  FilterClosestRayResultCallback : CollisionWorld::ClosestRayResultCallback
+       {
+               PHY_IPhysicsController* m_ignoreClient;
 
-               if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,bodyAabbMin,bodyAabbMax))
+               FilterClosestRayResultCallback (PHY_IPhysicsController* ignoreClient,const SimdVector3& rayFrom,const SimdVector3& rayTo)
+                       : CollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
+                       m_ignoreClient(ignoreClient)
                {
-                       if (body->GetCollisionShape()->IsConvex())
-                       {
-                               ConvexCast::CastResult rayResult;
-                               rayResult.m_fraction = 1.f;
-
-                               ConvexShape* convexShape = (ConvexShape*) body->GetCollisionShape();
-                               VoronoiSimplexSolver    simplexSolver;
-                               SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
-                               //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
-                               //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
-                               
-                               if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,body->getCenterOfMassTransform(),body->getCenterOfMassTransform(),rayResult))
-                               {
-                                       //add hit
-                                       if (rayResult.m_normal.length2() > 0.0001f)
-                                       {
-                                               rayResult.m_normal.normalize();
-                                               if (rayResult.m_fraction < minFraction)
-                                               {
 
+               }
 
-                                                       minFraction = rayResult.m_fraction;
-                                                       nearestHit = ctrl;
-                                                       normalX = rayResult.m_normal.getX();
-                                                       normalY = rayResult.m_normal.getY();
-                                                       normalZ = rayResult.m_normal.getZ();
-                                                       SimdVector3 hitWorld;
-                                                       hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rayResult.m_fraction);
-                                                       hitX = hitWorld.getX();
-                                                       hitY = hitWorld.getY();
-                                                       hitZ = hitWorld.getZ();
-
-                                               }
-                                       }
-                               }
+               virtual float   AddSingleResult(const CollisionWorld::LocalRayResult& rayResult)
+               {
+                       CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->m_userPointer);
+                       //ignore client...
+                       if (curHit != m_ignoreClient)
+                       {               
+                               //if valid
+                               return CollisionWorld::ClosestRayResultCallback::AddSingleResult(rayResult);
                        }
-                       else
-                       {
-                                       if (body->GetCollisionShape()->IsConcave())
-                                       {
+               }
 
-                                               TriangleMeshShape* triangleMesh = (TriangleMeshShape*)body->GetCollisionShape();
-                                               
-                                               SimdTransform worldToBody = body->getCenterOfMassTransform().inverse();
+       };
 
-                                               SimdVector3 rayFromLocal = worldToBody * rayFromTrans.getOrigin();
-                                               SimdVector3 rayToLocal = worldToBody * rayToTrans.getOrigin();
 
-                                               RaycastCallback rcb(rayFromLocal,rayToLocal);
-                                               rcb.m_hitFraction = minFraction;
+       PHY_IPhysicsController* nearestHit = 0;
 
-                                               SimdVector3 rayAabbMinLocal = rayFromLocal;
-                                               rayAabbMinLocal.setMin(rayToLocal);
-                                               SimdVector3 rayAabbMaxLocal = rayFromLocal;
-                                               rayAabbMaxLocal.setMax(rayToLocal);
+       m_collisionWorld->RayTest(rayFrom,rayTo,rayCallback);
+       if (rayCallback.HasHit())
+       {
+               nearestHit = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->m_userPointer);
+               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();
+
+       }       
 
-                                               triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
-                                               if (rcb.m_hitFound)
-                                               {
-                                                       nearestHit = ctrl;
-                                                       minFraction = rcb.m_hitFraction;
-                                                       SimdVector3 hitNormalWorld = body->getCenterOfMassTransform().getBasis()*rcb.m_hitNormalLocal;
-                                                       hitNormalWorld.normalize();
-
-                                                       normalX = hitNormalWorld.getX();
-                                                       normalY = hitNormalWorld.getY();
-                                                       normalZ = hitNormalWorld.getZ();
-                                                       SimdVector3 hitWorld;
-                                                       hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rcb.m_hitFraction);
-                                                       hitX = hitWorld.getX();
-                                                       hitY = hitWorld.getY();
-                                                       hitZ = hitWorld.getZ();
-                                                       
-                                               }
-                                       }
-                       }
-               }
-       }
 
        return nearestHit;
 }
@@ -1388,7 +1291,7 @@ int       CcdPhysicsEnvironment::getNumContactPoints()
 
 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
 {
-       
+
 }
 
 
@@ -1413,18 +1316,18 @@ CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher()
 
 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;
-       
+
 }
 
 
@@ -1476,31 +1379,31 @@ void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
 }
 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
 {
-/*     printf("addTouchCallback\n(response class = %i)\n",response_class);
+       /*      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;
+       printf("PHY_FH_RESPONSE\n");
+       break;
        case PHY_SENSOR_RESPONSE:
-               printf("PHY_SENSOR_RESPONSE\n");
-               break;
+       printf("PHY_SENSOR_RESPONSE\n");
+       break;
        case PHY_CAMERA_RESPONSE:
-               printf("PHY_CAMERA_RESPONSE\n");
-               break;
+       printf("PHY_CAMERA_RESPONSE\n");
+       break;
        case PHY_OBJECT_RESPONSE:
-               printf("PHY_OBJECT_RESPONSE\n");
-               break;
+       printf("PHY_OBJECT_RESPONSE\n");
+       break;
        case PHY_STATIC_RESPONSE:
-               printf("PHY_STATIC_RESPONSE\n");
-               break;
+       printf("PHY_STATIC_RESPONSE\n");
+       break;
        default:
-               assert(0);
-               return;
+       assert(0);
+       return;
        }
-*/
+       */
 
        m_triggerCallbacks[response_class] = callback;
        m_triggerCallbacksUserPtrs[response_class] = user;
@@ -1531,13 +1434,13 @@ void    CcdPhysicsEnvironment::CallbackTriggers()
                        {
                                RigidBody* obj0 = static_cast<RigidBody* >(manifold->GetBody0());
                                RigidBody* obj1 = static_cast<RigidBody* >(manifold->GetBody1());
-                               
+
                                //m_userPointer is set in 'addPhysicsController
                                CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->m_userPointer);
                                CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->m_userPointer);
 
                                std::vector<CcdPhysicsController*>::iterator i =
-                               std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
+                                       std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
                                if (i == m_triggerControllers.end())
                                {
                                        i = std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl1);
@@ -1551,7 +1454,7 @@ void      CcdPhysicsEnvironment::CallbackTriggers()
                        }
                }
 
-               
+
 
        }
 
@@ -1582,3 +1485,81 @@ PHY_IVehicle*    CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
 
 #endif //NEW_BULLET_VEHICLE_SUPPORT
 
+
+
+void   CcdPhysicsEnvironment::UpdateAabbs(float        timeStep)
+{
+       std::vector<CcdPhysicsController*>::iterator i;
+       BroadphaseInterface* scene =  GetBroadphase();
+
+       //
+                       // 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);
+
+
+                               SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
+                               minAabb -= manifoldExtraExtents;
+                               maxAabb += manifoldExtraExtents;
+
+                               BroadphaseProxy* bp = body->m_broadphaseHandle;
+                               if (bp)
+                               {
+
+#ifdef WIN32
+                                       SimdVector3 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() & IDebugDraw::DBG_DrawAabb)
+                                               {
+                                                       DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
+                                               }
+                                       }
+#endif
+
+                                       scene->SetAabb(bp,minAabb,maxAabb);
+
+
+
+                               }
+                       }
+}
\ No newline at end of file
index 2a4396d8c7892fd4536371be21599d37f1fd33af..08dff62e236e5e90a87a0e97d359f6179a9206a4 100644 (file)
@@ -1,3 +1,18 @@
+/*
+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
 
@@ -24,6 +39,7 @@ class PersistentManifold;
 class BroadphaseInterface;
 class IDebugDraw;
 
+/// CcdPhysicsEnvironment is 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
@@ -147,6 +163,8 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
                        m_enableSatCollisionDetection = enableSat;
                }
 
+               void    UpdateAabbs(float       timeStep);
+
                int     GetNumControllers();
 
                CcdPhysicsController* GetPhysicsController( int index);