diff options
38 files changed, 893 insertions, 169 deletions
diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index 713d7d1aa19..3462e6e472d 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -103,6 +103,10 @@ struct btBroadphaseProxy { return (proxyType == COMPOUND_SHAPE_PROXYTYPE); } + static inline bool isInfinite(int proxyType) + { + return (proxyType == STATIC_PLANE_PROXYTYPE); + } }; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h index a57c7619985..3195f996fe0 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h @@ -21,11 +21,14 @@ class btDispatcher; class btManifoldResult; struct btCollisionObject; struct btDispatcherInfo; +class btPersistentManifold; + struct btCollisionAlgorithmConstructionInfo { btCollisionAlgorithmConstructionInfo() - :m_dispatcher(0) + :m_dispatcher(0), + m_manifold(0) { } btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp) @@ -34,6 +37,7 @@ struct btCollisionAlgorithmConstructionInfo } btDispatcher* m_dispatcher; + btPersistentManifold* m_manifold; int getDispatcherId(); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h index f87c0281a97..75ef338fdde 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -65,7 +65,7 @@ class btDispatcher public: virtual ~btDispatcher() ; - virtual btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1) = 0; + virtual btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold=0) = 0; // // asume dispatchers to have unique id's in the range [0..max dispacher] diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index 85bb8265cf9..bc62961bf3c 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -66,7 +66,7 @@ class btOverlappingPairCache : public btBroadphaseInterface inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const { - bool collides = proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask; + bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); return collides; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp new file mode 100644 index 00000000000..1b25006deec --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -0,0 +1,201 @@ +/* +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 "SphereTriangleDetector.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + + +SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle) +:m_sphere(sphere), +m_triangle(triangle) +{ + +} + +void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +{ + + const btTransform& transformA = input.m_transformA; + const btTransform& transformB = input.m_transformB; + + btVector3 point,normal; + btScalar timeOfImpact = 1.f; + btScalar depth = 0.f; +// output.m_distance = 1e30f; + //move sphere into triangle space + btTransform sphereInTr = transformB.inverseTimes(transformA); + + if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact)) + { + output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth); + } + +} + +#define MAX_OVERLAP 0.f + + + +// See also geometrictools.com +// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv +float SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) { + btVector3 diff = p - from; + btVector3 v = to - from; + float t = v.dot(diff); + + if (t > 0) { + float dotVV = v.dot(v); + if (t < dotVV) { + t /= dotVV; + diff -= t*v; + } else { + t = 1; + diff -= v; + } + } else + t = 0; + + nearest = from + t*v; + return diff.dot(diff); +} + +bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) { + btVector3 lp(p); + btVector3 lnormal(normal); + + return pointInTriangle(vertices, lnormal, &lp); +} + +///combined discrete/continuous sphere-triangle +bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, float &timeOfImpact) +{ + + const btVector3* vertices = &m_triangle->getVertexPtr(0); + const btVector3& c = sphereCenter; + btScalar r = m_sphere->getRadius(); + + btVector3 delta (0,0,0); + + btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); + normal.normalize(); + btVector3 p1ToCentre = c - vertices[0]; + float distanceFromPlane = p1ToCentre.dot(normal); + + if (distanceFromPlane < 0.f) + { + //triangle facing the other way + + distanceFromPlane *= -1.f; + normal *= -1.f; + } + + ///todo: move this gContactBreakingThreshold into a proper structure + extern float gContactBreakingThreshold; + + float contactMargin = gContactBreakingThreshold; + bool isInsideContactPlane = distanceFromPlane < r + contactMargin; + bool isInsideShellPlane = distanceFromPlane < r; + + float deltaDotNormal = delta.dot(normal); + if (!isInsideShellPlane && deltaDotNormal >= 0.0f) + return false; + + // Check for contact / intersection + bool hasContact = false; + btVector3 contactPoint; + if (isInsideContactPlane) { + if (facecontains(c,vertices,normal)) { + // Inside the contact wedge - touches a point on the shell plane + hasContact = true; + contactPoint = c - normal*distanceFromPlane; + } else { + // Could be inside one of the contact capsules + float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); + btVector3 nearestOnEdge; + for (int i = 0; i < m_triangle->getNumEdges(); i++) { + + btPoint3 pa; + btPoint3 pb; + + m_triangle->getEdge(i,pa,pb); + + float distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge); + if (distanceSqr < contactCapsuleRadiusSqr) { + // Yep, we're inside a capsule + hasContact = true; + contactPoint = nearestOnEdge; + } + + } + } + } + + if (hasContact) { + btVector3 contactToCentre = c - contactPoint; + float distanceSqr = contactToCentre.length2(); + if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) { + float distance = sqrtf(distanceSqr); + if (1) + { + resultNormal = contactToCentre; + resultNormal.normalize(); + } + point = contactPoint; + depth = -(r-distance); + return true; + } + + if (delta.dot(contactToCentre) >= 0.0f) + return false; + + // Moving towards the contact point -> collision + point = contactPoint; + timeOfImpact = 0.0f; + return true; + } + + return false; +} + + +bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ) +{ + const btVector3* p1 = &vertices[0]; + const btVector3* p2 = &vertices[1]; + const btVector3* p3 = &vertices[2]; + + btVector3 edge1( *p2 - *p1 ); + btVector3 edge2( *p3 - *p2 ); + btVector3 edge3( *p1 - *p3 ); + + btVector3 p1_to_p( *p - *p1 ); + btVector3 p2_to_p( *p - *p2 ); + btVector3 p3_to_p( *p - *p3 ); + + btVector3 edge1_normal( edge1.cross(normal)); + btVector3 edge2_normal( edge2.cross(normal)); + btVector3 edge3_normal( edge3.cross(normal)); + + float r1, r2, r3; + r1 = edge1_normal.dot( p1_to_p ); + r2 = edge2_normal.dot( p2_to_p ); + r3 = edge3_normal.dot( p3_to_p ); + if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || + ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) + return true; + return false; + +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h new file mode 100644 index 00000000000..22262340222 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -0,0 +1,48 @@ +/* +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 SPHERE_TRIANGLE_DETECTOR_H +#define SPHERE_TRIANGLE_DETECTOR_H + +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" +#include "LinearMath/btPoint3.h" + + +class btSphereShape; +class btTriangleShape; + + + +/// sphere-triangle to match the btDiscreteCollisionDetectorInterface +struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface +{ + virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw); + + SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle); + + virtual ~SphereTriangleDetector() {}; + +private: + + bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, float &timeOfImpact); + bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ); + bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal); + + btSphereShape* m_sphere; + btTriangleShape* m_triangle; + + +}; +#endif //SPHERE_TRIANGLE_DETECTOR_H
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index d824f68ebe7..ebfccef9cb1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -142,13 +142,14 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) -btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1) +btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold) { #define USE_DISPATCH_REGISTRY_ARRAY 1 #ifdef USE_DISPATCH_REGISTRY_ARRAY btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher = this; + ci.m_manifold = sharedManifold; btCollisionAlgorithm* algo = m_doubleDispatch[body0->m_collisionShape->getShapeType()][body1->m_collisionShape->getShapeType()] ->CreateCollisionAlgorithm(ci,body0,body1); #else @@ -193,7 +194,7 @@ btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(in -btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1) +btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold) { m_count++; @@ -202,7 +203,7 @@ btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionOb if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConvex() ) { - return new btConvexConvexAlgorithm(0,ci,body0,body1); + return new btConvexConvexAlgorithm(sharedManifold,ci,body0,body1); } if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConcave()) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index def59650455..4e97bce9d65 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -56,7 +56,7 @@ class btCollisionDispatcher : public btDispatcher btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; - btCollisionAlgorithm* internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1); + btCollisionAlgorithm* internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0); public: @@ -78,7 +78,7 @@ public: int getNumManifolds() const { - return m_manifoldsPtr.size(); + return int( m_manifoldsPtr.size()); } btPersistentManifold** getInternalManifoldPointer() @@ -114,7 +114,7 @@ public: virtual void clearManifold(btPersistentManifold* manifold); - btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1); + btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0); virtual bool needsCollision(btCollisionObject* body0,btCollisionObject* body1); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 881a8c0042a..4179bc47e82 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -24,7 +24,7 @@ btCollisionObject::btCollisionObject() m_userObjectPointer(0), m_hitFraction(1.f), m_ccdSweptSphereRadius(0.f), - m_ccdSquareMotionTreshold(0.f) + m_ccdSquareMotionThreshold(0.f) { } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 3838fc98961..5df3de489cd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -43,6 +43,11 @@ struct btCollisionObject ///m_interpolationWorldTransform is used for CCD and interpolation ///it can be either previous or future (predicted) transform btTransform m_interpolationWorldTransform; + //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) + //without destroying the continuous interpolated motion (which uses this interpolation velocities) + btVector3 m_interpolationLinearVelocity; + btVector3 m_interpolationAngularVelocity; + enum CollisionFlags { @@ -73,32 +78,32 @@ struct btCollisionObject ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: float m_ccdSweptSphereRadius; - /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold - float m_ccdSquareMotionTreshold; + /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold + float m_ccdSquareMotionThreshold; inline bool mergesSimulationIslands() const { ///static objects, kinematic and object without contact response don't merge islands - return !(m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OJBECT | CF_NO_CONTACT_RESPONSE) ); + return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OJBECT | CF_NO_CONTACT_RESPONSE) )==0); } inline bool isStaticObject() const { - return m_collisionFlags & CF_STATIC_OBJECT; + return (m_collisionFlags & CF_STATIC_OBJECT) != 0; } inline bool isKinematicObject() const { - return m_collisionFlags & CF_KINEMATIC_OJBECT; + return (m_collisionFlags & CF_KINEMATIC_OJBECT) != 0; } inline bool isStaticOrKinematicObject() const { - return m_collisionFlags & (CF_KINEMATIC_OJBECT | CF_STATIC_OBJECT); + return (m_collisionFlags & (CF_KINEMATIC_OJBECT | CF_STATIC_OBJECT)) != 0 ; } inline bool hasContactResponse() const { - return !(m_collisionFlags & CF_NO_CONTACT_RESPONSE); + return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 2280399b2c8..a1cf3a0e5fd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -205,7 +205,7 @@ public: int getNumCollisionObjects() const { - return m_collisionObjects.size(); + return int(m_collisionObjects.size()); } /// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index d7d0055f714..7cb0bba6206 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -17,7 +17,6 @@ subject to the following restrictions: #include "btConvexConcaveCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "btConvexConvexAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionShapes/btConcaveShape.h" #include "BulletCollision/CollisionDispatch/btManifoldResult.h" @@ -115,10 +114,16 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i btCollisionShape* tmpShape = ob->m_collisionShape; ob->m_collisionShape = &tm; + + btCollisionAlgorithm* colAlgo = ci.m_dispatcher->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr); ///this should use the btDispatcher, so the actual registered algorithm is used - btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); - cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); - cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); + + m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex); + // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); +// cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + delete colAlgo; ob->m_collisionShape = tmpShape; } @@ -200,10 +205,10 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) - //only perform CCD above a certain treshold, this prevents blocking on the long run + //only perform CCD above a certain threshold, this prevents blocking on the long run //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... float squareMot0 = (convexbody->m_interpolationWorldTransform.getOrigin() - convexbody->m_worldTransform.getOrigin()).length2(); - if (squareMot0 < convexbody->m_ccdSquareMotionTreshold) + if (squareMot0 < convexbody->m_ccdSquareMotionThreshold) { return 1.f; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 54b8bc06aaa..5347ef05bef 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -165,14 +165,14 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl //TODO: if (dispatchInfo.m_useContinuous) m_gjkPairDetector.setMinkowskiA(min0); m_gjkPairDetector.setMinkowskiB(min1); - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingTreshold(); + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; // input.m_maximumDistanceSquared = 1e30f; input.m_transformA = body0->m_worldTransform; input.m_transformB = body1->m_worldTransform; - + resultOut->setPersistentManifold(m_manifoldPtr); m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); @@ -183,17 +183,17 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl bool disableCcd = false; float btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { - ///Rather then checking ALL pairs, only calculate TOI when motion exceeds treshold + ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold - ///Linear motion for one of objects needs to exceed m_ccdSquareMotionTreshold + ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold ///col0->m_worldTransform, float resultFraction = 1.f; float squareMot0 = (col0->m_interpolationWorldTransform.getOrigin() - col0->m_worldTransform.getOrigin()).length2(); - if (squareMot0 < col0->m_ccdSquareMotionTreshold && - squareMot0 < col0->m_ccdSquareMotionTreshold) + if (squareMot0 < col0->m_ccdSquareMotionThreshold && + squareMot0 < col0->m_ccdSquareMotionThreshold) return resultFraction; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index b8e121714ad..1cd94408f1f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -54,13 +54,6 @@ public: void setLowLevelOfDetail(bool useLowLevel); - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) - { - m_gjkPairDetector.m_partId0=partId0; - m_gjkPairDetector.m_partId1=partId1; - m_gjkPairDetector.m_index0=index0; - m_gjkPairDetector.m_index1=index1; - } const btPersistentManifold* getManifold() { @@ -71,7 +64,7 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btConvexConvexAlgorithm(0,ci,body0,body1); + return new btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 7031521f66b..1d3941101b2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -58,7 +58,7 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b assert(m_manifoldPtr); //order in manifold needs to match - if (depth > m_manifoldPtr->getContactBreakingTreshold()) + if (depth > m_manifoldPtr->getContactBreakingThreshold()) return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0; @@ -74,33 +74,28 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b int insertIndex = m_manifoldPtr->getCacheEntry(newPt); - if (insertIndex >= 0) - { -// This is not needed, just use the old info! -// const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); -// newPt.CopyPersistentInformation(oldPoint); -// m_manifoldPtr->replaceContactPoint(newPt,insertIndex); + newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1); + newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1); + //User can override friction and/or restitution + if (gContactAddedCallback && + //and if either of the two bodies requires custom material + ((m_body0->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || + (m_body1->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) + { + //experimental feature info, for per-triangle material etc. + btCollisionObject* obj0 = isSwapped? m_body1 : m_body0; + btCollisionObject* obj1 = isSwapped? m_body0 : m_body1; + (*gContactAddedCallback)(newPt,obj0,m_partId0,m_index0,obj1,m_partId1,m_index1); + } + if (insertIndex >= 0) + { + //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); + m_manifoldPtr->replaceContactPoint(newPt,insertIndex); } else { - - newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1); - newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1); - - //User can override friction and/or restitution - if (gContactAddedCallback && - //and if either of the two bodies requires custom material - ((m_body0->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || - (m_body1->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) - { - //experimental feature info, for per-triangle material etc. - btCollisionObject* obj0 = isSwapped? m_body1 : m_body0; - btCollisionObject* obj1 = isSwapped? m_body0 : m_body1; - (*gContactAddedCallback)(newPt,obj0,m_partId0,m_index0,obj1,m_partId1,m_index1); - } - m_manifoldPtr->AddManifoldPoint(newPt); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index fa52d7e055a..d6ac86d19bd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -1,4 +1,6 @@ + +#include "LinearMath/btScalar.h" #include "btSimulationIslandManager.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -51,7 +53,7 @@ void btSimulationIslandManager::findUnions(btDispatcher* dispatcher) void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) { - initUnionFind(colWorld->getCollisionObjectArray().size()); + initUnionFind( int (colWorld->getCollisionObjectArray().size())); // put the index into m_controllers into m_tag { @@ -253,7 +255,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, } } - int numManifolds = islandmanifold.size(); + int numManifolds = int (islandmanifold.size()); // Sort manifolds, based on islands // Sort the vector using predicate and std::sort diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp new file mode 100644 index 00000000000..ca589ef8629 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -0,0 +1,71 @@ +/* +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 "btSphereTriangleCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "SphereTriangleDetector.h" + + +btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped) +: btCollisionAlgorithm(ci), +m_ownManifold(false), +m_manifoldPtr(mf), +m_swapped(swapped) +{ + if (!m_manifoldPtr) + { + m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1); + m_ownManifold = true; + } +} + +btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + if (!m_manifoldPtr) + return; + + btSphereShape* sphere = (btSphereShape*)col0->m_collisionShape; + btTriangleShape* triangle = (btTriangleShape*)col1->m_collisionShape; + + /// report a contact. internally this will be kept persistent, and contact reduction is done + resultOut->setPersistentManifold(m_manifoldPtr); + SphereTriangleDetector detector(sphere,triangle); + + btDiscreteCollisionDetectorInterface::ClosestPointInput input; + input.m_maximumDistanceSquared = 1e30f;//todo: tighter bounds + input.m_transformA = col0->m_worldTransform; + input.m_transformB = col1->m_worldTransform; + + detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + +} + +float btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + //not yet + return 1.f; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h new file mode 100644 index 00000000000..82e4c5b37f1 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -0,0 +1,59 @@ +/* +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 SPHERE_TRIANGLE_COLLISION_ALGORITHM_H +#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; + +/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +/// Also provides the most basic sample for custom/user btCollisionAlgorithm +class btSphereTriangleCollisionAlgorithm : public btCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_swapped; + +public: + btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped); + + btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btCollisionAlgorithm(ci) {} + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + + virtual ~btSphereTriangleCollisionAlgorithm(); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + + return new btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped); + } + }; + +}; + +#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h index 8db1580e41e..b1baca9ff15 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -45,7 +45,7 @@ class btUnionFind inline int getNumElements() const { - return m_elements.size(); + return int(m_elements.size()); } inline bool isRoot(int x) const { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h index d015fb2baae..7b2a00a1c57 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -66,6 +66,12 @@ public: return btBroadphaseProxy::isCompound(getShapeType()); } + ///isInfinite is used to catch simulation error (aabb check) + inline bool isInfinite() const + { + return btBroadphaseProxy::isInfinite(getShapeType()); + } + virtual void setLocalScaling(const btVector3& scaling) =0; virtual const btVector3& getLocalScaling() const =0; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index 65a6809d4ff..c810a654834 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -46,7 +46,7 @@ public: int getNumChildShapes() const { - return m_childShapes.size(); + return int (m_childShapes.size()); } btCollisionShape* getChildShape(int index) diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index f6fdd6435cf..4b38ced7f12 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -36,11 +36,7 @@ m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), m_minkowskiA(objectA), m_minkowskiB(objectB), -m_ignoreMargin(false), -m_partId0(-1), -m_index0(-1), -m_partId1(-1), -m_index1(-1) +m_ignoreMargin(false) { } @@ -60,7 +56,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& marginB = 0.f; } -int curIter = 0; + int curIter = 0; + int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? bool isValid = false; bool checkSimplex = false; @@ -131,6 +128,25 @@ int curIter = 0; checkSimplex = true; break; } + + //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject + if (curIter++ > gGjkMaxIter) + { + #if defined(DEBUG) || defined (_DEBUG) + printf("btGjkPairDetector maxIter exceeded:%i\n",curIter); + printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", + m_cachedSeparatingAxis.getX(), + m_cachedSeparatingAxis.getY(), + m_cachedSeparatingAxis.getZ(), + squaredDistance, + m_minkowskiA->getShapeType(), + m_minkowskiB->getShapeType()); + #endif + break; + + } + + bool check = (!m_simplexSolver->fullSimplex()); //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); @@ -200,7 +216,6 @@ int curIter = 0; //spu_printf("distance\n"); #endif //__CELLOS_LV2__ - output.setShapeIdentifiers(m_partId0,m_index0,m_partId1,m_index1); output.addContactPoint( normalInB, diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index bccb0542370..c4842cd3023 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -43,12 +43,6 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface public: - //experimental feature information, per triangle, per convex etc. - //'material combiner' / contact added callback - int m_partId0; - int m_index0; - int m_partId1; - int m_index1; btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); virtual ~btGjkPairDetector() {}; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index fafceafa5ed..ee2be163063 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -18,7 +18,7 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include <assert.h> -float gContactBreakingTreshold = 0.02f; +float gContactBreakingThreshold = 0.02f; ContactDestroyedCallback gContactDestroyedCallback = 0; @@ -151,7 +151,7 @@ int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const { - btScalar shortestDist = getContactBreakingTreshold() * getContactBreakingTreshold(); + btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); int size = getNumContacts(); int nearestPoint = -1; for( int i = 0; i < size; i++ ) @@ -193,9 +193,9 @@ void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint) replaceContactPoint(newPoint,insertIndex); } -float btPersistentManifold::getContactBreakingTreshold() const +float btPersistentManifold::getContactBreakingThreshold() const { - return gContactBreakingTreshold; + return gContactBreakingThreshold; } void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB) @@ -229,7 +229,7 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1; projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint; distance2d = projectedDifference.dot(projectedDifference); - if (distance2d > getContactBreakingTreshold()*getContactBreakingTreshold() ) + if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() ) { removeContactPoint(i); } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index d0cc2577fb0..ab0e8767e5d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -23,8 +23,8 @@ subject to the following restrictions: struct btCollisionResult; -///contact breaking and merging treshold -extern float gContactBreakingTreshold; +///contact breaking and merging threshold +extern float gContactBreakingThreshold; typedef bool (*ContactDestroyedCallback)(void* userPersistentData); extern ContactDestroyedCallback gContactDestroyedCallback; @@ -97,7 +97,7 @@ public: } /// todo: get this margin from the current physics / collision environment - float getContactBreakingTreshold() const; + float getContactBreakingThreshold() const; int getCacheEntry(const btManifoldPoint& newPoint) const; @@ -124,7 +124,7 @@ public: bool validContactDistance(const btManifoldPoint& pt) const { - return pt.m_distance1 <= getContactBreakingTreshold(); + return pt.m_distance1 <= getContactBreakingThreshold(); } /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin void refreshContactPoints( const btTransform& trA,const btTransform& trB); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 9019035f586..429ad54d517 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -24,16 +24,7 @@ subject to the following restrictions: #define ASSERT2 assert -//some values to find stable tresholds - -float useGlobalSettingContacts = false;//true; -btScalar contactDamping = 0.2f; -btScalar contactTau = .02f;//0.02f;//*0.02f; - - - - - +#define USE_INTERNAL_APPLY_IMPULSE 1 //bilateral constraint between two dynamic objects @@ -75,7 +66,9 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, rel_vel = normal.dot(vel); - + + //todo: move this into proper structure + btScalar contactDamping = 0.2f; #ifdef ONLY_USE_LINEAR_MASS btScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); @@ -88,25 +81,17 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, - -//velocity + friction //response between two dynamic objects with friction float resolveSingleCollision( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, - const btContactSolverInfo& solverInfo - - ) + const btContactSolverInfo& solverInfo) { const btVector3& pos1 = contactPoint.getPositionWorldOnA(); const btVector3& pos2 = contactPoint.getPositionWorldOnB(); - - -// printf("distance=%f\n",distance); - - const btVector3& normal = contactPoint.m_normalWorldOnB; + const btVector3& normal = contactPoint.m_normalWorldOnB; btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); @@ -117,34 +102,18 @@ float resolveSingleCollision( btScalar rel_vel; rel_vel = normal.dot(vel); - btScalar Kfps = 1.f / solverInfo.m_timeStep ; float damping = solverInfo.m_damping ; float Kerp = solverInfo.m_erp; - - if (useGlobalSettingContacts) - { - damping = contactDamping; - Kerp = contactTau; - } - float Kcor = Kerp *Kfps; - //printf("dist=%f\n",distance); - - btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; + btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); - - btScalar distance = cpd->m_penetration;//contactPoint.getDistance(); - - - //distance = 0.f; + btScalar distance = cpd->m_penetration; btScalar positionalError = Kcor *-distance; - //jacDiagABInv; btScalar velocityError = cpd->m_restitution - rel_vel;// * damping; - btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv; btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv; @@ -158,9 +127,20 @@ float resolveSingleCollision( normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; +#ifdef USE_INTERNAL_APPLY_IMPULSE + if (body1.getInvMass()) + { + body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse); + } + if (body2.getInvMass()) + { + body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse); + } +#else USE_INTERNAL_APPLY_IMPULSE body1.applyImpulse(normal*(normalImpulse), rel_pos1); body2.applyImpulse(-normal*(normalImpulse), rel_pos2); - +#endif //USE_INTERNAL_APPLY_IMPULSE + return normalImpulse; } @@ -169,9 +149,86 @@ float resolveSingleFriction( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, - const btContactSolverInfo& solverInfo + const btContactSolverInfo& solverInfo) +{ + + const btVector3& pos1 = contactPoint.getPositionWorldOnA(); + const btVector3& pos2 = contactPoint.getPositionWorldOnB(); + + btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + + btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; + assert(cpd); + + float combinedFriction = cpd->m_friction; + + btScalar limit = cpd->m_appliedImpulse * combinedFriction; + //if (contactPoint.m_appliedImpulse>0.f) + //friction + { + //apply friction in the 2 tangential directions + + // 1st tangent + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + btScalar j1,j2; + + { + + btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel); + + // calculate j that moves us to zero relative velocity + j1 = -vrel * cpd->m_jacDiagABInvTangent0; + float total = cpd->m_accumulatedTangentImpulse0 + j1; + GEN_set_min(total, limit); + GEN_set_max(total, -limit); + j1 = total - cpd->m_accumulatedTangentImpulse0; + cpd->m_accumulatedTangentImpulse0 = total; + } + { + // 2nd tangent + + btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel); + + // calculate j that moves us to zero relative velocity + j2 = -vrel * cpd->m_jacDiagABInvTangent1; + float total = cpd->m_accumulatedTangentImpulse1 + j2; + GEN_set_min(total, limit); + GEN_set_max(total, -limit); + j2 = total - cpd->m_accumulatedTangentImpulse1; + cpd->m_accumulatedTangentImpulse1 = total; + } + +#ifdef USE_INTERNAL_APPLY_IMPULSE + if (body1.getInvMass()) + { + body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1); + body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2); + } + if (body2.getInvMass()) + { + body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1); + body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2); + } +#else USE_INTERNAL_APPLY_IMPULSE + body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1); + body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2); +#endif //USE_INTERNAL_APPLY_IMPULSE + + + } + return cpd->m_appliedImpulse; +} - ) + +float resolveSingleFrictionOriginal( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo) { const btVector3& pos1 = contactPoint.getPositionWorldOnA(); @@ -232,3 +289,110 @@ float resolveSingleFriction( } return cpd->m_appliedImpulse; } + + +//velocity + friction +//response between two dynamic objects with friction +float resolveSingleCollisionCombined( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo) +{ + + const btVector3& pos1 = contactPoint.getPositionWorldOnA(); + const btVector3& pos2 = contactPoint.getPositionWorldOnB(); + const btVector3& normal = contactPoint.m_normalWorldOnB; + + btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = normal.dot(vel); + + btScalar Kfps = 1.f / solverInfo.m_timeStep ; + + float damping = solverInfo.m_damping ; + float Kerp = solverInfo.m_erp; + float Kcor = Kerp *Kfps; + + btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; + assert(cpd); + btScalar distance = cpd->m_penetration; + btScalar positionalError = Kcor *-distance; + btScalar velocityError = cpd->m_restitution - rel_vel;// * damping; + + btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv; + + btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv; + + btScalar normalImpulse = penetrationImpulse+velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + float oldNormalImpulse = cpd->m_appliedImpulse; + float sum = oldNormalImpulse + normalImpulse; + cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum; + + normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; + + +#ifdef USE_INTERNAL_APPLY_IMPULSE + if (body1.getInvMass()) + { + body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse); + } + if (body2.getInvMass()) + { + body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse); + } +#else USE_INTERNAL_APPLY_IMPULSE + body1.applyImpulse(normal*(normalImpulse), rel_pos1); + body2.applyImpulse(-normal*(normalImpulse), rel_pos2); +#endif //USE_INTERNAL_APPLY_IMPULSE + + { + //friction + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + rel_vel = normal.dot(vel); + + + btVector3 lat_vel = vel - normal * rel_vel; + btScalar lat_rel_vel = lat_vel.length(); + + float combinedFriction = cpd->m_friction; + + if (cpd->m_appliedImpulse > 0) + if (lat_rel_vel > SIMD_EPSILON) + { + lat_vel /= lat_rel_vel; + btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel); + btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel); + btScalar friction_impulse = lat_rel_vel / + (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); + btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction; + + GEN_set_min(friction_impulse, normal_impulse); + GEN_set_max(friction_impulse, -normal_impulse); + body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1); + body2.applyImpulse(lat_vel * friction_impulse, rel_pos2); + } + } + + + + return normalImpulse; +} +float resolveSingleFrictionEmpty( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo) +{ + return 0.f; +};
\ No newline at end of file diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index 42ded30ae04..d88ba0d8ed4 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -72,6 +72,15 @@ struct btConstraintPersistentData float m_penetration; btVector3 m_frictionWorldTangential0; btVector3 m_frictionWorldTangential1; + + btVector3 m_frictionAngularComponent0A; + btVector3 m_frictionAngularComponent0B; + btVector3 m_frictionAngularComponent1A; + btVector3 m_frictionAngularComponent1B; + + //some data doesn't need to be persistent over frames: todo: clean/reuse this + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; ContactSolverFunc m_contactSolverFunc; ContactSolverFunc m_frictionSolverFunc; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index e747177a516..ac06f718c9e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -74,8 +74,6 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man for (j=0;j<numManifolds;j++) { int k=j; - //interleaving the preparation with solving impacts the behaviour a lot, todo: find out why - prepareConstraints(manifoldPtr[k],info,debugDrawer); solve(manifoldPtr[k],info,0,debugDrawer); } @@ -232,6 +230,7 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol cpd->m_penetration = 0.f; } + float relaxation = info.m_damping; cpd->m_appliedImpulse *= relaxation; @@ -266,6 +265,36 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol #endif //NO_FRICTION_WARMSTART cp.m_normalWorldOnB*cpd->m_appliedImpulse; + + + /// + { + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0; + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1; + } + { + btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0); + cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0; + } + { + btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1); + cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1; + } + { + btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0); + cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0; + } + { + btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1); + cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1; + } + + /// + + + //apply previous frames impulse on both bodies body0->applyImpulse(totalImpulse, rel_pos1); body1->applyImpulse(-totalImpulse, rel_pos2); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index feb1d2823f1..1bfc9b33348 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -127,15 +127,15 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() switch(colObj->GetActivationState()) { case ACTIVE_TAG: - color = btVector3(255.f,255.f,255.f); + color = btVector3(255.f,255.f,255.f); break; case ISLAND_SLEEPING: - color = btVector3(0.f,255.f,0.f); + color = btVector3(0.f,255.f,0.f);break; case WANTS_DEACTIVATION: - color = btVector3(0.f,255.f,255.f); + color = btVector3(0.f,255.f,255.f);break; case DISABLE_DEACTIVATION: - color = btVector3(255.f,0.f,0.f); + color = btVector3(255.f,0.f,0.f);break; case DISABLE_SIMULATION: - color = btVector3(255.f,255.f,0.f); + color = btVector3(255.f,255.f,0.f);break; default: { color = btVector3(255.f,0.f,0.f); @@ -150,7 +150,8 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() if (body->GetActivationState() != ISLAND_SLEEPING) { btTransform interpolatedTransform; - btTransformUtil::integrateTransform(body->m_interpolationWorldTransform,body->getLinearVelocity(),body->getAngularVelocity(),m_localTime,interpolatedTransform); + btTransformUtil::integrateTransform(body->m_interpolationWorldTransform, + body->m_interpolationLinearVelocity,body->m_interpolationAngularVelocity,m_localTime,interpolatedTransform); body->getMotionState()->setWorldTransform(interpolatedTransform); } } @@ -390,7 +391,7 @@ void btDiscreteDynamicsWorld::solveNoncontactConstraints(btContactSolverInfo& so BEGIN_PROFILE("solveNoncontactConstraints"); int i; - int numConstraints = m_constraints.size(); + int numConstraints = int(m_constraints.size()); ///constraint preparation: building jacobians for (i=0;i< numConstraints ; i++ ) @@ -424,7 +425,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() { int i; - int numConstraints = m_constraints.size(); + int numConstraints = int(m_constraints.size()); for (i=0;i< numConstraints ; i++ ) { btTypedConstraint* constraint = m_constraints[i]; @@ -502,7 +503,30 @@ void btDiscreteDynamicsWorld::updateAabbs() btPoint3 minAabb,maxAabb; colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb); btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache; - bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb); + + //moving objects should be moderately sized, probably something wrong if not + if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < 1e12f)) + { + bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb); + } else + { + //something went wrong, investigate + //this assert is unwanted in 3D modelers (danger of loosing work) + assert(0); + body->SetActivationState(DISABLE_SIMULATION); + + static bool reportMe = true; + if (reportMe) + { + reportMe = false; + printf("Overflow in AABB, object removed from simulation \n"); + printf("If you can reproduce this, please email bugs@continuousphysics.com\n"); + printf("Please include above information, your Platform, version of OS.\n"); + printf("Thanks.\n"); + } + + + } if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { DrawAabb(m_debugDrawer,minAabb,maxAabb,colorvec); @@ -670,10 +694,10 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, float radius = coneShape->getRadius();//+coneShape->getMargin(); float height = coneShape->getHeight();//+coneShape->getMargin(); btVector3 start = worldTransform.getOrigin(); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(radius,0,-0.5*height),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(-radius,0,-0.5*height),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(0,radius,-0.5*height),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(0,-radius,-0.5*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(radius,0.f,-0.5f*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(-radius,0.f,-0.5f*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(0.f,radius,-0.5f*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(0.f,-radius,-0.5f*height),color); break; } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 8a872264c22..fa916f65505 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -18,6 +18,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionWorld.h" class btTypedConstraint; +class btRaycastVehicle; ///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous class btDynamicsWorld : public btCollisionWorld @@ -47,6 +48,11 @@ class btDynamicsWorld : public btCollisionWorld virtual void removeConstraint(btTypedConstraint* constraint) {}; + virtual void addVehicle(btRaycastVehicle* vehicle) {}; + + virtual void removeVehicle(btRaycastVehicle* vehicle) {}; + + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) = 0; virtual btIDebugDraw* getDebugDrawer() = 0; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index 9e7c4978c31..a434f5e41dd 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -24,8 +24,8 @@ float gLinearAirDamping = 1.f; float gDeactivationTime = 2.f; bool gDisableDeactivation = false; -float gLinearSleepingTreshold = 0.8f; -float gAngularSleepingTreshold = 1.0f; +float gLinearSleepingThreshold = 0.8f; +float gAngularSleepingThreshold = 1.0f; static int uniqueId = 0; btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) @@ -45,7 +45,9 @@ btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShap motionState->getWorldTransform(m_worldTransform); m_interpolationWorldTransform = m_worldTransform; - + m_interpolationLinearVelocity.setValue(0,0,0); + m_interpolationAngularVelocity.setValue(0,0,0); + //moved to btCollisionObject m_friction = friction; m_restitution = restitution; @@ -79,6 +81,8 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi m_worldTransform = worldTransform; m_interpolationWorldTransform = m_worldTransform; + m_interpolationLinearVelocity.setValue(0,0,0); + m_interpolationAngularVelocity.setValue(0,0,0); //moved to btCollisionObject m_friction = friction; @@ -96,12 +100,32 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi } +#define EXPERIMENTAL_JITTER_REMOVAL 1 +#ifdef EXPERIMENTAL_JITTER_REMOVAL +//Bullet 2.20b has experimental code to reduce jitter just before objects fall asleep/deactivate +//doesn't work very well yet (value 0 only reduces performance a bit, no difference in functionality) +float gClippedAngvelThresholdSqr = 0.f; +float gClippedLinearThresholdSqr = 0.f; +#endif //EXPERIMENTAL_JITTER_REMOVAL +void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) +{ +#ifdef EXPERIMENTAL_JITTER_REMOVAL + //clip to avoid jitter + if (m_angularVelocity.length2() < gClippedAngvelThresholdSqr) + { + m_angularVelocity.setValue(0,0,0); + printf("clipped!\n"); + } + + if (m_linearVelocity.length2() < gClippedLinearThresholdSqr) + { + m_linearVelocity.setValue(0,0,0); + printf("clipped!\n"); + } +#endif //EXPERIMENTAL_JITTER_REMOVAL - -void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) const -{ btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); } @@ -114,7 +138,10 @@ void btRigidBody::saveKinematicState(btScalar timeStep) if (getMotionState()) getMotionState()->getWorldTransform(m_worldTransform); btVector3 linVel,angVel; + btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity); + m_interpolationLinearVelocity = m_linearVelocity; + m_interpolationAngularVelocity = m_angularVelocity; m_interpolationWorldTransform = m_worldTransform; //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ()); } @@ -257,6 +284,8 @@ btQuaternion btRigidBody::getOrientation() const void btRigidBody::setCenterOfMassTransform(const btTransform& xform) { m_interpolationWorldTransform = m_worldTransform; + m_interpolationLinearVelocity = getLinearVelocity(); + m_interpolationAngularVelocity = getAngularVelocity(); m_worldTransform = xform; updateInertiaTensor(); } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index c6d3bd50a35..fca3658cb31 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -34,8 +34,8 @@ extern bool gUseEpa; extern float gDeactivationTime; extern bool gDisableDeactivation; -extern float gLinearSleepingTreshold; -extern float gAngularSleepingTreshold; +extern float gLinearSleepingThreshold; +extern float gAngularSleepingThreshold; /// btRigidBody class for btRigidBody Dynamics @@ -79,7 +79,7 @@ public: } /// continuous collision detection needs prediction - void predictIntegratedTransform(btScalar step, btTransform& predictedTransform) const; + void predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ; void saveKinematicState(btScalar step); @@ -158,6 +158,16 @@ public: applyTorqueImpulse(rel_pos.cross(impulse)); } } + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,float impulseMagnitude) + { + if (m_inverseMass != 0.f) + { + m_linearVelocity += linearComponent*impulseMagnitude; + m_angularVelocity += angularComponent*impulseMagnitude; + } + } void clearForces() { @@ -240,8 +250,8 @@ public: if ( (GetActivationState() == ISLAND_SLEEPING) || (GetActivationState() == DISABLE_DEACTIVATION)) return; - if ((getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) && - (getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold)) + if ((getLinearVelocity().length2() < gLinearSleepingThreshold*gLinearSleepingThreshold) && + (getAngularVelocity().length2() < gAngularSleepingThreshold*gAngularSleepingThreshold)) { m_deactivationTime += timeStep; } else diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index 03841f99a70..7a2043e2b38 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -9,11 +9,12 @@ * It is provided "as is" without express or implied warranty. */ +#include "LinearMath/btVector3.h" #include "btRaycastVehicle.h" #include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h" #include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" #include "LinearMath/btQuaternion.h" -#include "LinearMath/btVector3.h" +#include "BulletDynamics/Dynamics/btDynamicsWorld.h" #include "btVehicleRaycaster.h" #include "btWheelInfo.h" @@ -141,8 +142,12 @@ void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel ) { wheel.m_raycastInfo.m_isInContact = false; - const btTransform& chassisTrans = getRigidBody()->getCenterOfMassTransform(); - + btTransform chassisTrans; + if (getRigidBody()->getMotionState()) + getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); + else + chassisTrans = getRigidBody()->getCenterOfMassTransform(); + wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS ); wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ; wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS; @@ -166,6 +171,8 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) btVehicleRaycaster::btVehicleRaycasterResult rayResults; + assert(m_vehicleRaycaster); + void* object = m_vehicleRaycaster->castRay(source,target,rayResults); wheel.m_raycastInfo.m_groundObject = 0; @@ -593,3 +600,28 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) delete[]forwardImpulse; delete[] sideImpulse; } + + +void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) +{ +// RayResultCallback& resultCallback; + + btCollisionWorld::ClosestRayResultCallback rayCallback(from,to); + + m_dynamicsWorld->rayTest(from, to, rayCallback); + + if (rayCallback.HasHit()) + { + + btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject); + if (body) + { + result.m_hitPointInWorld = rayCallback.m_hitPointWorld; + result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld; + result.m_hitNormalInWorld.normalize(); + result.m_distFraction = rayCallback.m_closestHitFraction; + return body; + } + } + return 0; +} diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h index 8468bc52016..a7534c67555 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -13,11 +13,11 @@ #include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" - +#include "btVehicleRaycaster.h" +class btDynamicsWorld; #include "btWheelInfo.h" -struct btVehicleRaycaster; class btVehicleTuning; ///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle. @@ -93,7 +93,7 @@ public: btWheelInfo& addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel); inline int getNumWheels() const { - return m_wheelInfo.size(); + return int (m_wheelInfo.size()); } std::vector<btWheelInfo> m_wheelInfo; @@ -163,5 +163,19 @@ public: }; +class btDefaultVehicleRaycaster : public btVehicleRaycaster +{ + btDynamicsWorld* m_dynamicsWorld; +public: + btDefaultVehicleRaycaster(btDynamicsWorld* world) + :m_dynamicsWorld(world) + { + } + + virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result); + +}; + + #endif //RAYCASTVEHICLE_H diff --git a/extern/bullet2/src/LinearMath/btDefaultMotionState.h b/extern/bullet2/src/LinearMath/btDefaultMotionState.h index c64f1352e10..805631ac56f 100644 --- a/extern/bullet2/src/LinearMath/btDefaultMotionState.h +++ b/extern/bullet2/src/LinearMath/btDefaultMotionState.h @@ -9,7 +9,7 @@ struct btDefaultMotionState : public btMotionState btTransform m_startWorldTrans; void* m_userPointer; - btDefaultMotionState(const btTransform& startTrans,const btTransform& centerOfMassOffset = btTransform::getIdentity()) + btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity()) : m_graphicsWorldTrans(startTrans), m_centerOfMassOffset(centerOfMassOffset), m_startWorldTrans(startTrans), @@ -31,4 +31,4 @@ struct btDefaultMotionState : public btMotionState } }; -#endif //DEFAULT_MOTION_STATE_H
\ No newline at end of file +#endif //DEFAULT_MOTION_STATE_H diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h index da8e4aa72a8..12ca634c232 100644 --- a/extern/bullet2/src/LinearMath/btTransformUtil.h +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -17,7 +17,7 @@ subject to the following restrictions: #define SIMD_TRANSFORM_UTIL_H #include "LinearMath/btTransform.h" -#define ANGULAR_MOTION_TRESHOLD 0.5f*SIMD_HALF_PI +#define ANGULAR_MOTION_THRESHOLD 0.5f*SIMD_HALF_PI @@ -82,9 +82,9 @@ public: btVector3 axis; btScalar fAngle = angvel.length(); //limit the angular motion - if (fAngle*timeStep > ANGULAR_MOTION_TRESHOLD) + if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD) { - fAngle = ANGULAR_MOTION_TRESHOLD / timeStep; + fAngle = ANGULAR_MOTION_THRESHOLD / timeStep; } if ( fAngle < 0.001f ) diff --git a/extern/bullet2/src/btBulletCollisionCommon.h b/extern/bullet2/src/btBulletCollisionCommon.h index 6ee7941dfcf..834dca09e47 100644 --- a/extern/bullet2/src/btBulletCollisionCommon.h +++ b/extern/bullet2/src/btBulletCollisionCommon.h @@ -49,11 +49,12 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btAxisSweep3.h" -///Math library +///Math library & Utils #include "LinearMath/btQuaternion.h" #include "LinearMath/btTransform.h" - - +#include "LinearMath/btDefaultMotionState.h" +#include "LinearMath/btQuickprof.h" +#include "LinearMath/btIDebugDraw.h" #endif //BULLET_COLLISION_COMMON_H diff --git a/extern/bullet2/src/btBulletDynamicsCommon.h b/extern/bullet2/src/btBulletDynamicsCommon.h index 051ee8ce0bf..89211d230fd 100644 --- a/extern/bullet2/src/btBulletDynamicsCommon.h +++ b/extern/bullet2/src/btBulletDynamicsCommon.h @@ -34,5 +34,8 @@ subject to the following restrictions: + + + #endif //BULLET_DYNAMICS_COMMON_H |