diff options
Diffstat (limited to 'extern/bullet/Bullet')
20 files changed, 365 insertions, 27 deletions
diff --git a/extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.cpp b/extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.cpp index 000db0169b7..f86ad9a7698 100644 --- a/extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.cpp +++ b/extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.cpp @@ -71,14 +71,17 @@ AxisSweep3::AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAab // handle 0 is reserved as the null index, and is also used as the sentinel m_firstFreeHandle = 1; - for (int i = m_firstFreeHandle; i < maxHandles; i++) - m_pHandles[i].SetNextFree(i + 1); - m_pHandles[maxHandles - 1].SetNextFree(0); + { + for (int i = m_firstFreeHandle; i < maxHandles; i++) + m_pHandles[i].SetNextFree(i + 1); + m_pHandles[maxHandles - 1].SetNextFree(0); + } + { // allocate edge buffers for (int i = 0; i < 3; i++) m_pEdges[i] = new Edge[maxHandles * 2]; - + } //removed overlap management // make boundary sentinels diff --git a/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h b/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h index ed556c2e60a..579731569f8 100644 --- a/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h +++ b/extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h @@ -41,7 +41,8 @@ struct DispatcherInfo :m_dispatchFunc(DISPATCH_DISCRETE), m_timeOfImpact(1.f), m_useContinuous(false), - m_debugDraw(0) + m_debugDraw(0), + m_enableSatConvex(false) { } @@ -51,6 +52,7 @@ struct DispatcherInfo float m_timeOfImpact; bool m_useContinuous; class IDebugDraw* m_debugDraw; + bool m_enableSatConvex; }; diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp index 49037eb8675..c138d5a140e 100644 --- a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp @@ -38,9 +38,26 @@ subject to the following restrictions: #include "CollisionShapes/SphereShape.h" #include "NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h" -#ifdef USE_EPA + #include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h" -#endif + +#ifdef WIN32 +#if _MSC_VER >= 1310 +//only use SIMD Hull code under Win32 +#ifdef TEST_HULL +#define USE_HULL 1 +#endif //TEST_HULL +#endif //_MSC_VER +#endif //WIN32 + + +#ifdef USE_HULL + +#include "NarrowPhaseCollision/Hull.h" +#include "NarrowPhaseCollision/HullContactCollector.h" + + +#endif //USE_HULL bool gUseEpa = false; @@ -134,11 +151,10 @@ public: static MinkowskiPenetrationDepthSolver gPenetrationDepthSolver; +static EpaPenetrationDepthSolver gEpaPenetrationDepthSolver; #ifdef USE_EPA Solid3EpaPenetrationDepth gSolidEpaPenetrationSolver; -static EpaPenetrationDepthSolver gEpaPenetrationDepthSolver; - #endif //USE_EPA void ConvexConvexAlgorithm::CheckPenetrationDepthSolver() @@ -148,10 +164,10 @@ void ConvexConvexAlgorithm::CheckPenetrationDepthSolver() m_useEpa = gUseEpa; if (m_useEpa) { -#ifdef USE_EPA + m_gjkPairDetector.SetPenetrationDepthSolver(&gEpaPenetrationDepthSolver); -#endif + } else { m_gjkPairDetector.SetPenetrationDepthSolver(&gPenetrationDepthSolver); @@ -160,6 +176,70 @@ void ConvexConvexAlgorithm::CheckPenetrationDepthSolver() } +#ifdef USE_HULL + +Transform GetTransformFromSimdTransform(const SimdTransform& trans) +{ + //const SimdVector3& rowA0 = trans.getBasis().getRow(0); + ////const SimdVector3& rowA1 = trans.getBasis().getRow(1); + //const SimdVector3& rowA2 = trans.getBasis().getRow(2); + + SimdVector3 rowA0 = trans.getBasis().getColumn(0); + SimdVector3 rowA1 = trans.getBasis().getColumn(1); + SimdVector3 rowA2 = trans.getBasis().getColumn(2); + + + Vector3 x(rowA0.getX(),rowA0.getY(),rowA0.getZ()); + Vector3 y(rowA1.getX(),rowA1.getY(),rowA1.getZ()); + Vector3 z(rowA2.getX(),rowA2.getY(),rowA2.getZ()); + + Matrix33 ornA(x,y,z); + + Point3 transA( + trans.getOrigin().getX(), + trans.getOrigin().getY(), + trans.getOrigin().getZ()); + + return Transform(ornA,transA); +} + +class ManifoldResultCollector : public HullContactCollector +{ +public: + ManifoldResult& m_manifoldResult; + + ManifoldResultCollector(ManifoldResult& manifoldResult) + :m_manifoldResult(manifoldResult) + { + + } + + + virtual ~ManifoldResultCollector() {}; + + virtual int BatchAddContactGroup(const Separation& sep,int numContacts,const Vector3& normalWorld,const Vector3& tangent,const Point3* positionsWorld,const float* depths) + { + for (int i=0;i<numContacts;i++) + { + //printf("numContacts = %i\n",numContacts); + SimdVector3 normalOnBInWorld(sep.m_axis.GetX(),sep.m_axis.GetY(),sep.m_axis.GetZ()); + //normalOnBInWorld.normalize(); + SimdVector3 pointInWorld(positionsWorld[i].GetX(),positionsWorld[i].GetY(),positionsWorld[i].GetZ()); + float depth = -depths[i]; + m_manifoldResult.AddContactPoint(normalOnBInWorld,pointInWorld,depth); + + } + return 0; + } + + virtual int GetMaxNumContacts() const + { + return 4; + } + +}; +#endif //USE_HULL + // // box-box collision algorithm, for simplicity also applies resolution-impulse // @@ -179,6 +259,53 @@ void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy CollisionObject* col0 = static_cast<CollisionObject*>(m_box0.m_clientObject); CollisionObject* col1 = static_cast<CollisionObject*>(m_box1.m_clientObject); + +#ifdef USE_HULL + + + if (dispatchInfo.m_enableSatConvex) + { + if ((col0->m_collisionShape->IsPolyhedral()) && + (col1->m_collisionShape->IsPolyhedral())) + { + + + PolyhedralConvexShape* polyhedron0 = static_cast<PolyhedralConvexShape*>(col0->m_collisionShape); + PolyhedralConvexShape* polyhedron1 = static_cast<PolyhedralConvexShape*>(col1->m_collisionShape); + if (polyhedron0->m_optionalHull && polyhedron1->m_optionalHull) + { + //printf("Hull-Hull"); + + //todo: cache this information, rather then initialize + Separation sep; + sep.m_featureA = 0; + sep.m_featureB = 0; + sep.m_contact = -1; + sep.m_separator = 0; + + //convert from SimdTransform to Transform + + Transform trA = GetTransformFromSimdTransform(col0->m_worldTransform); + Transform trB = GetTransformFromSimdTransform(col1->m_worldTransform); + + //either use persistent manifold or clear it every time + m_manifoldPtr->ClearManifold(); + ManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr); + + ManifoldResultCollector hullContactCollector(*resultOut); + + Hull::ProcessHullHull(sep,*polyhedron0->m_optionalHull,*polyhedron1->m_optionalHull, + trA,trB,&hullContactCollector); + + + //user provided hull's, so we use SAT Hull collision detection + return; + } + } + } + +#endif //USE_HULL + ManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr); diff --git a/extern/bullet/Bullet/CollisionShapes/BoxShape.h b/extern/bullet/Bullet/CollisionShapes/BoxShape.h index 4569474d795..f7c5bba7345 100644 --- a/extern/bullet/Bullet/CollisionShapes/BoxShape.h +++ b/extern/bullet/Bullet/CollisionShapes/BoxShape.h @@ -69,8 +69,7 @@ public: return supVertex; } - - virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const + virtual inline SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const { SimdVector3 halfExtents = GetHalfExtents(); SimdVector3 margin(GetMargin(),GetMargin(),GetMargin()); @@ -81,6 +80,23 @@ public: vec.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); } + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const + { + SimdVector3 halfExtents = GetHalfExtents(); + SimdVector3 margin(GetMargin(),GetMargin(),GetMargin()); + halfExtents -= margin; + + + for (int i=0;i<numVectors;i++) + { + const SimdVector3& vec = vectors[i]; + supportVerticesOut[i].setValue(vec.x() < SimdScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), + vec.y() < SimdScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), + vec.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + } + + } + BoxShape( const SimdVector3& boxHalfExtents) : m_boxHalfExtents1(boxHalfExtents){}; diff --git a/extern/bullet/Bullet/CollisionShapes/ConeShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConeShape.cpp index baf94d16b70..9543aa48139 100644 --- a/extern/bullet/Bullet/CollisionShapes/ConeShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/ConeShape.cpp @@ -71,6 +71,16 @@ SimdVector3 ConeShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& return ConeLocalSupport(vec); } +void ConeShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + for (int i=0;i<numVectors;i++) + { + const SimdVector3& vec = vectors[i]; + supportVerticesOut[i] = ConeLocalSupport(vec); + } +} + + SimdVector3 ConeShape::LocalGetSupportingVertex(const SimdVector3& vec) const { SimdVector3 supVertex = ConeLocalSupport(vec); diff --git a/extern/bullet/Bullet/CollisionShapes/ConeShape.h b/extern/bullet/Bullet/CollisionShapes/ConeShape.h index 418f9538b1a..b7408df13cc 100644 --- a/extern/bullet/Bullet/CollisionShapes/ConeShape.h +++ b/extern/bullet/Bullet/CollisionShapes/ConeShape.h @@ -36,6 +36,7 @@ public: virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec) const; virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec) const; + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const; float GetRadius() const { return m_radius;} float GetHeight() const { return m_height;} diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp index 2fcf76ffdc3..5de2ea3dcb8 100644 --- a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp @@ -56,6 +56,39 @@ SimdVector3 ConvexHullShape::LocalGetSupportingVertexWithoutMargin(const SimdVec return supVec; } +void ConvexHullShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + SimdScalar newDot; + //use 'w' component of supportVerticesOut? + { + for (int i=0;i<numVectors;i++) + { + supportVerticesOut[i][3] = -1e30f; + } + } + for (size_t i=0;i<m_points.size();i++) + { + SimdPoint3 vtx = m_points[i] * m_localScaling; + + for (int j=0;j<numVectors;j++) + { + const SimdVector3& vec = vectors[j]; + + newDot = vec.dot(vtx); + if (newDot > supportVerticesOut[j][3]) + { + //WARNING: don't swap next lines, the w component would get overwritten! + supportVerticesOut[j] = vtx; + supportVerticesOut[j][3] = newDot; + } + } + } + + + +} + + SimdVector3 ConvexHullShape::LocalGetSupportingVertex(const SimdVector3& vec)const { diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.h b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.h index 41effc645c5..cbcc5687a28 100644 --- a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.h +++ b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.h @@ -38,7 +38,8 @@ public: } virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec)const; virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const; - + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const; + virtual int GetShapeType()const { return CONVEX_HULL_SHAPE_PROXYTYPE; } diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexShape.h b/extern/bullet/Bullet/CollisionShapes/ConvexShape.h index ad4aefc128c..8961f451755 100644 --- a/extern/bullet/Bullet/CollisionShapes/ConvexShape.h +++ b/extern/bullet/Bullet/CollisionShapes/ConvexShape.h @@ -40,6 +40,9 @@ public: virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec)const; virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec) const= 0; + + //notice that the vectors should be unit length + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const= 0; // testing for hullnode code diff --git a/extern/bullet/Bullet/CollisionShapes/MinkowskiSumShape.cpp b/extern/bullet/Bullet/CollisionShapes/MinkowskiSumShape.cpp index b5ca6a9fab1..9bd6f9c350c 100644 --- a/extern/bullet/Bullet/CollisionShapes/MinkowskiSumShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/MinkowskiSumShape.cpp @@ -31,6 +31,18 @@ SimdVector3 MinkowskiSumShape::LocalGetSupportingVertexWithoutMargin(const SimdV return supVertexA + supVertexB; } +void MinkowskiSumShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + //todo: could make recursive use of batching. probably this shape is not used frequently. + for (int i=0;i<numVectors;i++) + { + supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]); + } + +} + + + float MinkowskiSumShape::GetMargin() const { return m_shapeA->GetMargin() + m_shapeB->GetMargin(); diff --git a/extern/bullet/Bullet/CollisionShapes/MinkowskiSumShape.h b/extern/bullet/Bullet/CollisionShapes/MinkowskiSumShape.h index d956ad4323e..69dee7c0382 100644 --- a/extern/bullet/Bullet/CollisionShapes/MinkowskiSumShape.h +++ b/extern/bullet/Bullet/CollisionShapes/MinkowskiSumShape.h @@ -34,6 +34,9 @@ public: virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const; + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const; + + virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia); void SetTransformA(const SimdTransform& transA) { m_transA = transA;} diff --git a/extern/bullet/Bullet/CollisionShapes/MultiSphereShape.cpp b/extern/bullet/Bullet/CollisionShapes/MultiSphereShape.cpp index e4e93774c2a..83d1a72d67a 100644 --- a/extern/bullet/Bullet/CollisionShapes/MultiSphereShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/MultiSphereShape.cpp @@ -79,7 +79,35 @@ MultiSphereShape::MultiSphereShape (const SimdVector3& inertiaHalfExtents,const } + void MultiSphereShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + + for (int j=0;j<numVectors;j++) + { + SimdScalar maxDot(-1e30f); + + const SimdVector3& vec = vectors[j]; + + SimdVector3 vtx; + SimdScalar newDot; + const SimdVector3* pos = &m_localPositions[0]; + const SimdScalar* rad = &m_radi[0]; + + for (int i=0;i<m_numSpheres;i++) + { + vtx = (*pos) +vec*((*rad)-m_minRadius); + pos++; + rad++; + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = vtx; + } + } + } +} diff --git a/extern/bullet/Bullet/CollisionShapes/MultiSphereShape.h b/extern/bullet/Bullet/CollisionShapes/MultiSphereShape.h index d0cf2ad3f0b..19c9d978f48 100644 --- a/extern/bullet/Bullet/CollisionShapes/MultiSphereShape.h +++ b/extern/bullet/Bullet/CollisionShapes/MultiSphereShape.h @@ -46,6 +46,8 @@ public: /// ConvexShape Interface virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const; + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const; + virtual int GetShapeType() const { return MULTI_SPHERE_SHAPE_PROXYTYPE; } diff --git a/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.cpp b/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.cpp index 89bd5af59ce..bfdf7501451 100644 --- a/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.cpp @@ -16,10 +16,13 @@ subject to the following restrictions: #include <CollisionShapes/PolyhedralConvexShape.h> PolyhedralConvexShape::PolyhedralConvexShape() - +:m_optionalHull(0) { + } + + SimdVector3 PolyhedralConvexShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const { int i; @@ -56,6 +59,34 @@ SimdVector3 PolyhedralConvexShape::LocalGetSupportingVertexWithoutMargin(const S } +void PolyhedralConvexShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + int i; + + SimdVector3 vtx; + SimdScalar newDot; + + for (int j=0;j<numVectors;j++) + { + SimdScalar maxDot(-1e30f); + + const SimdVector3& vec = vectors[j]; + + for (i=0;i<GetNumVertices();i++) + { + GetVertex(i,vtx); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supportVerticesOut[i] = vtx; + } + } + } +} + + + void PolyhedralConvexShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) { //not yet, return box inertia diff --git a/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.h b/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.h index 7b0c54d96cc..358bf71d0d9 100644 --- a/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.h +++ b/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.h @@ -31,8 +31,9 @@ public: //brute force implementations virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const; - - virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia); + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const; + + virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia); @@ -46,6 +47,9 @@ public: virtual bool IsInside(const SimdPoint3& pt,SimdScalar tolerance) const = 0; + /// optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp + class Hull* m_optionalHull; + }; #endif //BU_SHAPE diff --git a/extern/bullet/Bullet/CollisionShapes/SphereShape.cpp b/extern/bullet/Bullet/CollisionShapes/SphereShape.cpp index 047a22aabe9..6cc9fd09887 100644 --- a/extern/bullet/Bullet/CollisionShapes/SphereShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/SphereShape.cpp @@ -29,6 +29,15 @@ SimdVector3 SphereShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3 return SimdVector3(0.f,0.f,0.f); } +void SphereShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + for (int i=0;i<numVectors;i++) + { + supportVerticesOut[i].setValue(0.f,0.f,0.f); + } +} + + SimdVector3 SphereShape::LocalGetSupportingVertex(const SimdVector3& vec)const { SimdVector3 supVertex; diff --git a/extern/bullet/Bullet/CollisionShapes/SphereShape.h b/extern/bullet/Bullet/CollisionShapes/SphereShape.h index 5e490ca1402..d93174c7bd7 100644 --- a/extern/bullet/Bullet/CollisionShapes/SphereShape.h +++ b/extern/bullet/Bullet/CollisionShapes/SphereShape.h @@ -31,6 +31,8 @@ public: virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec)const; virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const; + //notice that the vectors should be unit length + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const; virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia); diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleShape.h b/extern/bullet/Bullet/CollisionShapes/TriangleShape.h index 6f7fb195e8c..d1b4a993030 100644 --- a/extern/bullet/Bullet/CollisionShapes/TriangleShape.h +++ b/extern/bullet/Bullet/CollisionShapes/TriangleShape.h @@ -70,6 +70,18 @@ public: } + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const + { + for (int i=0;i<numVectors;i++) + { + const SimdVector3& dir = vectors[i]; + SimdVector3 dots(dir.dot(m_vertices1[0]), dir.dot(m_vertices1[1]), dir.dot(m_vertices1[2])); + supportVerticesOut[i] = m_vertices1[dots.maxAxis()]; + } + + } + + TriangleShape(const SimdVector3& p0,const SimdVector3& p1,const SimdVector3& p2) { diff --git a/extern/bullet/Bullet/Doxyfile b/extern/bullet/Bullet/Doxyfile index 94d1d3c4b27..4ecb6acb62f 100644 --- a/extern/bullet/Bullet/Doxyfile +++ b/extern/bullet/Bullet/Doxyfile @@ -15,7 +15,7 @@ # The PROJECT_NAME tag is a single word (or a sequence of words surrounded # by quotes) that should identify the project. -PROJECT_NAME = Continuous Collision Detection Library +PROJECT_NAME = "Bullet Continuous Collision Detection Library" # The PROJECT_NUMBER tag can be used to enter a project or revision number. # This could be handy for archiving the generated documentation or diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp index 15ff6c788fe..a5dabd4232d 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp @@ -105,32 +105,68 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl SimdVector3 seperatingAxisInA,seperatingAxisInB; SimdVector3 pInA,qInB,pWorld,qWorld,w; +#define USE_BATCHED_SUPPORT 1 +#ifdef USE_BATCHED_SUPPORT + SimdVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS]; + SimdVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS]; + SimdVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS]; + SimdVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS]; + int i; + + for (i=0;i<NUM_UNITSPHERE_POINTS;i++) + { + const SimdVector3& norm = sPenetrationDirections[i]; + seperatingAxisInABatch[i] = (-norm)* transA.getBasis(); + seperatingAxisInBBatch[i] = norm * transB.getBasis(); + } + + convexA->BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,NUM_UNITSPHERE_POINTS); + convexB->BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,NUM_UNITSPHERE_POINTS); + for (i=0;i<NUM_UNITSPHERE_POINTS;i++) + { + const SimdVector3& norm = sPenetrationDirections[i]; + seperatingAxisInA = seperatingAxisInABatch[i]; + seperatingAxisInB = seperatingAxisInBBatch[i]; + + pInA = supportVerticesABatch[i]; + qInB = supportVerticesBBatch[i]; + + pWorld = transA(pInA); + qWorld = transB(qInB); + w = qWorld - pWorld; + float delta = norm.dot(w); + //find smallest delta + if (delta < minProj) + { + minProj = delta; + minNorm = norm; + minA = pWorld; + minB = qWorld; + } + } +#else for (int i=0;i<NUM_UNITSPHERE_POINTS;i++) { const SimdVector3& norm = sPenetrationDirections[i]; - seperatingAxisInA = (-norm)* transA.getBasis(); seperatingAxisInB = norm* transB.getBasis(); - pInA = convexA->LocalGetSupportingVertexWithoutMargin(seperatingAxisInA); qInB = convexB->LocalGetSupportingVertexWithoutMargin(seperatingAxisInB); pWorld = transA(pInA); qWorld = transB(qInB); - w = qWorld - pWorld; float delta = norm.dot(w); //find smallest delta - if (delta < minProj) { minProj = delta; minNorm = norm; minA = pWorld; minB = qWorld; - } } - +#endif //USE_BATCHED_SUPPORT + //add the margins minA += minNorm*convexA->GetMargin(); @@ -158,7 +194,7 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl GjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0); - SimdScalar offsetDist = (minProj+0.1f); + SimdScalar offsetDist = minProj; SimdVector3 offset = minNorm * offsetDist; @@ -177,14 +213,17 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl MyResult res; gjkdet.GetClosestPoints(input,res,debugDraw); + float correctedMinNorm = minProj - res.m_depth; + + //the penetration depth is over-estimated, relax it - float penetration_relaxation= 0.1f; + float penetration_relaxation= 1.f; minNorm*=penetration_relaxation; if (res.m_hasResult) { - pa = res.m_pointInWorld - minNorm * minProj; + pa = res.m_pointInWorld - minNorm * correctedMinNorm; pb = res.m_pointInWorld; #ifdef DEBUG_DRAW |