diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp index 1a3f192e533..5fe5157b420 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp @@ -128,6 +128,7 @@ void CollisionDispatcher::BuildAndProcessIslands(int numBodies, IslandCallback* CollisionObject* colObj0 = static_cast(manifold->GetBody0()); CollisionObject* colObj1 = static_cast(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()); + } } } diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp index 5264662b519..812ffa391dd 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp @@ -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 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::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 diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h index 0e7cb80bf20..c83192d9d2f 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h @@ -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 @@ -26,13 +30,15 @@ class BroadphaseInterface; class CollisionWorld { + + std::vector 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); diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp b/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp index d600c061638..292b0d72de1 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp @@ -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(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; diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h b/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h index 7569f4ed32e..18652334b4e 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h +++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h @@ -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(); diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp index 2bb3016acdb..adb310c3f99 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp @@ -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(m_box0.m_clientObject); CollisionObject* col1 = static_cast(m_box1.m_clientObject); + SphereShape sphere(0.f); + ConvexShape* min0 = static_cast(col0->m_collisionShape); ConvexShape* min1 = static_cast(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; - } diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h index c96d03e3989..c8ace81dda1 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h +++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h @@ -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; diff --git a/extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.cpp b/extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.cpp index c054d689e75..d2624dcdfe1 100644 --- a/extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.cpp @@ -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); } diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleCallback.h b/extern/bullet/Bullet/CollisionShapes/TriangleCallback.h index 80519160c9c..810d4e62644 100644 --- a/extern/bullet/Bullet/CollisionShapes/TriangleCallback.h +++ b/extern/bullet/Bullet/CollisionShapes/TriangleCallback.h @@ -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 diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp b/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp index 486b41fbc00..73b53cd0266 100644 --- a/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp @@ -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); } } diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/ContinuousConvexCollision.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/ContinuousConvexCollision.cpp index f5762ee248c..710208a916e 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/ContinuousConvexCollision.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/ContinuousConvexCollision.cpp @@ -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); diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp index 344a4555bcf..f87e9ced8a7 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp @@ -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; diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp index 1390667a58a..f497a77d7e5 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp @@ -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; diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h b/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h index e18fd20697c..c550728f845 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h @@ -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 diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp index 9401aa05699..fc4a533b225 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp @@ -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; } } } diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.h b/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.h index dc31eca76fe..c58fa202fc8 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.h @@ -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; - - - RaycastCallback(const SimdVector3& from,const SimdVector3& to); + float m_hitFraction; + TriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to); + + virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex); + + virtual void ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) = 0; - virtual void ProcessTriangle(SimdVector3* triangle); }; #endif //RAYCAST_TRI_CALLBACK_H diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp index 68010b85180..79b311bd691 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp @@ -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" diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h index e670755b1f2..f1933d55137 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h @@ -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 diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index 5cb2859046c..d2559e88e10 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -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" @@ -17,10 +35,10 @@ #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,26 @@ 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::iterator i; - + for (i=m_constraints.begin(); - !(i==m_constraints.end()); i++) - { - TypedConstraint* constraint = (*i); - if ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() || - (&constraint->GetRigidBodyB() == ctrl->GetRigidBody()))) - { - removeConstraint(constraint->GetUserConstraintId()); - //only 1 constraint per constroller - break; - } - } - } - - { - std::vector::iterator i; - - for (i=m_constraints.begin(); - !(i==m_constraints.end()); i++) + !(i==m_constraints.end()); i++) { TypedConstraint* constraint = (*i); if ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() || @@ -442,11 +443,28 @@ void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr } } } - - + + { + std::vector::iterator i; + + for (i=m_constraints.begin(); + !(i==m_constraints.end()); i++) + { + TypedConstraint* constraint = (*i); + if ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() || + (&constraint->GetRigidBodyB() == ctrl->GetRigidBody()))) + { + removeConstraint(constraint->GetUserConstraintId()); + //only 1 constraint per constroller + break; + } + } + } + + m_collisionWorld->RemoveCollisionObject(ctrl->GetRigidBody()); - + { std::vector::iterator i = std::find(m_controllers.begin(), m_controllers.end(), ctrl); @@ -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::iterator i; - - - + // std::vector::iterator i; + + + int k; for (k=0;kintegrateVelocities( 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 - Profiler::beginBlock("DispatchAllCollisionPairs"); -#endif //USE_PROFILE - + +#ifdef USE_QUICKPROF + Profiler::beginBlock("DispatchAllCollisionPairs"); +#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;gBuildJacobian(); 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;iGetVehicle(); - vehicle->UpdateVehicle( timeStep); - } +#ifdef NEW_BULLET_VEHICLE_SUPPORT + //vehicles + int numVehicles = m_wrapperVehicles.size(); + for (int i=0;iGetVehicle(); + vehicle->UpdateVehicle( timeStep); + } #endif //NEW_BULLET_VEHICLE_SUPPORT - - struct InplaceSolverIslandCallback : public 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::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(); + UpdateAabbs(timeStep); - - 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); - - - - } - } - 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::iterator i; + std::vector::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 m_sleepingControllers; - + + std::vector::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;iSyncWheels(); } #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) - { - } + 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]; + /* 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) + { - 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; - } - */ + float hit[3]; + float normal[3]; - - - } - //? - return hitObject; - } + 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::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(); - 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::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++) + + struct FilterClosestRayResultCallback : CollisionWorld::ClosestRayResultCallback { - CcdPhysicsController* ctrl = (*i); - if (ctrl == ignoreClient) - continue; - RigidBody* body = ctrl->GetRigidBody(); - SimdVector3 bodyAabbMin,bodyAabbMax; - body->getAabb(bodyAabbMin,bodyAabbMax); + PHY_IPhysicsController* m_ignoreClient; - //check aabb overlap - - 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(); - - } - } - } - } - 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; - - SimdVector3 rayAabbMinLocal = rayFromLocal; - rayAabbMinLocal.setMin(rayToLocal); - SimdVector3 rayAabbMaxLocal = rayFromLocal; - rayAabbMaxLocal.setMax(rayToLocal); - - 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(); - - } - } + virtual float AddSingleResult(const CollisionWorld::LocalRayResult& rayResult) + { + CcdPhysicsController* curHit = static_cast(rayResult.m_collisionObject->m_userPointer); + //ignore client... + if (curHit != m_ignoreClient) + { + //if valid + return CollisionWorld::ClosestRayResultCallback::AddSingleResult(rayResult); } } - } + + }; + + + PHY_IPhysicsController* nearestHit = 0; + + m_collisionWorld->RayTest(rayFrom,rayTo,rayCallback); + if (rayCallback.HasHit()) + { + nearestHit = static_cast(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(); + + } + 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(manifold->GetBody0()); RigidBody* obj1 = static_cast(manifold->GetBody1()); - + //m_userPointer is set in 'addPhysicsController CcdPhysicsController* ctrl0 = static_cast(obj0->m_userPointer); CcdPhysicsController* ctrl1 = static_cast(obj1->m_userPointer); std::vector::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::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 diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h index 2a4396d8c78..08dff62e236 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h @@ -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); diff --git a/extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h b/extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h index 8e60fdb126e..79cd57756b0 100644 --- a/extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h +++ b/extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h @@ -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; diff --git a/extern/bullet/LinearMath/IDebugDraw.h b/extern/bullet/LinearMath/IDebugDraw.h index 5ef4d8d126b..be6c3fd4fbb 100644 --- a/extern/bullet/LinearMath/IDebugDraw.h +++ b/extern/bullet/LinearMath/IDebugDraw.h @@ -48,6 +48,7 @@ class IDebugDraw DBG_ProfileTimings = 128, DBG_EnableSatComparison = 256, DBG_DisableBulletLCP = 512, + DBG_EnableCCD = 1024, DBG_MAX_DEBUG_DRAW_MODE }; diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp index 68010b85180..79b311bd691 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp @@ -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" diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.h b/source/gameengine/Physics/Bullet/CcdPhysicsController.h index e670755b1f2..f1933d55137 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsController.h +++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.h @@ -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 diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp index 5cb2859046c..d2559e88e10 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp @@ -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" @@ -17,10 +35,10 @@ #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,26 @@ 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::iterator i; - + for (i=m_constraints.begin(); - !(i==m_constraints.end()); i++) - { - TypedConstraint* constraint = (*i); - if ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() || - (&constraint->GetRigidBodyB() == ctrl->GetRigidBody()))) - { - removeConstraint(constraint->GetUserConstraintId()); - //only 1 constraint per constroller - break; - } - } - } - - { - std::vector::iterator i; - - for (i=m_constraints.begin(); - !(i==m_constraints.end()); i++) + !(i==m_constraints.end()); i++) { TypedConstraint* constraint = (*i); if ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() || @@ -442,11 +443,28 @@ void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr } } } - - + + { + std::vector::iterator i; + + for (i=m_constraints.begin(); + !(i==m_constraints.end()); i++) + { + TypedConstraint* constraint = (*i); + if ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() || + (&constraint->GetRigidBodyB() == ctrl->GetRigidBody()))) + { + removeConstraint(constraint->GetUserConstraintId()); + //only 1 constraint per constroller + break; + } + } + } + + m_collisionWorld->RemoveCollisionObject(ctrl->GetRigidBody()); - + { std::vector::iterator i = std::find(m_controllers.begin(), m_controllers.end(), ctrl); @@ -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::iterator i; - - - + // std::vector::iterator i; + + + int k; for (k=0;kintegrateVelocities( 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 - Profiler::beginBlock("DispatchAllCollisionPairs"); -#endif //USE_PROFILE - + +#ifdef USE_QUICKPROF + Profiler::beginBlock("DispatchAllCollisionPairs"); +#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;gBuildJacobian(); 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;iGetVehicle(); - vehicle->UpdateVehicle( timeStep); - } +#ifdef NEW_BULLET_VEHICLE_SUPPORT + //vehicles + int numVehicles = m_wrapperVehicles.size(); + for (int i=0;iGetVehicle(); + vehicle->UpdateVehicle( timeStep); + } #endif //NEW_BULLET_VEHICLE_SUPPORT - - struct InplaceSolverIslandCallback : public 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::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(); + UpdateAabbs(timeStep); - - 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); - - - - } - } - 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::iterator i; + std::vector::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 m_sleepingControllers; - + + std::vector::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;iSyncWheels(); } #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) - { - } + 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]; + /* 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) + { - 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; - } - */ + float hit[3]; + float normal[3]; - - - } - //? - return hitObject; - } + 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::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(); - 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::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++) + + struct FilterClosestRayResultCallback : CollisionWorld::ClosestRayResultCallback { - CcdPhysicsController* ctrl = (*i); - if (ctrl == ignoreClient) - continue; - RigidBody* body = ctrl->GetRigidBody(); - SimdVector3 bodyAabbMin,bodyAabbMax; - body->getAabb(bodyAabbMin,bodyAabbMax); + PHY_IPhysicsController* m_ignoreClient; - //check aabb overlap - - 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(); - - } - } - } - } - 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; - - SimdVector3 rayAabbMinLocal = rayFromLocal; - rayAabbMinLocal.setMin(rayToLocal); - SimdVector3 rayAabbMaxLocal = rayFromLocal; - rayAabbMaxLocal.setMax(rayToLocal); - - 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(); - - } - } + virtual float AddSingleResult(const CollisionWorld::LocalRayResult& rayResult) + { + CcdPhysicsController* curHit = static_cast(rayResult.m_collisionObject->m_userPointer); + //ignore client... + if (curHit != m_ignoreClient) + { + //if valid + return CollisionWorld::ClosestRayResultCallback::AddSingleResult(rayResult); } } - } + + }; + + + PHY_IPhysicsController* nearestHit = 0; + + m_collisionWorld->RayTest(rayFrom,rayTo,rayCallback); + if (rayCallback.HasHit()) + { + nearestHit = static_cast(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(); + + } + 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(manifold->GetBody0()); RigidBody* obj1 = static_cast(manifold->GetBody1()); - + //m_userPointer is set in 'addPhysicsController CcdPhysicsController* ctrl0 = static_cast(obj0->m_userPointer); CcdPhysicsController* ctrl1 = static_cast(obj1->m_userPointer); std::vector::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::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 diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h index 2a4396d8c78..08dff62e236 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h @@ -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);