From a6d9cf382f02addfa6dab75a34645dc9c8476816 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 30 Aug 2013 16:30:22 -0700 Subject: [PATCH] more work towards cpu pipeline, sharing OpenCL kernel code --- Demos3/CpuDemos/rigidbody/RigidBodyDemo.cpp | 20 +- Demos3/CpuDemos/rigidbody/RigidBodyDemo.h | 3 +- build3/premake4.lua | 2 +- .../b3DynamicBvhBroadphase.cpp | 7 +- .../b3DynamicBvhBroadphase.h | 2 +- .../BroadPhaseCollision/b3OverlappingPair.h | 41 +- .../b3OverlappingPairCache.cpp | 12 +- .../b3CpuCollisionWorld.cpp | 46 -- .../b3CpuCollisionWorld.h | 28 - .../NarrowPhaseCollision/b3CpuNarrowPhase.cpp | 111 +++- .../NarrowPhaseCollision/b3CpuNarrowPhase.h | 5 +- .../shared/b3ContactConvexConvexSAT.h | 523 ++++++++++++++++++ .../shared/b3ContactSphereSphere.h | 162 ++++++ .../shared/b3FindSeparatingAxis.h | 206 +++++++ .../shared/b3ReduceContacts.h | 97 ++++ .../shared/b3UpdateAabbs.h | 31 ++ src/Bullet3Common/shared/b3Float4.h | 18 + .../b3CpuRigidBodyPipeline.cpp | 60 +- .../RigidBody/b3GpuRigidBodyPipeline.cpp | 3 +- 19 files changed, 1247 insertions(+), 130 deletions(-) delete mode 100644 src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.cpp delete mode 100644 src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.h create mode 100644 src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h create mode 100644 src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h create mode 100644 src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h create mode 100644 src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h create mode 100644 src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h diff --git a/Demos3/CpuDemos/rigidbody/RigidBodyDemo.cpp b/Demos3/CpuDemos/rigidbody/RigidBodyDemo.cpp index e41ee733a..d158d325d 100644 --- a/Demos3/CpuDemos/rigidbody/RigidBodyDemo.cpp +++ b/Demos3/CpuDemos/rigidbody/RigidBodyDemo.cpp @@ -9,7 +9,6 @@ #include "Bullet3Dynamics/shared/b3IntegrateTransforms.h" #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.h" static b3Vector4 colors[4] = { @@ -25,17 +24,16 @@ void RigidBodyDemo::initPhysics(const ConstructionInfo& ci) { m_instancingRenderer = ci.m_instancingRenderer; - int x_dim=30; - int y_dim=30; - int z_dim=30; + int x_dim=1; + int y_dim=2; + int z_dim=1; - int aabbCapacity = x_dim*y_dim*z_dim+1; + int aabbCapacity = x_dim*y_dim*z_dim+10; b3Config config; m_bp = new b3DynamicBvhBroadphase(aabbCapacity); m_np = new b3CpuNarrowPhase(config); - m_cd = new b3CpuCollisionWorld(m_bp,m_np); m_rb = new b3CpuRigidBodyPipeline(m_np,m_bp, config); @@ -69,7 +67,6 @@ void RigidBodyDemo::initPhysics(const ConstructionInfo& ci) float mass=0.f; int collidableIndex = m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); int bodyIndex = m_rb->getNumBodies(); - m_cd->addCollidable(bodyIndex,collidableIndex,position,orn); int userData=-1; @@ -82,19 +79,19 @@ void RigidBodyDemo::initPhysics(const ConstructionInfo& ci) b3Vector4 scaling=b3MakeVector4(1,1,1,1); int collidableIndex = m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); - for (int x=-x_dim/2;xregisterGraphicsInstance(shapeId,position,orn,color,scaling); @@ -102,7 +99,6 @@ void RigidBodyDemo::initPhysics(const ConstructionInfo& ci) int userData=-1; int bodyIndex = m_rb->getNumBodies(); - m_cd->addCollidable(bodyIndex,collidableIndex,position,orn); int rbid = m_rb->registerPhysicsInstance(mass, position, orn, collidableIndex, userData); } diff --git a/Demos3/CpuDemos/rigidbody/RigidBodyDemo.h b/Demos3/CpuDemos/rigidbody/RigidBodyDemo.h index d05b53f3c..3bf33ee44 100644 --- a/Demos3/CpuDemos/rigidbody/RigidBodyDemo.h +++ b/Demos3/CpuDemos/rigidbody/RigidBodyDemo.h @@ -8,8 +8,7 @@ struct RigidBodyDemo : public CpuDemo struct b3DynamicBvhBroadphase* m_bp; struct b3CpuNarrowPhase* m_np; - class b3CpuCollisionWorld* m_cd; - + struct b3CpuRigidBodyPipeline* m_rb; GLInstancingRenderer* m_instancingRenderer; diff --git a/build3/premake4.lua b/build3/premake4.lua index 0bd34d1d0..2147ee78e 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -101,7 +101,7 @@ if not _OPTIONS["ios"] then - include "../Demos3/CpuDemos" +-- include "../Demos3/CpuDemos" include "../Demos3/GpuDemos" include "../Demos3/Wavefront" include "../btgui/MultiThreading" diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp index 97c5190d9..ca08429a3 100644 --- a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp +++ b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp @@ -289,12 +289,13 @@ void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin,const b3Vector3& // -void b3DynamicBvhBroadphase::setAabb( b3BroadphaseProxy* absproxy, +void b3DynamicBvhBroadphase::setAabb(int objectId, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* /*dispatcher*/) { - b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy; + b3DbvtProxy* proxy=&m_proxies[objectId]; +// b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy; B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax); #if B3_DBVT_BP_PREVENTFALSEUPDATE if(b3NotEqual(aabb,proxy->leaf->volume)) @@ -440,7 +441,7 @@ void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher) int i; - b3BroadphasePair previousPair(-1,-1); + b3BroadphasePair previousPair = b3MakeBroadphasePair(-1,-1); diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h index 767ee72bf..74e6ef047 100644 --- a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h +++ b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h @@ -167,7 +167,7 @@ struct b3DynamicBvhBroadphase /* b3BroadphaseInterface Implementation */ b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int objectIndex,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask); virtual void destroyProxy(b3BroadphaseProxy* proxy,b3Dispatcher* dispatcher); - virtual void setAabb(b3BroadphaseProxy* proxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher); + virtual void setAabb(int objectId,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher); virtual void rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3MakeVector3(0,0,0), const b3Vector3& aabbMax = b3MakeVector3(0,0,0)); virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback); diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h index 611e183bc..39bf27de3 100644 --- a/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h +++ b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h @@ -21,26 +21,33 @@ subject to the following restrictions: #define B3_NEW_PAIR_MARKER -1 #define B3_REMOVED_PAIR_MARKER -2 -//typedef b3Int2 b3BroadphasePair; -struct b3BroadphasePair : public b3Int4 +typedef b3Int4 b3BroadphasePair; + +inline b3Int4 b3MakeBroadphasePair(int xx,int yy) +{ + b3Int4 pair; + + if (xx < yy) + { + pair.x = xx; + pair.y = yy; + } + else + { + pair.x = yy; + pair.y = xx; + } + pair.z = B3_NEW_PAIR_MARKER; + pair.w = B3_NEW_PAIR_MARKER; + return pair; +} + +/*struct b3BroadphasePair : public b3Int4 { explicit b3BroadphasePair(){} - b3BroadphasePair(int xx,int yy) - { - if (xx < yy) - { - x = xx; - y = yy; - } - else - { - x = yy; - y = xx; - } - z = B3_NEW_PAIR_MARKER; - w = B3_NEW_PAIR_MARKER; - } + }; +*/ class b3BroadphasePairSortPredicate { diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp index a57d6f5b7..b50619390 100644 --- a/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp +++ b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp @@ -240,7 +240,7 @@ b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int }*/ int count = m_overlappingPairArray.size(); int oldCapacity = m_overlappingPairArray.capacity(); - void* mem = &m_overlappingPairArray.expandNonInitializing(); + pair = &m_overlappingPairArray.expandNonInitializing(); //this is where we add an actual pair, so also call the 'ghost' // if (m_ghostPairCallback) @@ -255,7 +255,7 @@ b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); } - pair = new (mem) b3BroadphasePair(proxy0,proxy1); + *pair = b3MakeBroadphasePair(proxy0,proxy1); // pair->m_pProxy0 = proxy0; // pair->m_pProxy1 = proxy1; @@ -433,7 +433,7 @@ void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1, { if (!hasDeferredRemoval()) { - b3BroadphasePair findPair(proxy0,proxy1); + b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0,proxy1); int findIndex = m_overlappingPairArray.findLinearSearch(findPair); @@ -470,8 +470,8 @@ b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,in if (!needsBroadphaseCollision(proxy0,proxy1)) return 0; - void* mem = &m_overlappingPairArray.expandNonInitializing(); - b3BroadphasePair* pair = new (mem) b3BroadphasePair(proxy0,proxy1); + b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing(); + *pair = b3MakeBroadphasePair(proxy0,proxy1); b3g_overlappingPairs++; @@ -492,7 +492,7 @@ b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,in if (!needsBroadphaseCollision(proxy0,proxy1)) return 0; - b3BroadphasePair tmpPair(proxy0,proxy1); + b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0,proxy1); int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair); if (findIndex < m_overlappingPairArray.size()) diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.cpp b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.cpp deleted file mode 100644 index 5eded6c6e..000000000 --- a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.cpp +++ /dev/null @@ -1,46 +0,0 @@ - -#include "b3CpuCollisionWorld.h" -#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h" -#include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h" - - -b3CpuCollisionWorld::b3CpuCollisionWorld(b3DynamicBvhBroadphase* bp, b3CpuNarrowPhase* np) - :m_bp(bp), - m_np(np) -{ - -} - -b3CpuCollisionWorld::~b3CpuCollisionWorld() -{ - -} - -void b3CpuCollisionWorld::addCollidable(int bodyIndex, int collidableIndex,const b3Vector3& position, const b3Quaternion& orientation) -{ - b3Vector3 aabbMinWorld, aabbMaxWorld; - - if (collidableIndex>=0) - { - b3Aabb localAabb = m_np->getLocalSpaceAabb(collidableIndex); - b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]); - b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]); - - b3Scalar margin = 0.01f; - b3Transform t; - t.setIdentity(); - t.setOrigin(b3MakeVector3(position[0],position[1],position[2])); - t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3])); - b3TransformAabb(localAabbMin,localAabbMax, margin,t,aabbMinWorld,aabbMaxWorld); - - m_bp->createProxy(aabbMinWorld,aabbMaxWorld,bodyIndex,0,1,1); - - b3Vector3 aabbMin,aabbMax; - m_bp->getAabb(bodyIndex,aabbMin,aabbMax); - - - } else - { - b3Error("registerPhysicsInstance using invalid collidableIndex\n"); - } -} \ No newline at end of file diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.h b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.h deleted file mode 100644 index ad6e7c573..000000000 --- a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.h +++ /dev/null @@ -1,28 +0,0 @@ - -#ifndef B3_CPU2_COLLISION_WORLD_H -#define B3_CPU2_COLLISION_WORLD_H - -class b3CpuNarrowPhase; -struct b3DynamicBvhBroadphase; -#include "Bullet3Common/b3Quaternion.h" -#include "Bullet3Common/b3Vector3.h" - -class b3CpuCollisionWorld -{ -protected: - - b3DynamicBvhBroadphase* m_bp; - b3CpuNarrowPhase* m_np; - -public: - - b3CpuCollisionWorld(b3DynamicBvhBroadphase* bp, b3CpuNarrowPhase* np); - - void addCollidable(int bodyIndex, int collidableIndex,const b3Vector3& position, const b3Quaternion& orientation); - - virtual ~b3CpuCollisionWorld(); - -}; - - -#endif //B3_CPU_COLLISION_WORLD_H \ No newline at end of file diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp index 89e2efd1c..16f5d59b1 100644 --- a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp +++ b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp @@ -3,6 +3,8 @@ #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h" + struct b3CpuNarrowPhaseInternalData { @@ -18,10 +20,21 @@ struct b3CpuNarrowPhaseInternalData b3AlignedObjectArray m_convexIndices; b3AlignedObjectArray m_convexFaces; + b3AlignedObjectArray m_contacts; int m_numAcceleratedShapes; }; +b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) +{ + return m_data->m_collidablesCPU[collidableIndex]; +} + +const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) const +{ + return m_data->m_collidablesCPU[collidableIndex]; +} + b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config) { @@ -35,9 +48,105 @@ b3CpuNarrowPhase::~b3CpuNarrowPhase() delete m_data; } -void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray* broadphasePairs, b3AlignedObjectArray* aabbsWorldSpace) +void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray& pairs, b3AlignedObjectArray& aabbsWorldSpace, b3AlignedObjectArray& bodies) { + int nPairs = pairs.size(); + int numContacts = 0; + int maxContactCapacity = m_data->m_config.m_maxContactCapacity; + m_data->m_contacts.resize(maxContactCapacity); + for (int i=0;im_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { +// computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE) + { +// computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); + //printf("convex-sphere\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE) + { +// computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); +// printf("convex-plane\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { +// computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); +// printf("plane-convex\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) + { +// computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0], +// nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU); +// printf("convex-plane\n"); + + } + + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE) + { +// computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); +// printf("convex-plane\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) + { +// computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); +// printf("plane-convex\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { + //printf("pairs[i].z=%d\n",pairs[i].z); + //int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies, + // m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts); + int contactIndex = b3ContactConvexConvexSAT(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies, + m_data->m_collidablesCPU,m_data->m_convexPolyhedra,m_data->m_convexVertices,m_data->m_uniqueEdges,m_data->m_convexIndices,m_data->m_convexFaces,m_data->m_contacts,numContacts,maxContactCapacity); + + + if (contactIndex>=0) + { + pairs[i].z = contactIndex; + } +// printf("plane-convex\n"); + + } + + + } + + m_data->m_contacts.resize(numContacts); } int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr) diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h index fcff82613..4da80ad8a 100644 --- a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h +++ b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h @@ -6,6 +6,7 @@ #include "Bullet3Common/b3Vector3.h" #include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" #include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" class b3CpuNarrowPhase { @@ -54,8 +55,8 @@ public: //virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects); - virtual void computeContacts(b3AlignedObjectArray* broadphasePairs, b3AlignedObjectArray* aabbsWorldSpace); - + virtual void computeContacts(b3AlignedObjectArray& pairs, b3AlignedObjectArray& aabbsWorldSpace, b3AlignedObjectArray& bodies); + const struct b3RigidBodyCL* getBodiesCpu() const; diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h new file mode 100644 index 000000000..b3d0c8031 --- /dev/null +++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h @@ -0,0 +1,523 @@ + +#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H +#define B3_CONTACT_CONVEX_CONVEX_SAT_H + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h" + +#define B3_MAX_VERTS 1024 + + + +inline b3Float4 b3Lerp3(const b3Float4& a,const b3Float4& b, float t) +{ + return b3MakeVector3( a.x + (b.x - a.x) * t, + a.y + (b.y - a.y) * t, + a.z + (b.z - a.z) * t, + 0.f); +} + + +// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut +inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS,float planeEqWS, b3Float4* ppVtxOut) +{ + + int ve; + float ds, de; + int numVertsOut = 0; + if (numVertsIn < 2) + return 0; + + b3Float4 firstVertex=pVtxIn[numVertsIn-1]; + b3Float4 endVertex = pVtxIn[0]; + + ds = b3Dot3F4(planeNormalWS,firstVertex)+planeEqWS; + + for (ve = 0; ve < numVertsIn; ve++) + { + endVertex=pVtxIn[ve]; + + de = b3Dot3F4(planeNormalWS,endVertex)+planeEqWS; + + if (ds<0) + { + if (de<0) + { + // Start < 0, end < 0, so output endVertex + ppVtxOut[numVertsOut++] = endVertex; + } + else + { + // Start < 0, end >= 0, so output intersection + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + } + } + else + { + if (de<0) + { + // Start >= 0, end < 0 so output intersection and end + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + ppVtxOut[numVertsOut++] = endVertex; + } + } + firstVertex = endVertex; + ds = de; + } + return numVertsOut; +} + + +inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA, + const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1, + b3Float4* worldVertsB2, int capacityWorldVertsB2, + const float minDist, float maxDist, + const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& facesA, const b3AlignedObjectArray& indicesA, + //const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB, + b3Float4* contactsOut, + int contactCapacity) +{ + int numContactsOut = 0; + + b3Float4* pVtxIn = worldVertsB1; + b3Float4* pVtxOut = worldVertsB2; + + int numVertsIn = numWorldVertsB1; + int numVertsOut = 0; + + int closestFaceA=-1; + { + float dmin = FLT_MAX; + for(int face=0;facem_numFaces;face++) + { + const b3Float4 Normal = b3MakeVector3( + facesA[hullA->m_faceOffset+face].m_plane.x, + facesA[hullA->m_faceOffset+face].m_plane.y, + facesA[hullA->m_faceOffset+face].m_plane.z,0.f); + const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal); + + float d = b3Dot3F4(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + } + } + } + if (closestFaceA<0) + return numContactsOut; + + b3GpuFace polyA = facesA[hullA->m_faceOffset+closestFaceA]; + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + int numContacts = numWorldVertsB1; + int numVerticesA = polyA.m_numIndices; + for(int e0=0;e0m_vertexOffset+indicesA[polyA.m_indexOffset+e0]]; + const b3Float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]]; + const b3Float4 edge0 = a - b; + const b3Float4 WorldEdge0 = b3QuatRotate(ornA,edge0); + b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA,planeNormalA); + + b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0,worldPlaneAnormal1); + b3Float4 worldA1 = b3TransformPoint(a,posA,ornA); + float planeEqWS1 = -b3Dot3F4(worldA1,planeNormalWS1); + + b3Float4 planeNormalWS = planeNormalWS1; + float planeEqWS=planeEqWS1; + + //clip face + //clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); + numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut); + + //btSwap(pVtxIn,pVtxOut); + b3Float4* tmp = pVtxOut; + pVtxOut = pVtxIn; + pVtxIn = tmp; + numVertsIn = numVertsOut; + numVertsOut = 0; + } + + + // only keep points that are behind the witness face + { + b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + float localPlaneEq = polyA.m_plane.w; + b3Float4 planeNormalWS = b3QuatRotate(ornA,localPlaneNormal); + float planeEqWS=localPlaneEq-b3Dot3F4(planeNormalWS,posA); + for (int i=0;i& verticesA, const b3AlignedObjectArray& facesA, const b3AlignedObjectArray& indicesA, + const b3AlignedObjectArray& verticesB, const b3AlignedObjectArray& facesB, const b3AlignedObjectArray& indicesB, + + b3Float4* contactsOut, + int contactCapacity) +{ + int numContactsOut = 0; + int numWorldVertsB1= 0; + + B3_PROFILE("clipHullAgainstHull"); + + float curMaxDist=maxDist; + int closestFaceB=-1; + float dmax = -FLT_MAX; + + { + //B3_PROFILE("closestFaceB"); + if (hullB.m_numFaces!=1) + { + //printf("wtf\n"); + } + static bool once = true; + //printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z); + + for(int face=0;facem_numIndices;i++) + { + b3Float4 vert = verticesB[hullB.m_vertexOffset+indicesB[faceB->m_indexOffset+i]]; + printf("vert[%d] = %f,%f,%f\n",i,vert.x,vert.y,vert.z); + } + } +#endif //BT_DEBUG_SAT_FACE + //if (facesB[hullB.m_faceOffset+face].m_numIndices>2) + { + const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset+face].m_plane.x, + facesB[hullB.m_faceOffset+face].m_plane.y, facesB[hullB.m_faceOffset+face].m_plane.z,0.f); + const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal); +#ifdef BT_DEBUG_SAT_FACE + if (once) + printf("faceNormal = %f,%f,%f\n",Normal.x,Normal.y,Normal.z); +#endif + float d = b3Dot3F4(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + once = false; + } + + + b3Assert(closestFaceB>=0); + { + //B3_PROFILE("worldVertsB1"); + const b3GpuFace& polyB = facesB[hullB.m_faceOffset+closestFaceB]; + const int numVertices = polyB.m_numIndices; + for(int e0=0;e0=0) + { + //B3_PROFILE("clipFaceAgainstHull"); + numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA, + posA,ornA, + worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist, + verticesA, facesA, indicesA, + contactsOut,contactCapacity); + } + + return numContactsOut; +} + + + + +inline int b3ClipHullHullSingle( + int bodyIndexA, int bodyIndexB, + const b3Float4& posA, + const b3Quaternion& ornA, + const b3Float4& posB, + const b3Quaternion& ornB, + + int collidableIndexA, int collidableIndexB, + + const b3AlignedObjectArray* bodyBuf, + b3AlignedObjectArray* globalContactOut, + int& nContacts, + + const b3AlignedObjectArray& hostConvexDataA, + const b3AlignedObjectArray& hostConvexDataB, + + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& uniqueEdgesA, + const b3AlignedObjectArray& facesA, + const b3AlignedObjectArray& indicesA, + + const b3AlignedObjectArray& verticesB, + const b3AlignedObjectArray& uniqueEdgesB, + const b3AlignedObjectArray& facesB, + const b3AlignedObjectArray& indicesB, + + const b3AlignedObjectArray& hostCollidablesA, + const b3AlignedObjectArray& hostCollidablesB, + const b3Vector3& sepNormalWorldSpace, + int maxContactCapacity ) +{ + int contactIndex = -1; + b3ConvexPolyhedronData hullA, hullB; + + b3Collidable colA = hostCollidablesA[collidableIndexA]; + hullA = hostConvexDataA[colA.m_shapeIndex]; + //printf("numvertsA = %d\n",hullA.m_numVertices); + + + b3Collidable colB = hostCollidablesB[collidableIndexB]; + hullB = hostConvexDataB[colB.m_shapeIndex]; + //printf("numvertsB = %d\n",hullB.m_numVertices); + + + b3Float4 contactsOut[B3_MAX_VERTS]; + int localContactCapacity = B3_MAX_VERTS; + +#ifdef _WIN32 + b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x)); + b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x)); +#endif + + + { + + b3Float4 worldVertsB1[B3_MAX_VERTS]; + b3Float4 worldVertsB2[B3_MAX_VERTS]; + int capacityWorldVerts = B3_MAX_VERTS; + + b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z,0.f); + int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex; + int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex; + + b3Scalar minDist = -1; + b3Scalar maxDist = 0.; + + + + b3Transform trA,trB; + { + //B3_PROFILE("b3TransformPoint computation"); + //trA.setIdentity(); + trA.setOrigin(b3MakeVector3(posA.x,posA.y,posA.z)); + trA.setRotation(b3Quaternion(ornA.x,ornA.y,ornA.z,ornA.w)); + + //trB.setIdentity(); + trB.setOrigin(b3MakeVector3(posB.x,posB.y,posB.z)); + trB.setRotation(b3Quaternion(ornB.x,ornB.y,ornB.z,ornB.w)); + } + + b3Quaternion trAorn = trA.getRotation(); + b3Quaternion trBorn = trB.getRotation(); + + int numContactsOut = b3ClipHullAgainstHull(hostNormal, + hostConvexDataA.at(shapeA), + hostConvexDataB.at(shapeB), + (b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn, + (b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn, + worldVertsB1,worldVertsB2,capacityWorldVerts, + minDist, maxDist, + verticesA, facesA,indicesA, + verticesB, facesB,indicesB, + + contactsOut,localContactCapacity); + + if (numContactsOut>0) + { + B3_PROFILE("overlap"); + + b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal; + b3Float4 centerOut; + + b3Int4 contactIdx; + contactIdx.x = 0; + contactIdx.y = 1; + contactIdx.z = 2; + contactIdx.w = 3; + + int numPoints = 0; + + { + B3_PROFILE("extractManifold"); + numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx); + } + + b3Assert(numPoints); + + if (nContactsexpand(); + b3Contact4Data& contact = globalContactOut->at(nContacts); + contact.m_batchIdx = 0;//i; + contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA; + contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB; + + contact.m_frictionCoeffCmp = 45874; + contact.m_restituitionCoeffCmp = 0; + + float distance = 0.f; + for (int p=0;p& rigidBodies, + const b3AlignedObjectArray& collidables, + const b3AlignedObjectArray& convexShapes, + const b3AlignedObjectArray& convexVertices, + const b3AlignedObjectArray& uniqueEdges, + const b3AlignedObjectArray& convexIndices, + const b3AlignedObjectArray& faces, + b3AlignedObjectArray& globalContactsOut, + int& nGlobalContactsOut, + int maxContactCapacity) +{ + int contactIndex = -1; + + + b3Float4 posA = rigidBodies[bodyIndexA].m_pos; + b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat; + b3Float4 posB = rigidBodies[bodyIndexB].m_pos; + b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat; + + + b3ConvexPolyhedronData hullA, hullB; + + b3Float4 sepNormalWorldSpace; + + + + b3Collidable colA = collidables[collidableIndexA]; + hullA = convexShapes[colA.m_shapeIndex]; + //printf("numvertsA = %d\n",hullA.m_numVertices); + + + b3Collidable colB = collidables[collidableIndexB]; + hullB = convexShapes[colB.m_shapeIndex]; + //printf("numvertsB = %d\n",hullB.m_numVertices); + + + b3Float4 contactsOut[B3_MAX_VERTS]; + int contactCapacity = B3_MAX_VERTS; + int numContactsOut=0; + + +#ifdef _WIN32 + b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x)); + b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x)); +#endif + + bool foundSepAxis = b3FindSeparatingAxis(hullA,hullB, + posA, + ornA, + posB, + ornB, + + convexVertices,uniqueEdges,faces,convexIndices, + convexVertices,uniqueEdges,faces,convexIndices, + + sepNormalWorldSpace + ); + + + if (foundSepAxis) + { + + + contactIndex = b3ClipHullHullSingle( + bodyIndexA, bodyIndexB, + posA,ornA, + posB,ornB, + collidableIndexA, collidableIndexB, + &rigidBodies, + &globalContactsOut, + nGlobalContactsOut, + + convexShapes, + convexShapes, + + convexVertices, + uniqueEdges, + faces, + convexIndices, + + convexVertices, + uniqueEdges, + faces, + convexIndices, + + collidables, + collidables, + sepNormalWorldSpace, + maxContactCapacity); + + } + + return contactIndex; +} + +#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h new file mode 100644 index 000000000..dab27ba61 --- /dev/null +++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h @@ -0,0 +1,162 @@ + +#ifndef B3_CONTACT_SPHERE_SPHERE_H +#define B3_CONTACT_SPHERE_SPHERE_H + + + + + +void computeContactSphereConvex(int pairIndex, + int bodyIndexA, int bodyIndexB, + int collidableIndexA, int collidableIndexB, + const b3RigidBodyCL* rigidBodies, + const b3Collidable* collidables, + const b3ConvexPolyhedronCL* convexShapes, + const b3Vector3* convexVertices, + const int* convexIndices, + const b3GpuFace* faces, + b3Contact4* globalContactsOut, + int& nGlobalContactsOut, + int maxContactCapacity) +{ + + float radius = collidables[collidableIndexA].m_radius; + float4 spherePos1 = rigidBodies[bodyIndexA].m_pos; + b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat; + + + + float4 pos = rigidBodies[bodyIndexB].m_pos; + + + b3Quaternion quat = rigidBodies[bodyIndexB].m_quat; + + b3Transform tr; + tr.setIdentity(); + tr.setOrigin(pos); + tr.setRotation(quat); + b3Transform trInv = tr.inverse(); + + float4 spherePos = trInv(spherePos1); + + int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx; + int shapeIndex = collidables[collidableIndex].m_shapeIndex; + int numFaces = convexShapes[shapeIndex].m_numFaces; + float4 closestPnt = b3MakeVector3(0, 0, 0, 0); + float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0); + float minDist = -1000000.f; // TODO: What is the largest/smallest float? + bool bCollide = true; + int region = -1; + float4 localHitNormal; + for ( int f = 0; f < numFaces; f++ ) + { + b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f]; + float4 planeEqn; + float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); + float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal); + planeEqn = n1; + planeEqn[3] = face.m_plane.w; + + float4 pntReturn; + float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn); + + if ( dist > radius) + { + bCollide = false; + break; + } + + if ( dist > 0 ) + { + //might hit an edge or vertex + b3Vector3 out; + + bool isInPoly = IsPointInPolygon(spherePos, + &face, + &convexVertices[convexShapes[shapeIndex].m_vertexOffset], + convexIndices, + &out); + if (isInPoly) + { + if (dist>minDist) + { + minDist = dist; + closestPnt = pntReturn; + localHitNormal = planeEqn; + region=1; + } + } else + { + b3Vector3 tmp = spherePos-out; + b3Scalar l2 = tmp.length2(); + if (l2minDist) + { + minDist = dist; + closestPnt = out; + localHitNormal = tmp/dist; + region=2; + } + + } else + { + bCollide = false; + break; + } + } + } + else + { + if ( dist > minDist ) + { + minDist = dist; + closestPnt = pntReturn; + localHitNormal = planeEqn; + region=3; + } + } + } + static int numChecks = 0; + numChecks++; + + if (bCollide && minDist > -10000) + { + + float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld; + float4 pOnB1 = tr(closestPnt); + //printf("dist ,%f,",minDist); + float actualDepth = minDist-radius; + if (actualDepth<0) + { + //printf("actualDepth = ,%f,", actualDepth); + //printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z); + //printf("region=,%d,\n", region); + pOnB1[3] = actualDepth; + + int dstIdx; +// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx ); + + if (nGlobalContactsOut < maxContactCapacity) + { + dstIdx=nGlobalContactsOut; + nGlobalContactsOut++; + + b3Contact4* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = normalOnSurfaceB1; + c->setFrictionCoeff(0.7); + c->setRestituitionCoeff(0.f); + + c->m_batchIdx = pairIndex; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB; + c->m_worldPosB[0] = pOnB1; + int numPoints = 1; + c->m_worldNormalOnB.w = (b3Scalar)numPoints; + }//if (dstIdx < numPairs) + } + }//if (hasCollision) + +} +#endif //B3_CONTACT_SPHERE_SPHERE_H diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h new file mode 100644 index 000000000..039130016 --- /dev/null +++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h @@ -0,0 +1,206 @@ +#ifndef B3_FIND_SEPARATING_AXIS_H +#define B3_FIND_SEPARATING_AXIS_H + + +inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray& vertices, b3Scalar& min, b3Scalar& max) +{ + min = FLT_MAX; + max = -FLT_MAX; + int numVerts = hull.m_numVertices; + + const b3Float4 localDir = b3QuatRotate(orn.inverse(),dir); + + b3Scalar offset = b3Dot3F4(pos,dir); + + for(int i=0;i max) max = dp; + } + if(min>max) + { + b3Scalar tmp = min; + min = max; + max = tmp; + } + min += offset; + max += offset; +} + + +inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const b3Float4& posA,const b3Quaternion& ornA, + const b3Float4& posB,const b3Quaternion& ornB, + const b3Float4& sep_axis, const b3AlignedObjectArray& verticesA,const b3AlignedObjectArray& verticesB,b3Scalar& depth) +{ + b3Scalar Min0,Max0; + b3Scalar Min1,Max1; + b3ProjectAxis(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0); + b3ProjectAxis(hullB,posB,ornB, sep_axis,verticesB, Min1, Max1); + + if(Max0=0.0f); + b3Scalar d1 = Max1 - Min0; + b3Assert(d1>=0.0f); + depth = d0& verticesA, + const b3AlignedObjectArray& uniqueEdgesA, + const b3AlignedObjectArray& facesA, + const b3AlignedObjectArray& indicesA, + const b3AlignedObjectArray& verticesB, + const b3AlignedObjectArray& uniqueEdgesB, + const b3AlignedObjectArray& facesB, + const b3AlignedObjectArray& indicesB, + + b3Vector3& sep) +{ + B3_PROFILE("findSeparatingAxis"); + + b3Float4 posA = posA1; + posA.w = 0.f; + b3Float4 posB = posB1; + posB.w = 0.f; +//#ifdef TEST_INTERNAL_OBJECTS + b3Float4 c0local = (b3Float4&)hullA.m_localCenter; + + b3Float4 c0 = b3TransformPoint(c0local, posA, ornA); + b3Float4 c1local = (b3Float4&)hullB.m_localCenter; + b3Float4 c1 = b3TransformPoint(c1local,posB,ornB); + const b3Float4 deltaC2 = c0 - c1; +//#endif + + b3Scalar dmin = FLT_MAX; + int curPlaneTests=0; + + int numFacesA = hullA.m_numFaces; + // Test normals from hullA + for(int i=0;i0.0f) + sep = -sep; + + return true; +} + +#endif //B3_FIND_SEPARATING_AXIS_H + diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h new file mode 100644 index 000000000..35b519700 --- /dev/null +++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h @@ -0,0 +1,97 @@ +#ifndef B3_REDUCE_CONTACTS_H +#define B3_REDUCE_CONTACTS_H + +inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx) +{ + if( nPoints == 0 ) + return 0; + + if (nPoints <=4) + return nPoints; + + + if (nPoints >64) + nPoints = 64; + + b3Float4 center = b3MakeFloat4(0,0,0,0); + { + + for (int i=0;im_pos; + b3Quat orientation = body->m_quat; + + int collidableIndex = body->m_collidableIdx; + int shapeIndex = collidables[collidableIndex].m_shapeIndex; + + if (shapeIndex>=0) + { + + b3Aabb localAabb = localShapeAABB[shapeIndex]; + b3Aabb worldAabb; + + b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&worldAabb.m_minVec,&worldAabb.m_maxVec); + worldAabbs[bodyId] = worldAabb; + } +} + +#endif //B3_UPDATE_AABBS_H diff --git a/src/Bullet3Common/shared/b3Float4.h b/src/Bullet3Common/shared/b3Float4.h index d6ac69d39..a8159e51f 100644 --- a/src/Bullet3Common/shared/b3Float4.h +++ b/src/Bullet3Common/shared/b3Float4.h @@ -10,6 +10,13 @@ #define b3Dot3F4 b3Dot #define b3Cross3 b3Cross #define b3MakeFloat4 b3MakeVector3 + + inline b3Float4 b3FastNormalized3(b3Float4ConstArg v) + { + return v.normalized(); + } + + #else typedef float4 b3Float4; #define b3Float4ConstArg const b3Float4 @@ -28,4 +35,15 @@ } #endif + + +inline bool b3IsAlmostZero(b3Float4ConstArg v) +{ + if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) + return false; + return true; +} + + + #endif //B3_FLOAT4_H diff --git a/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp b/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp index a0ddc7e56..48a46ecf1 100644 --- a/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp +++ b/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp @@ -5,11 +5,15 @@ #include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h" #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" #include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3CollidableData.h" struct b3CpuRigidBodyPipelineInternalData { b3AlignedObjectArray m_rigidBodies; + b3AlignedObjectArray m_aabbWorldSpace; + b3DynamicBvhBroadphase* m_bp; b3CpuNarrowPhase* m_np; b3Config m_config; @@ -32,6 +36,27 @@ b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline() void b3CpuRigidBodyPipeline::updateAabbWorldSpace() { + for (int i=0;igetNumBodies();i++) + { + b3RigidBodyData* body = &m_data->m_rigidBodies[i]; + b3Float4 position = body->m_pos; + b3Quat orientation = body->m_quat; + + int collidableIndex = body->m_collidableIdx; + b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex); + int shapeIndex = collidable.m_shapeIndex; + + if (shapeIndex>=0) + { + + + b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex); + b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i]; + float margin=0.f; + b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&worldAabb.m_minVec,&worldAabb.m_maxVec); + m_data->m_bp->setAabb(i,worldAabb.m_minVec,worldAabb.m_maxVec,0); + } + } } void b3CpuRigidBodyPipeline::computeOverlappingPairs() @@ -39,15 +64,15 @@ void b3CpuRigidBodyPipeline::computeOverlappingPairs() int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs(); m_data->m_bp->calculateOverlappingPairs(); numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs(); + //printf("numPairs=%d\n",numPairs); } void b3CpuRigidBodyPipeline::computeContactPoints() { - b3AlignedObjectArray aabbWorldSpace; - b3AlignedObjectArray pairs; + b3AlignedObjectArray& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray(); - m_data->m_np->computeContacts(&pairs,&aabbWorldSpace); + m_data->m_np->computeContacts(pairs,m_data->m_aabbWorldSpace, m_data->m_rigidBodies); } void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime) @@ -87,7 +112,7 @@ void b3CpuRigidBodyPipeline::integrate(float deltaTime) int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData) { b3RigidBodyData body; - int index = m_data->m_rigidBodies.size(); + int bodyIndex = m_data->m_rigidBodies.size(); body.m_invMass = mass ? 1.f/mass : 0.f; body.m_angVel.setValue(0,0,0); body.m_collidableIdx = collidableIndex; @@ -99,15 +124,32 @@ int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po m_data->m_rigidBodies.push_back(body); + + if (collidableIndex>=0) + { + b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand(); + b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex); + b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]); + b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]); + + b3Scalar margin = 0.01f; + b3Transform t; + t.setIdentity(); + t.setOrigin(b3MakeVector3(position[0],position[1],position[2])); + t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3])); + b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec); + m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1); +// b3Vector3 aabbMin,aabbMax; + // m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax); + } else + { + b3Error("registerPhysicsInstance using invalid collidableIndex\n"); + } - - - - - return index; + return bodyIndex; } diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp index 4f916cc93..0fe30dd0c 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp @@ -227,10 +227,9 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) m_data->m_allAabbsGPU->copyToHost(m_data->m_allAabbsCPU); for (int i=0;im_allAabbsCPU.size();i++) { - b3BroadphaseProxy* proxy = &m_data->m_broadphaseDbvt->m_proxies[i]; b3Vector3 aabbMin=b3MakeVector3(m_data->m_allAabbsCPU[i].m_min[0],m_data->m_allAabbsCPU[i].m_min[1],m_data->m_allAabbsCPU[i].m_min[2]); b3Vector3 aabbMax=b3MakeVector3(m_data->m_allAabbsCPU[i].m_max[0],m_data->m_allAabbsCPU[i].m_max[1],m_data->m_allAabbsCPU[i].m_max[2]); - m_data->m_broadphaseDbvt->setAabb(proxy,aabbMin,aabbMax,0); + m_data->m_broadphaseDbvt->setAabb(i,aabbMin,aabbMax,0); } }