From e12c89e8c9896b2e5cdd70dbd2d2acb449ff4b94 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=A9mi=20Verschelde?= Date: Sat, 13 Jan 2018 14:01:53 +0100 Subject: bullet: Streamline bundling, remove extraneous src/ folder Document version and how to extract sources in thirdparty/README.md. Drop unnecessary CMake and Premake files. Simplify SCsub, drop unused one. --- .../NarrowPhaseCollision/b3Config.h | 41 + .../NarrowPhaseCollision/b3Contact4.h | 46 ++ .../NarrowPhaseCollision/b3ConvexUtility.cpp | 520 ++++++++++++ .../NarrowPhaseCollision/b3ConvexUtility.h | 62 ++ .../NarrowPhaseCollision/b3CpuNarrowPhase.cpp | 323 ++++++++ .../NarrowPhaseCollision/b3CpuNarrowPhase.h | 105 +++ .../NarrowPhaseCollision/b3RaycastInfo.h | 24 + .../NarrowPhaseCollision/b3RigidBodyCL.h | 30 + .../shared/b3BvhSubtreeInfoData.h | 20 + .../NarrowPhaseCollision/shared/b3BvhTraversal.h | 126 +++ .../NarrowPhaseCollision/shared/b3ClipFaces.h | 188 +++++ .../NarrowPhaseCollision/shared/b3Collidable.h | 76 ++ .../NarrowPhaseCollision/shared/b3Contact4Data.h | 40 + .../shared/b3ContactConvexConvexSAT.h | 520 ++++++++++++ .../shared/b3ContactSphereSphere.h | 162 ++++ .../shared/b3ConvexPolyhedronData.h | 40 + .../shared/b3FindConcaveSatAxis.h | 832 +++++++++++++++++++ .../shared/b3FindSeparatingAxis.h | 206 +++++ .../NarrowPhaseCollision/shared/b3MprPenetration.h | 920 +++++++++++++++++++++ .../shared/b3NewContactReduction.h | 196 +++++ .../shared/b3QuantizedBvhNodeData.h | 90 ++ .../NarrowPhaseCollision/shared/b3ReduceContacts.h | 97 +++ .../NarrowPhaseCollision/shared/b3RigidBodyData.h | 34 + .../NarrowPhaseCollision/shared/b3UpdateAabbs.h | 40 + 24 files changed, 4738 insertions(+) create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Config.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h create mode 100644 thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h (limited to 'thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision') diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Config.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Config.h new file mode 100644 index 0000000000..65d4a21613 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Config.h @@ -0,0 +1,41 @@ +#ifndef B3_CONFIG_H +#define B3_CONFIG_H + +struct b3Config +{ + int m_maxConvexBodies; + int m_maxConvexShapes; + int m_maxBroadphasePairs; + int m_maxContactCapacity; + int m_compoundPairCapacity; + + int m_maxVerticesPerFace; + int m_maxFacesPerShape; + int m_maxConvexVertices; + int m_maxConvexIndices; + int m_maxConvexUniqueEdges; + + int m_maxCompoundChildShapes; + + int m_maxTriConvexPairCapacity; + + b3Config() + :m_maxConvexBodies(128*1024), + m_maxVerticesPerFace(64), + m_maxFacesPerShape(12), + m_maxConvexVertices(8192), + m_maxConvexIndices(81920), + m_maxConvexUniqueEdges(8192), + m_maxCompoundChildShapes(8192), + m_maxTriConvexPairCapacity(256*1024) + { + m_maxConvexShapes = m_maxConvexBodies; + m_maxBroadphasePairs = 16*m_maxConvexBodies; + m_maxContactCapacity = m_maxBroadphasePairs; + m_compoundPairCapacity = 1024*1024; + } +}; + + +#endif//B3_CONFIG_H + diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h new file mode 100644 index 0000000000..fb25165673 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h @@ -0,0 +1,46 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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 B3_CONTACT4_H +#define B3_CONTACT4_H + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +B3_ATTRIBUTE_ALIGNED16(struct) b3Contact4 : public b3Contact4Data +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + + int getBodyA()const {return abs(m_bodyAPtrAndSignBit);} + int getBodyB()const {return abs(m_bodyBPtrAndSignBit);} + bool isBodyAFixed()const { return m_bodyAPtrAndSignBit<0;} + bool isBodyBFixed()const { return m_bodyBPtrAndSignBit<0;} + // todo. make it safer + int& getBatchIdx() { return m_batchIdx; } + const int& getBatchIdx() const { return m_batchIdx; } + float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp/(float)0xffff); } + void setRestituitionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_restituitionCoeffCmp = (unsigned short)(c*0xffff); } + float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp/(float)0xffff); } + void setFrictionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_frictionCoeffCmp = (unsigned short)(c*0xffff); } + + //float& getNPoints() { return m_worldNormal[3]; } + int getNPoints() const { return (int) m_worldNormalOnB.w; } + + float getPenetration(int idx) const { return m_worldPosB[idx].w; } + + bool isInvalid() const { return (getBodyA()==0 || getBodyB()==0); } +}; + +#endif //B3_CONTACT4_H diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp new file mode 100644 index 0000000000..55706fa631 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp @@ -0,0 +1,520 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +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. +*/ +//Originally written by Erwin Coumans + + +#include "b3ConvexUtility.h" +#include "Bullet3Geometry/b3ConvexHullComputer.h" +#include "Bullet3Geometry/b3GrahamScan2dConvexHull.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3HashMap.h" + + + + + +b3ConvexUtility::~b3ConvexUtility() +{ +} + +bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles) +{ + + + + b3ConvexHullComputer conv; + conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3),numPoints,0.f,0.f); + + b3AlignedObjectArray faceNormals; + int numFaces = conv.faces.size(); + faceNormals.resize(numFaces); + b3ConvexHullComputer* convexUtil = &conv; + + + b3AlignedObjectArray tmpFaces; + tmpFaces.resize(numFaces); + + int numVertices = convexUtil->vertices.size(); + m_vertices.resize(numVertices); + for (int p=0;pvertices[p]; + } + + + for (int i=0;ifaces[i]; + //printf("face=%d\n",face); + const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; + const b3ConvexHullComputer::Edge* edge = firstEdge; + + b3Vector3 edges[3]; + int numEdges = 0; + //compute face normals + + do + { + + int src = edge->getSourceVertex(); + tmpFaces[i].m_indices.push_back(src); + int targ = edge->getTargetVertex(); + b3Vector3 wa = convexUtil->vertices[src]; + + b3Vector3 wb = convexUtil->vertices[targ]; + b3Vector3 newEdge = wb-wa; + newEdge.normalize(); + if (numEdges<2) + edges[numEdges++] = newEdge; + + edge = edge->getNextEdgeOfFace(); + } while (edge!=firstEdge); + + b3Scalar planeEq = 1e30f; + + + if (numEdges==2) + { + faceNormals[i] = edges[0].cross(edges[1]); + faceNormals[i].normalize(); + tmpFaces[i].m_plane[0] = faceNormals[i].getX(); + tmpFaces[i].m_plane[1] = faceNormals[i].getY(); + tmpFaces[i].m_plane[2] = faceNormals[i].getZ(); + tmpFaces[i].m_plane[3] = planeEq; + + } + else + { + b3Assert(0);//degenerate? + faceNormals[i].setZero(); + } + + for (int v=0;veq) + { + planeEq=eq; + } + } + tmpFaces[i].m_plane[3] = -planeEq; + } + + //merge coplanar faces and copy them to m_polyhedron + + b3Scalar faceWeldThreshold= 0.999f; + b3AlignedObjectArray todoFaces; + for (int i=0;i coplanarFaceGroup; + int refFace = todoFaces[todoFaces.size()-1]; + + coplanarFaceGroup.push_back(refFace); + b3MyFace& faceA = tmpFaces[refFace]; + todoFaces.pop_back(); + + b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]); + for (int j=todoFaces.size()-1;j>=0;j--) + { + int i = todoFaces[j]; + b3MyFace& faceB = tmpFaces[i]; + b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]); + if (faceNormalA.dot(faceNormalB)>faceWeldThreshold) + { + coplanarFaceGroup.push_back(i); + todoFaces.remove(i); + } + } + + + bool did_merge = false; + if (coplanarFaceGroup.size()>1) + { + //do the merge: use Graham Scan 2d convex hull + + b3AlignedObjectArray orgpoints; + b3Vector3 averageFaceNormal = b3MakeVector3(0,0,0); + + for (int i=0;im_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); + + b3MyFace& face = tmpFaces[coplanarFaceGroup[i]]; + b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0],face.m_plane[1],face.m_plane[2]); + averageFaceNormal+=faceNormal; + for (int f=0;f hull; + + averageFaceNormal.normalize(); + b3GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal); + + for (int i=0;i1e-6 || fabsf(v.getY())>1e-6 || fabsf(v.getZ())>1e-6) return false; + return true; +} + +struct b3InternalVertexPair +{ + b3InternalVertexPair(short int v0,short int v1) + :m_v0(v0), + m_v1(v1) + { + if (m_v1>m_v0) + b3Swap(m_v0,m_v1); + } + short int m_v0; + short int m_v1; + int getHash() const + { + return m_v0+(m_v1<<16); + } + bool equals(const b3InternalVertexPair& other) const + { + return m_v0==other.m_v0 && m_v1==other.m_v1; + } +}; + +struct b3InternalEdge +{ + b3InternalEdge() + :m_face0(-1), + m_face1(-1) + { + } + short int m_face0; + short int m_face1; +}; + +// + +#ifdef TEST_INTERNAL_OBJECTS +bool b3ConvexUtility::testContainment() const +{ + for(int p=0;p<8;p++) + { + b3Vector3 LocalPt; + if(p==0) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]); + else if(p==1) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]); + else if(p==2) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]); + else if(p==3) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]); + else if(p==4) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]); + else if(p==5) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]); + else if(p==6) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]); + else if(p==7) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]); + + for(int i=0;i0.0f) + return false; + } + } + return true; +} +#endif + +void b3ConvexUtility::initialize() +{ + + b3HashMap edges; + + b3Scalar TotalArea = 0.0f; + + m_localCenter.setValue(0, 0, 0); + for(int i=0;im_face0>=0); + // b3Assert(edptr->m_face1<0); + edptr->m_face1 = i; + } else + { + b3InternalEdge ed; + ed.m_face0 = i; + edges.insert(vp,ed); + } + } + } + +#ifdef USE_CONNECTED_FACES + for(int i=0;im_face0>=0); + b3Assert(edptr->m_face1>=0); + + int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0; + m_faces[i].m_connectedFaces[j] = connectedFace; + } + } +#endif//USE_CONNECTED_FACES + + for(int i=0;iMaxX) MaxX = pt.getX(); + if(pt.getY()MaxY) MaxY = pt.getY(); + if(pt.getZ()MaxZ) MaxZ = pt.getZ(); + } + mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ); + mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ); + + + +// const b3Scalar r = m_radius / sqrtf(2.0f); + const b3Scalar r = m_radius / sqrtf(3.0f); + const int LargestExtent = mE.maxAxis(); + const b3Scalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f; + m_extents[0] = m_extents[1] = m_extents[2] = r; + m_extents[LargestExtent] = mE[LargestExtent]*0.5f; + bool FoundBox = false; + for(int j=0;j<1024;j++) + { + if(testContainment()) + { + FoundBox = true; + break; + } + + m_extents[LargestExtent] -= Step; + } + if(!FoundBox) + { + m_extents[0] = m_extents[1] = m_extents[2] = r; + } + else + { + // Refine the box + const b3Scalar Step = (m_radius - r)/1024.0f; + const int e0 = (1< m_indices; + b3Scalar m_plane[4]; +}; + +B3_ATTRIBUTE_ALIGNED16(class) b3ConvexUtility +{ + public: + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3Vector3 m_localCenter; + b3Vector3 m_extents; + b3Vector3 mC; + b3Vector3 mE; + b3Scalar m_radius; + + b3AlignedObjectArray m_vertices; + b3AlignedObjectArray m_faces; + b3AlignedObjectArray m_uniqueEdges; + + + b3ConvexUtility() + { + } + virtual ~b3ConvexUtility(); + + bool initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles=true); + + void initialize(); + bool testContainment() const; + + + +}; +#endif + \ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp new file mode 100644 index 0000000000..c3134b2c65 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp @@ -0,0 +1,323 @@ +#include "b3CpuNarrowPhase.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h" + + +struct b3CpuNarrowPhaseInternalData +{ + b3AlignedObjectArray m_localShapeAABBCPU; + b3AlignedObjectArray m_collidablesCPU; + b3AlignedObjectArray m_convexData; + b3Config m_config; + + + b3AlignedObjectArray m_convexPolyhedra; + b3AlignedObjectArray m_uniqueEdges; + b3AlignedObjectArray m_convexVertices; + b3AlignedObjectArray m_convexIndices; + b3AlignedObjectArray m_convexFaces; + + b3AlignedObjectArray m_contacts; + + int m_numAcceleratedShapes; +}; + + +const b3AlignedObjectArray& b3CpuNarrowPhase::getContacts() const +{ + return m_data->m_contacts; +} + +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) +{ + m_data = new b3CpuNarrowPhaseInternalData; + m_data->m_config = config; + m_data->m_numAcceleratedShapes = 0; +} + +b3CpuNarrowPhase::~b3CpuNarrowPhase() +{ + delete m_data; +} + +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) +{ + int collidableIndex = allocateCollidable(); + if (collidableIndex<0) + return collidableIndex; + + + b3Collidable& col = m_data->m_collidablesCPU[collidableIndex]; + col.m_shapeType = SHAPE_CONVEX_HULL; + col.m_shapeIndex = -1; + + + { + b3Vector3 localCenter=b3MakeVector3(0,0,0); + for (int i=0;im_vertices.size();i++) + localCenter+=utilPtr->m_vertices[i]; + localCenter*= (1.f/utilPtr->m_vertices.size()); + utilPtr->m_localCenter = localCenter; + + col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col); + } + + if (col.m_shapeIndex>=0) + { + b3Aabb aabb; + + b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f); + b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f); + + for (int i=0;im_vertices.size();i++) + { + myAabbMin.setMin(utilPtr->m_vertices[i]); + myAabbMax.setMax(utilPtr->m_vertices[i]); + } + aabb.m_min[0] = myAabbMin[0]; + aabb.m_min[1] = myAabbMin[1]; + aabb.m_min[2] = myAabbMin[2]; + aabb.m_minIndices[3] = 0; + + aabb.m_max[0] = myAabbMax[0]; + aabb.m_max[1] = myAabbMax[1]; + aabb.m_max[2] = myAabbMax[2]; + aabb.m_signedMaxIndices[3] = 0; + + m_data->m_localShapeAABBCPU.push_back(aabb); + + } + + return collidableIndex; +} + +int b3CpuNarrowPhase::allocateCollidable() +{ + int curSize = m_data->m_collidablesCPU.size(); + if (curSizem_config.m_maxConvexShapes) + { + m_data->m_collidablesCPU.expand(); + return curSize; + } + else + { + b3Error("allocateCollidable out-of-range %d\n",m_data->m_config.m_maxConvexShapes); + } + return -1; + +} + +int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling) +{ + b3AlignedObjectArray verts; + + unsigned char* vts = (unsigned char*) vertices; + for (int i=0;iinitializePolyhedralFeatures(&verts[0],verts.size(),merge); + } + + int collidableIndex = registerConvexHullShape(utilPtr); + + delete utilPtr; + return collidableIndex; +} + + +int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col) +{ + + m_data->m_convexData.resize(m_data->m_numAcceleratedShapes+1); + m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1); + + + b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1); + convex.mC = convexPtr->mC; + convex.mE = convexPtr->mE; + convex.m_extents= convexPtr->m_extents; + convex.m_localCenter = convexPtr->m_localCenter; + convex.m_radius = convexPtr->m_radius; + + convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size(); + int edgeOffset = m_data->m_uniqueEdges.size(); + convex.m_uniqueEdgesOffset = edgeOffset; + + m_data->m_uniqueEdges.resize(edgeOffset+convex.m_numUniqueEdges); + + //convex data here + int i; + for ( i=0;im_uniqueEdges.size();i++) + { + m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[i]; + } + + int faceOffset = m_data->m_convexFaces.size(); + convex.m_faceOffset = faceOffset; + convex.m_numFaces = convexPtr->m_faces.size(); + + m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces); + + + for (i=0;im_faces.size();i++) + { + m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0], + convexPtr->m_faces[i].m_plane[1], + convexPtr->m_faces[i].m_plane[2], + convexPtr->m_faces[i].m_plane[3]); + + + int indexOffset = m_data->m_convexIndices.size(); + int numIndices = convexPtr->m_faces[i].m_indices.size(); + m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices; + m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset; + m_data->m_convexIndices.resize(indexOffset+numIndices); + for (int p=0;pm_convexIndices[indexOffset+p] = convexPtr->m_faces[i].m_indices[p]; + } + } + + convex.m_numVertices = convexPtr->m_vertices.size(); + int vertexOffset = m_data->m_convexVertices.size(); + convex.m_vertexOffset =vertexOffset; + + m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices); + for (int i=0;im_vertices.size();i++) + { + m_data->m_convexVertices[vertexOffset+i] = convexPtr->m_vertices[i]; + } + + (m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr; + + + + return m_data->m_numAcceleratedShapes++; +} + +const b3Aabb& b3CpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const +{ + return m_data->m_localShapeAABBCPU[collidableIndex]; +} diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h new file mode 100644 index 0000000000..528be3346d --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h @@ -0,0 +1,105 @@ +#ifndef B3_CPU_NARROWPHASE_H +#define B3_CPU_NARROWPHASE_H + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +class b3CpuNarrowPhase +{ +protected: + + struct b3CpuNarrowPhaseInternalData* m_data; + int m_acceleratedCompanionShapeIndex; + int m_planeBodyIndex; + int m_static0Index; + + int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr,b3Collidable& col); + int registerConcaveMeshShape(b3AlignedObjectArray* vertices, b3AlignedObjectArray* indices, b3Collidable& col, const float* scaling); + +public: + + + + + b3CpuNarrowPhase(const struct b3Config& config); + + virtual ~b3CpuNarrowPhase(void); + + int registerSphereShape(float radius); + int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant); + + int registerCompoundShape(b3AlignedObjectArray* childShapes); + int registerFace(const b3Vector3& faceNormal, float faceConstant); + + int registerConcaveMesh(b3AlignedObjectArray* vertices, b3AlignedObjectArray* indices,const float* scaling); + + //do they need to be merged? + + int registerConvexHullShape(b3ConvexUtility* utilPtr); + int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling); + + //int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu); + void setObjectTransform(const float* position, const float* orientation , int bodyIndex); + + void writeAllBodiesToGpu(); + void reset(); + void readbackAllBodiesToCpu(); + bool getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const; + + void setObjectTransformCpu(float* position, float* orientation , int bodyIndex); + void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex); + + + //virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects); + virtual void computeContacts(b3AlignedObjectArray& pairs, b3AlignedObjectArray& aabbsWorldSpace, b3AlignedObjectArray& bodies); + + + + const struct b3RigidBodyData* getBodiesCpu() const; + //struct b3RigidBodyData* getBodiesCpu(); + + int getNumBodiesGpu() const; + + + int getNumBodyInertiasGpu() const; + + + const struct b3Collidable* getCollidablesCpu() const; + int getNumCollidablesGpu() const; + + + /*const struct b3Contact4* getContactsCPU() const; + + + int getNumContactsGpu() const; + */ + + const b3AlignedObjectArray& getContacts() const; + + + int getNumRigidBodies() const; + + int allocateCollidable(); + + int getStatic0Index() const + { + return m_static0Index; + } + b3Collidable& getCollidableCpu(int collidableIndex); + const b3Collidable& getCollidableCpu(int collidableIndex) const; + + const b3CpuNarrowPhaseInternalData* getInternalData() const + { + return m_data; + } + + const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const; +}; + +#endif //B3_CPU_NARROWPHASE_H + diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h new file mode 100644 index 0000000000..fba8bd07a4 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h @@ -0,0 +1,24 @@ + +#ifndef B3_RAYCAST_INFO_H +#define B3_RAYCAST_INFO_H + +#include "Bullet3Common/b3Vector3.h" + +B3_ATTRIBUTE_ALIGNED16(struct) b3RayInfo +{ + b3Vector3 m_from; + b3Vector3 m_to; +}; + +B3_ATTRIBUTE_ALIGNED16(struct) b3RayHit +{ + b3Scalar m_hitFraction; + int m_hitBody; + int m_hitResult1; + int m_hitResult2; + b3Vector3 m_hitPoint; + b3Vector3 m_hitNormal; +}; + +#endif //B3_RAYCAST_INFO_H + diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h new file mode 100644 index 0000000000..d58f71802f --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h @@ -0,0 +1,30 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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 B3_RIGID_BODY_CL +#define B3_RIGID_BODY_CL + +#include "Bullet3Common/b3Scalar.h" +#include "Bullet3Common/b3Matrix3x3.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + + +inline float b3GetInvMass(const b3RigidBodyData& body) +{ + return body.m_invMass; +} + + +#endif//B3_RIGID_BODY_CL diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h new file mode 100644 index 0000000000..8788ccbb47 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h @@ -0,0 +1,20 @@ + +#ifndef B3_BVH_SUBTREE_INFO_DATA_H +#define B3_BVH_SUBTREE_INFO_DATA_H + +typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t; + +struct b3BvhSubtreeInfoData +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes, points to the root of the subtree + int m_rootNodeIndex; + //4 bytes + int m_subtreeSize; + int m_padding[3]; +}; + +#endif //B3_BVH_SUBTREE_INFO_DATA_H + diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h new file mode 100644 index 0000000000..2618da24bc --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h @@ -0,0 +1,126 @@ + + +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" + + + +// work-in-progress +void b3BvhTraversal( __global const b3Int4* pairs, + __global const b3RigidBodyData* rigidBodies, + __global const b3Collidable* collidables, + __global b3Aabb* aabbs, + __global b3Int4* concavePairsOut, + __global volatile int* numConcavePairsOut, + __global const b3BvhSubtreeInfo* subtreeHeadersRoot, + __global const b3QuantizedBvhNode* quantizedNodesRoot, + __global const b3BvhInfo* bvhInfos, + int numPairs, + int maxNumConcavePairsCapacity, + int id) +{ + + int bodyIndexA = pairs[id].x; + int bodyIndexB = pairs[id].y; + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + //once the broadphase avoids static-static pairs, we can remove this test + if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0)) + { + return; + } + + if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH) + return; + + int shapeTypeB = collidables[collidableIndexB].m_shapeType; + + if (shapeTypeB!=SHAPE_CONVEX_HULL && + shapeTypeB!=SHAPE_SPHERE && + shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS + ) + return; + + b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes]; + + b3Float4 bvhAabbMin = bvhInfo.m_aabbMin; + b3Float4 bvhAabbMax = bvhInfo.m_aabbMax; + b3Float4 bvhQuantization = bvhInfo.m_quantization; + int numSubtreeHeaders = bvhInfo.m_numSubTrees; + __global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset]; + __global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset]; + + + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization); + b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization); + + for (int i=0;i= 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; +} + + +__kernel void clipFacesAndFindContactsKernel( __global const b3Float4* separatingNormals, + __global const int* hasSeparatingAxis, + __global b3Int4* clippingFacesOut, + __global b3Float4* worldVertsA1, + __global b3Float4* worldNormalsA1, + __global b3Float4* worldVertsB1, + __global b3Float4* worldVertsB2, + int vertexFaceCapacity, + int pairIndex + ) +{ +// int i = get_global_id(0); + //int pairIndex = i; + int i = pairIndex; + + float minDist = -1e30f; + float maxDist = 0.02f; + +// if (i=0) + { + + + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + + for(int e0=0;e0m_worldNormalOnB.w; +}; + +inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints) +{ + contact->m_worldNormalOnB.w = (float)numPoints; +}; + + + +#endif //B3_CONTACT4DATA_H \ No newline at end of file diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h new file mode 100644 index 0000000000..f295f01a6c --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h @@ -0,0 +1,520 @@ + +#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); + + + + +#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/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h new file mode 100644 index 0000000000..a3fa82287b --- /dev/null +++ b/thirdparty/bullet/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 b3RigidBodyData* rigidBodies, + const b3Collidable* collidables, + const b3ConvexPolyhedronData* 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/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h new file mode 100644 index 0000000000..5c5f4e297f --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h @@ -0,0 +1,40 @@ + +#ifndef B3_CONVEX_POLYHEDRON_DATA_H +#define B3_CONVEX_POLYHEDRON_DATA_H + + + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Common/shared/b3Quat.h" + +typedef struct b3GpuFace b3GpuFace_t; +struct b3GpuFace +{ + b3Float4 m_plane; + int m_indexOffset; + int m_numIndices; + int m_unusedPadding1; + int m_unusedPadding2; +}; + +typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t; + +struct b3ConvexPolyhedronData +{ + b3Float4 m_localCenter; + b3Float4 m_extents; + b3Float4 mC; + b3Float4 mE; + + float m_radius; + int m_faceOffset; + int m_numFaces; + int m_numVertices; + + int m_vertexOffset; + int m_uniqueEdgesOffset; + int m_numUniqueEdges; + int m_unused; +}; + +#endif //B3_CONVEX_POLYHEDRON_DATA_H diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h new file mode 100644 index 0000000000..89993f3565 --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h @@ -0,0 +1,832 @@ +#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H +#define B3_FIND_CONCAVE_SEPARATING_AXIS_H + +#define B3_TRIANGLE_NUM_CONVEX_FACES 5 + + +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" + + +inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn, +const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max) +{ + min[0] = FLT_MAX; + max[0] = -FLT_MAX; + int numVerts = hull->m_numVertices; + + const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),*dir); + float offset = b3Dot(pos,*dir); + for(int i=0;im_vertexOffset+i],localDir); + if(dp < min[0]) + min[0] = dp; + if(dp > max[0]) + max[0] = dp; + } + if(min[0]>max[0]) + { + float tmp = min[0]; + min[0] = max[0]; + max[0] = tmp; + } + min[0] += offset; + max[0] += offset; +} + + +inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, + b3Float4ConstArg posA,b3QuatConstArg ornA, + b3Float4ConstArg posB,b3QuatConstArg ornB, + b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB,float* depth) +{ + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1); + + if(Max0m_numFaces*hullB->m_numVertices; + curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices; + + if (curFaceVertexAB>maxFaceVertex) + { + maxFaceVertex = curFaceVertexAB; + printf("curFaceVertexAB = %d\n",curFaceVertexAB); + printf("hullA->m_numFaces = %d\n",hullA->m_numFaces); + printf("hullA->m_numVertices = %d\n",hullA->m_numVertices); + printf("hullB->m_numVertices = %d\n",hullB->m_numVertices); + } +*/ + + int curPlaneTests=0; + { + int numFacesA = hullA->m_numFaces; + // Test normals from hullA + for(int i=0;im_faceOffset+i].m_plane; + b3Float4 faceANormalWS = b3QuatRotate(ornA,normal); + if (b3Dot(DeltaC2,faceANormalWS)<0) + faceANormalWS*=-1.f; + curPlaneTests++; + float d; + if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d)) + return false; + if(d<*dmin) + { + *dmin = d; + *sep = faceANormalWS; + } + } + } + if((b3Dot(-DeltaC2,*sep))>0.0f) + { + *sep = -(*sep); + } + return true; +} + + +b3Vector3 unitSphere162[]= +{ + b3MakeVector3(0.000000,-1.000000,0.000000), +b3MakeVector3(0.203181,-0.967950,0.147618), +b3MakeVector3(-0.077607,-0.967950,0.238853), +b3MakeVector3(0.723607,-0.447220,0.525725), +b3MakeVector3(0.609547,-0.657519,0.442856), +b3MakeVector3(0.812729,-0.502301,0.295238), +b3MakeVector3(-0.251147,-0.967949,0.000000), +b3MakeVector3(-0.077607,-0.967950,-0.238853), +b3MakeVector3(0.203181,-0.967950,-0.147618), +b3MakeVector3(0.860698,-0.251151,0.442858), +b3MakeVector3(-0.276388,-0.447220,0.850649), +b3MakeVector3(-0.029639,-0.502302,0.864184), +b3MakeVector3(-0.155215,-0.251152,0.955422), +b3MakeVector3(-0.894426,-0.447216,0.000000), +b3MakeVector3(-0.831051,-0.502299,0.238853), +b3MakeVector3(-0.956626,-0.251149,0.147618), +b3MakeVector3(-0.276388,-0.447220,-0.850649), +b3MakeVector3(-0.483971,-0.502302,-0.716565), +b3MakeVector3(-0.436007,-0.251152,-0.864188), +b3MakeVector3(0.723607,-0.447220,-0.525725), +b3MakeVector3(0.531941,-0.502302,-0.681712), +b3MakeVector3(0.687159,-0.251152,-0.681715), +b3MakeVector3(0.687159,-0.251152,0.681715), +b3MakeVector3(-0.436007,-0.251152,0.864188), +b3MakeVector3(-0.956626,-0.251149,-0.147618), +b3MakeVector3(-0.155215,-0.251152,-0.955422), +b3MakeVector3(0.860698,-0.251151,-0.442858), +b3MakeVector3(0.276388,0.447220,0.850649), +b3MakeVector3(0.483971,0.502302,0.716565), +b3MakeVector3(0.232822,0.657519,0.716563), +b3MakeVector3(-0.723607,0.447220,0.525725), +b3MakeVector3(-0.531941,0.502302,0.681712), +b3MakeVector3(-0.609547,0.657519,0.442856), +b3MakeVector3(-0.723607,0.447220,-0.525725), +b3MakeVector3(-0.812729,0.502301,-0.295238), +b3MakeVector3(-0.609547,0.657519,-0.442856), +b3MakeVector3(0.276388,0.447220,-0.850649), +b3MakeVector3(0.029639,0.502302,-0.864184), +b3MakeVector3(0.232822,0.657519,-0.716563), +b3MakeVector3(0.894426,0.447216,0.000000), +b3MakeVector3(0.831051,0.502299,-0.238853), +b3MakeVector3(0.753442,0.657515,0.000000), +b3MakeVector3(-0.232822,-0.657519,0.716563), +b3MakeVector3(-0.162456,-0.850654,0.499995), +b3MakeVector3(0.052790,-0.723612,0.688185), +b3MakeVector3(0.138199,-0.894429,0.425321), +b3MakeVector3(0.262869,-0.525738,0.809012), +b3MakeVector3(0.361805,-0.723611,0.587779), +b3MakeVector3(0.531941,-0.502302,0.681712), +b3MakeVector3(0.425323,-0.850654,0.309011), +b3MakeVector3(0.812729,-0.502301,-0.295238), +b3MakeVector3(0.609547,-0.657519,-0.442856), +b3MakeVector3(0.850648,-0.525736,0.000000), +b3MakeVector3(0.670817,-0.723611,-0.162457), +b3MakeVector3(0.670817,-0.723610,0.162458), +b3MakeVector3(0.425323,-0.850654,-0.309011), +b3MakeVector3(0.447211,-0.894428,0.000001), +b3MakeVector3(-0.753442,-0.657515,0.000000), +b3MakeVector3(-0.525730,-0.850652,0.000000), +b3MakeVector3(-0.638195,-0.723609,0.262864), +b3MakeVector3(-0.361801,-0.894428,0.262864), +b3MakeVector3(-0.688189,-0.525736,0.499997), +b3MakeVector3(-0.447211,-0.723610,0.525729), +b3MakeVector3(-0.483971,-0.502302,0.716565), +b3MakeVector3(-0.232822,-0.657519,-0.716563), +b3MakeVector3(-0.162456,-0.850654,-0.499995), +b3MakeVector3(-0.447211,-0.723611,-0.525727), +b3MakeVector3(-0.361801,-0.894429,-0.262863), +b3MakeVector3(-0.688189,-0.525736,-0.499997), +b3MakeVector3(-0.638195,-0.723609,-0.262863), +b3MakeVector3(-0.831051,-0.502299,-0.238853), +b3MakeVector3(0.361804,-0.723612,-0.587779), +b3MakeVector3(0.138197,-0.894429,-0.425321), +b3MakeVector3(0.262869,-0.525738,-0.809012), +b3MakeVector3(0.052789,-0.723611,-0.688186), +b3MakeVector3(-0.029639,-0.502302,-0.864184), +b3MakeVector3(0.956626,0.251149,0.147618), +b3MakeVector3(0.956626,0.251149,-0.147618), +b3MakeVector3(0.951058,-0.000000,0.309013), +b3MakeVector3(1.000000,0.000000,0.000000), +b3MakeVector3(0.947213,-0.276396,0.162458), +b3MakeVector3(0.951058,0.000000,-0.309013), +b3MakeVector3(0.947213,-0.276396,-0.162458), +b3MakeVector3(0.155215,0.251152,0.955422), +b3MakeVector3(0.436007,0.251152,0.864188), +b3MakeVector3(-0.000000,-0.000000,1.000000), +b3MakeVector3(0.309017,0.000000,0.951056), +b3MakeVector3(0.138199,-0.276398,0.951055), +b3MakeVector3(0.587786,0.000000,0.809017), +b3MakeVector3(0.447216,-0.276398,0.850648), +b3MakeVector3(-0.860698,0.251151,0.442858), +b3MakeVector3(-0.687159,0.251152,0.681715), +b3MakeVector3(-0.951058,-0.000000,0.309013), +b3MakeVector3(-0.809018,0.000000,0.587783), +b3MakeVector3(-0.861803,-0.276396,0.425324), +b3MakeVector3(-0.587786,0.000000,0.809017), +b3MakeVector3(-0.670819,-0.276397,0.688191), +b3MakeVector3(-0.687159,0.251152,-0.681715), +b3MakeVector3(-0.860698,0.251151,-0.442858), +b3MakeVector3(-0.587786,-0.000000,-0.809017), +b3MakeVector3(-0.809018,-0.000000,-0.587783), +b3MakeVector3(-0.670819,-0.276397,-0.688191), +b3MakeVector3(-0.951058,0.000000,-0.309013), +b3MakeVector3(-0.861803,-0.276396,-0.425324), +b3MakeVector3(0.436007,0.251152,-0.864188), +b3MakeVector3(0.155215,0.251152,-0.955422), +b3MakeVector3(0.587786,-0.000000,-0.809017), +b3MakeVector3(0.309017,-0.000000,-0.951056), +b3MakeVector3(0.447216,-0.276398,-0.850648), +b3MakeVector3(0.000000,0.000000,-1.000000), +b3MakeVector3(0.138199,-0.276398,-0.951055), +b3MakeVector3(0.670820,0.276396,0.688190), +b3MakeVector3(0.809019,-0.000002,0.587783), +b3MakeVector3(0.688189,0.525736,0.499997), +b3MakeVector3(0.861804,0.276394,0.425323), +b3MakeVector3(0.831051,0.502299,0.238853), +b3MakeVector3(-0.447216,0.276397,0.850649), +b3MakeVector3(-0.309017,-0.000001,0.951056), +b3MakeVector3(-0.262869,0.525738,0.809012), +b3MakeVector3(-0.138199,0.276397,0.951055), +b3MakeVector3(0.029639,0.502302,0.864184), +b3MakeVector3(-0.947213,0.276396,-0.162458), +b3MakeVector3(-1.000000,0.000001,0.000000), +b3MakeVector3(-0.850648,0.525736,-0.000000), +b3MakeVector3(-0.947213,0.276397,0.162458), +b3MakeVector3(-0.812729,0.502301,0.295238), +b3MakeVector3(-0.138199,0.276397,-0.951055), +b3MakeVector3(-0.309016,-0.000000,-0.951057), +b3MakeVector3(-0.262869,0.525738,-0.809012), +b3MakeVector3(-0.447215,0.276397,-0.850649), +b3MakeVector3(-0.531941,0.502302,-0.681712), +b3MakeVector3(0.861804,0.276396,-0.425322), +b3MakeVector3(0.809019,0.000000,-0.587782), +b3MakeVector3(0.688189,0.525736,-0.499997), +b3MakeVector3(0.670821,0.276397,-0.688189), +b3MakeVector3(0.483971,0.502302,-0.716565), +b3MakeVector3(0.077607,0.967950,0.238853), +b3MakeVector3(0.251147,0.967949,0.000000), +b3MakeVector3(0.000000,1.000000,0.000000), +b3MakeVector3(0.162456,0.850654,0.499995), +b3MakeVector3(0.361800,0.894429,0.262863), +b3MakeVector3(0.447209,0.723612,0.525728), +b3MakeVector3(0.525730,0.850652,0.000000), +b3MakeVector3(0.638194,0.723610,0.262864), +b3MakeVector3(-0.203181,0.967950,0.147618), +b3MakeVector3(-0.425323,0.850654,0.309011), +b3MakeVector3(-0.138197,0.894430,0.425320), +b3MakeVector3(-0.361804,0.723612,0.587778), +b3MakeVector3(-0.052790,0.723612,0.688185), +b3MakeVector3(-0.203181,0.967950,-0.147618), +b3MakeVector3(-0.425323,0.850654,-0.309011), +b3MakeVector3(-0.447210,0.894429,0.000000), +b3MakeVector3(-0.670817,0.723611,-0.162457), +b3MakeVector3(-0.670817,0.723611,0.162457), +b3MakeVector3(0.077607,0.967950,-0.238853), +b3MakeVector3(0.162456,0.850654,-0.499995), +b3MakeVector3(-0.138197,0.894430,-0.425320), +b3MakeVector3(-0.052790,0.723612,-0.688185), +b3MakeVector3(-0.361804,0.723612,-0.587778), +b3MakeVector3(0.361800,0.894429,-0.262863), +b3MakeVector3(0.638194,0.723610,-0.262864), +b3MakeVector3(0.447209,0.723612,-0.525728) +}; + + +bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, + b3Float4ConstArg posA1, + b3QuatConstArg ornA, + b3Float4ConstArg posB1, + b3QuatConstArg ornB, + b3Float4ConstArg DeltaC2, + const b3Float4* verticesA, + const b3Float4* uniqueEdgesA, + const b3GpuFace* facesA, + const int* indicesA, + __global const b3Float4* verticesB, + __global const b3Float4* uniqueEdgesB, + __global const b3GpuFace* facesB, + __global const int* indicesB, + b3Float4* sep, + float* dmin, + bool searchAllEdgeEdge) +{ + + + b3Float4 posA = posA1; + posA.w = 0.f; + b3Float4 posB = posB1; + posB.w = 0.f; + +// int curPlaneTests=0; + + int curEdgeEdge = 0; + // Test edges + static int maxEdgeTests = 0; + int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges; + if (curEdgeTests >maxEdgeTests ) + { + maxEdgeTests = curEdgeTests ; + printf("maxEdgeTests = %d\n",maxEdgeTests ); + printf("hullA->m_numUniqueEdges = %d\n",hullA->m_numUniqueEdges); + printf("hullB->m_numUniqueEdges = %d\n",hullB->m_numUniqueEdges); + + } + + + if (searchAllEdgeEdge) + { + for(int e0=0;e0m_numUniqueEdges;e0++) + { + const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0]; + b3Float4 edge0World = b3QuatRotate(ornA,edge0); + + for(int e1=0;e1m_numUniqueEdges;e1++) + { + const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1]; + b3Float4 edge1World = b3QuatRotate(ornB,edge1); + + + b3Float4 crossje = b3Cross(edge0World,edge1World); + + curEdgeEdge++; + if(!b3IsAlmostZero(crossje)) + { + crossje = b3Normalized(crossje); + if (b3Dot(DeltaC2,crossje)<0) + crossje *= -1.f; + + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); + + if(Max00) + { + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); + + if(Max00.0f) + { + *sep = -(*sep); + } + return true; +} + + + +inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal, + __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB, + b3Float4ConstArg posA, b3QuatConstArg ornA,b3Float4ConstArg posB, b3QuatConstArg ornB, + __global b3Float4* worldVertsA1, + __global b3Float4* worldNormalsA1, + __global b3Float4* worldVertsB1, + int capacityWorldVerts, + const float minDist, float maxDist, + __global const b3Float4* verticesA, + __global const b3GpuFace_t* facesA, + __global const int* indicesA, + __global const b3Float4* verticesB, + __global const b3GpuFace_t* facesB, + __global const int* indicesB, + + __global b3Int4* clippingFaces, int pairIndex) +{ + int numContactsOut = 0; + int numWorldVertsB1= 0; + + + int closestFaceB=-1; + float dmax = -FLT_MAX; + + { + for(int face=0;facem_numFaces;face++) + { + const b3Float4 Normal = b3MakeFloat4(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); + float d = b3Dot(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + + { + const b3GpuFace_t polyB = facesB[hullB->m_faceOffset+closestFaceB]; + const int numVertices = polyB.m_numIndices; + for(int e0=0;e0m_vertexOffset+indicesB[polyB.m_indexOffset+e0]]; + worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = b3TransformPoint(b,posB,ornB); + } + } + + int closestFaceA=-1; + { + float dmin = FLT_MAX; + for(int face=0;facem_numFaces;face++) + { + const b3Float4 Normal = b3MakeFloat4( + 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 = b3Dot(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + worldNormalsA1[pairIndex] = faceANormalWS; + } + } + } + + int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices; + for(int e0=0;e0m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]]; + worldVertsA1[pairIndex*capacityWorldVerts+e0] = b3TransformPoint(a, posA,ornA); + } + + clippingFaces[pairIndex].x = closestFaceA; + clippingFaces[pairIndex].y = closestFaceB; + clippingFaces[pairIndex].z = numVerticesA; + clippingFaces[pairIndex].w = numWorldVertsB1; + + + return numContactsOut; +} + + + + +__kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs, + __global const b3RigidBodyData* rigidBodies, + __global const b3Collidable* collidables, + __global const b3ConvexPolyhedronData* convexShapes, + __global const b3Float4* vertices, + __global const b3Float4* uniqueEdges, + __global const b3GpuFace* faces, + __global const int* indices, + __global const b3GpuChildShape* gpuChildShapes, + __global b3Aabb* aabbs, + __global b3Float4* concaveSeparatingNormalsOut, + __global b3Int4* clippingFacesOut, + __global b3Vector3* worldVertsA1Out, + __global b3Vector3* worldNormalsA1Out, + __global b3Vector3* worldVertsB1Out, + __global int* hasSeparatingNormals, + int vertexFaceCapacity, + int numConcavePairs, + int pairIdx + ) +{ + int i = pairIdx; +/* int i = get_global_id(0); + if (i>=numConcavePairs) + return; + int pairIdx = i; + */ + + int bodyIndexA = concavePairs[i].x; + int bodyIndexB = concavePairs[i].y; + + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&& + collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + concavePairs[pairIdx].w = -1; + return; + } + + hasSeparatingNormals[i] = 0; + +// int numFacesA = convexShapes[shapeIndexA].m_numFaces; + int numActualConcaveConvexTests = 0; + + int f = concavePairs[i].z; + + bool overlap = false; + + b3ConvexPolyhedronData convexPolyhedronA; + + //add 3 vertices of the triangle + convexPolyhedronA.m_numVertices = 3; + convexPolyhedronA.m_vertexOffset = 0; + b3Float4 localCenter = b3MakeFloat4(0.f,0.f,0.f,0.f); + + b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f]; + b3Aabb triAabb; + triAabb.m_minVec = b3MakeFloat4(1e30f,1e30f,1e30f,0.f); + triAabb.m_maxVec = b3MakeFloat4(-1e30f,-1e30f,-1e30f,0.f); + + b3Float4 verticesA[3]; + for (int i=0;i<3;i++) + { + int index = indices[face.m_indexOffset+i]; + b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index]; + verticesA[i] = vert; + localCenter += vert; + + triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec,vert); + triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec,vert); + + } + + overlap = true; + overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap; + overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap; + overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap; + + if (overlap) + { + float dmin = FLT_MAX; + int hasSeparatingAxis=5; + b3Float4 sepAxis=b3MakeFloat4(1,2,3,4); + + // int localCC=0; + numActualConcaveConvexTests++; + + //a triangle has 3 unique edges + convexPolyhedronA.m_numUniqueEdges = 3; + convexPolyhedronA.m_uniqueEdgesOffset = 0; + b3Float4 uniqueEdgesA[3]; + + uniqueEdgesA[0] = (verticesA[1]-verticesA[0]); + uniqueEdgesA[1] = (verticesA[2]-verticesA[1]); + uniqueEdgesA[2] = (verticesA[0]-verticesA[2]); + + + convexPolyhedronA.m_faceOffset = 0; + + b3Float4 normal = b3MakeFloat4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); + + b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES]; + int indicesA[3+3+2+2+2]; + int curUsedIndices=0; + int fidx=0; + + //front size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[0] = 0; + indicesA[1] = 1; + indicesA[2] = 2; + curUsedIndices+=3; + float c = face.m_plane.w; + facesA[fidx].m_plane.x = normal.x; + facesA[fidx].m_plane.y = normal.y; + facesA[fidx].m_plane.z = normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + //back size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[3]=2; + indicesA[4]=1; + indicesA[5]=0; + curUsedIndices+=3; + float c = b3Dot(normal,verticesA[0]); + // float c1 = -face.m_plane.w; + facesA[fidx].m_plane.x = -normal.x; + facesA[fidx].m_plane.y = -normal.y; + facesA[fidx].m_plane.z = -normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + + bool addEdgePlanes = true; + if (addEdgePlanes) + { + int numVertices=3; + int prevVertex = numVertices-1; + for (int i=0;i& 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/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h new file mode 100644 index 0000000000..6c3ad7c9dd --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h @@ -0,0 +1,920 @@ + +/*** + * --------------------------------- + * Copyright (c)2012 Daniel Fiser + * + * This file was ported from mpr.c file, part of libccd. + * The Minkoski Portal Refinement implementation was ported + * to OpenCL by Erwin Coumans for the Bullet 3 Physics library. + * at http://github.com/erwincoumans/bullet3 + * + * Distributed under the OSI-approved BSD License (the "License"); + * see . + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + + + + +#ifndef B3_MPR_PENETRATION_H +#define B3_MPR_PENETRATION_H + +#include "Bullet3Common/shared/b3PlatformDefinitions.h" +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" + + + + +#ifdef __cplusplus +#define B3_MPR_SQRT sqrtf +#else +#define B3_MPR_SQRT sqrt +#endif +#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y)) +#define B3_MPR_FABS fabs + +#define B3_MPR_TOLERANCE 1E-6f +#define B3_MPR_MAX_ITERATIONS 1000 + +struct _b3MprSupport_t +{ + b3Float4 v; //!< Support point in minkowski sum + b3Float4 v1; //!< Support point in obj1 + b3Float4 v2; //!< Support point in obj2 +}; +typedef struct _b3MprSupport_t b3MprSupport_t; + +struct _b3MprSimplex_t +{ + b3MprSupport_t ps[4]; + int last; //!< index of last added point +}; +typedef struct _b3MprSimplex_t b3MprSimplex_t; + +inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx) +{ + return &s->ps[idx]; +} + +inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size) +{ + s->last = size - 1; +} + + +inline int b3MprSimplexSize(const b3MprSimplex_t *s) +{ + return s->last + 1; +} + + +inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} + +inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s) +{ + *d = *s; +} + +inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a) +{ + b3MprSupportCopy(s->ps + pos, a); +} + + +inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2) +{ + b3MprSupport_t supp; + + b3MprSupportCopy(&supp, &s->ps[pos1]); + b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]); + b3MprSupportCopy(&s->ps[pos2], &supp); +} + + +inline int b3MprIsZero(float val) +{ + return B3_MPR_FABS(val) < FLT_EPSILON; +} + + + +inline int b3MprEq(float _a, float _b) +{ + float ab; + float a, b; + + ab = B3_MPR_FABS(_a - _b); + if (B3_MPR_FABS(ab) < FLT_EPSILON) + return 1; + + a = B3_MPR_FABS(_a); + b = B3_MPR_FABS(_b); + if (b > a){ + return ab < FLT_EPSILON * b; + }else{ + return ab < FLT_EPSILON * a; + } +} + + +inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b) +{ + return b3MprEq((*a).x, (*b).x) + && b3MprEq((*a).y, (*b).y) + && b3MprEq((*a).z, (*b).z); +} + + + +inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, b3ConstArray(b3Float4) verticesA) +{ + b3Float4 supVec = b3MakeFloat4(0,0,0,0); + float maxDot = -B3_LARGE_FLOAT; + + if( 0 < hull->m_numVertices ) + { + const b3Float4 scaled = supportVec; + int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot); + return verticesA[hull->m_vertexOffset+index]; + } + + return supVec; + +} + + +B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + const b3Float4* _dir, b3Float4* outp, int logme) +{ + //dir is in worldspace, move to local space + + b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos; + b3Quat orn = cpuBodyBuf[bodyIndex].m_quat; + + b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f); + + const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir); + + + //find local support vertex + int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx; + + b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL); + __global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex]; + + b3Float4 pInA; + if (logme) + { + + + // b3Float4 supVec = b3MakeFloat4(0,0,0,0); + float maxDot = -B3_LARGE_FLOAT; + + if( 0 < hull->m_numVertices ) + { + const b3Float4 scaled = localDir; + int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot); + pInA = cpuVertices[hull->m_vertexOffset+index]; + + } + + + } else + { + pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices); + } + + //move vertex to world space + *outp = b3TransformPoint(pInA,pos,orn); + +} + +inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + const b3Float4* _dir, b3MprSupport_t *supp) +{ + b3Float4 dir; + dir = *_dir; + b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0); + dir = *_dir*-1.f; + b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0); + supp->v = supp->v1 - supp->v2; +} + + + + + + + + + +inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center) +{ + + center->v1 = cpuBodyBuf[bodyIndexA].m_pos; + center->v2 = cpuBodyBuf[bodyIndexB].m_pos; + center->v = center->v1 - center->v2; +} + +inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z) +{ + (*v).x = x; + (*v).y = y; + (*v).z = z; + (*v).w = 0.f; +} + +inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w) +{ + (*v).x += (*w).x; + (*v).y += (*w).y; + (*v).z += (*w).z; +} + +inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w) +{ + *v = *w; +} + +inline void b3MprVec3Scale(b3Float4 *d, float k) +{ + *d *= k; +} + +inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b) +{ + float dot; + + dot = b3Dot3F4(*a,*b); + return dot; +} + + +inline float b3MprVec3Len2(const b3Float4 *v) +{ + return b3MprVec3Dot(v, v); +} + +inline void b3MprVec3Normalize(b3Float4 *d) +{ + float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d)); + b3MprVec3Scale(d, k); +} + +inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b) +{ + *d = b3Cross3(*a,*b); + +} + + +inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w) +{ + *d = *v - *w; +} + +inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir) +{ + b3Float4 v2v1, v3v1; + + b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 1)->v); + b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 1)->v); + b3MprVec3Cross(dir, &v2v1, &v3v1); + b3MprVec3Normalize(dir); +} + + +inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal, + const b3Float4 *dir) +{ + float dot; + dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v); + return b3MprIsZero(dot) || dot > 0.f; +} + +inline int portalReachTolerance(const b3MprSimplex_t *portal, + const b3MprSupport_t *v4, + const b3Float4 *dir) +{ + float dv1, dv2, dv3, dv4; + float dot1, dot2, dot3; + + // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} + + dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir); + dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir); + dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir); + dv4 = b3MprVec3Dot(&v4->v, dir); + + dot1 = dv4 - dv1; + dot2 = dv4 - dv2; + dot3 = dv4 - dv3; + + dot1 = B3_MPR_FMIN(dot1, dot2); + dot1 = B3_MPR_FMIN(dot1, dot3); + + return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE; +} + +inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal, + const b3MprSupport_t *v4, + const b3Float4 *dir) +{ + float dot; + dot = b3MprVec3Dot(&v4->v, dir); + return b3MprIsZero(dot) || dot > 0.f; +} + +inline void b3ExpandPortal(b3MprSimplex_t *portal, + const b3MprSupport_t *v4) +{ + float dot; + b3Float4 v4v0; + + b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v); + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0); + if (dot > 0.f){ + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0); + if (dot > 0.f){ + b3MprSimplexSet(portal, 1, v4); + }else{ + b3MprSimplexSet(portal, 3, v4); + } + }else{ + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0); + if (dot > 0.f){ + b3MprSimplexSet(portal, 2, v4); + }else{ + b3MprSimplexSet(portal, 1, v4); + } + } +} + + + +B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + __global int* hasSepAxis, + b3MprSimplex_t *portal) +{ + b3Float4 dir, va, vb; + float dot; + int cont; + + + + // vertex 0 is center of portal + b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0)); + // vertex 0 is center of portal + b3MprSimplexSetSize(portal, 1); + + + + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){ + // Portal's center lies on origin (0,0,0) => we know that objects + // intersect but we would need to know penetration info. + // So move center little bit... + b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); + b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va); + } + + + // vertex 1 = support in direction of origin + b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Scale(&dir, -1.f); + b3MprVec3Normalize(&dir); + + + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1)); + + b3MprSimplexSetSize(portal, 2); + + // test if origin isn't outside of v1 + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir); + + + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + + // vertex 2 + b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v, + &b3MprSimplexPoint(portal, 1)->v); + if (b3MprIsZero(b3MprVec3Len2(&dir))){ + if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){ + // origin lies on v1 + return 1; + }else{ + // origin lies on v0-v1 segment + return 2; + } + } + + b3MprVec3Normalize(&dir); + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2)); + + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir); + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + b3MprSimplexSetSize(portal, 3); + + // vertex 3 direction + b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Cross(&dir, &va, &vb); + b3MprVec3Normalize(&dir); + + // it is better to form portal faces to be oriented "outside" origin + dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v); + if (dot > 0.f){ + b3MprSimplexSwap(portal, 1, 2); + b3MprVec3Scale(&dir, -1.f); + } + + while (b3MprSimplexSize(portal) < 4){ + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3)); + + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir); + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + cont = 0; + + // test if origin is outside (v1, v0, v3) - set v2 as v3 and + // continue + b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 3)->v); + dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !b3MprIsZero(dot)){ + b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3)); + cont = 1; + } + + if (!cont){ + // test if origin is outside (v3, v0, v2) - set v1 as v3 and + // continue + b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 2)->v); + dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !b3MprIsZero(dot)){ + b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3)); + cont = 1; + } + } + + if (cont){ + b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Cross(&dir, &va, &vb); + b3MprVec3Normalize(&dir); + }else{ + b3MprSimplexSetSize(portal, 4); + } + } + + return 0; +} + + +B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + b3MprSimplex_t *portal) +{ + b3Float4 dir; + b3MprSupport_t v4; + + for (int i=0;iv, + &b3MprSimplexPoint(portal, 2)->v); + b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 2)->v); + b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v); + + sum = b[0] + b[1] + b[2] + b[3]; + + if (b3MprIsZero(sum) || sum < 0.f){ + b[0] = 0.f; + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 3)->v); + b[1] = b3MprVec3Dot(&vec, &dir); + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[2] = b3MprVec3Dot(&vec, &dir); + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 2)->v); + b[3] = b3MprVec3Dot(&vec, &dir); + + sum = b[1] + b[2] + b[3]; + } + + inv = 1.f / sum; + + b3MprVec3Copy(&p1, b3mpr_vec3_origin); + b3MprVec3Copy(&p2, b3mpr_vec3_origin); + for (i = 0; i < 4; i++){ + b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1); + b3MprVec3Scale(&vec, b[i]); + b3MprVec3Add(&p1, &vec); + + b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2); + b3MprVec3Scale(&vec, b[i]); + b3MprVec3Add(&p2, &vec); + } + b3MprVec3Scale(&p1, inv); + b3MprVec3Scale(&p2, inv); + + b3MprVec3Copy(pos, &p1); + b3MprVec3Add(pos, &p2); + b3MprVec3Scale(pos, 0.5); +} + +inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b) +{ + b3Float4 ab; + b3MprVec3Sub2(&ab, a, b); + return b3MprVec3Len2(&ab); +} + +inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P, + const b3Float4 *x0, + const b3Float4 *b, + b3Float4 *witness) +{ + // The computation comes from solving equation of segment: + // S(t) = x0 + t.d + // where - x0 is initial point of segment + // - d is direction of segment from x0 (|d| > 0) + // - t belongs to <0, 1> interval + // + // Than, distance from a segment to some point P can be expressed: + // D(t) = |x0 + t.d - P|^2 + // which is distance from any point on segment. Minimization + // of this function brings distance from P to segment. + // Minimization of D(t) leads to simple quadratic equation that's + // solving is straightforward. + // + // Bonus of this method is witness point for free. + + float dist, t; + b3Float4 d, a; + + // direction of segment + b3MprVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + b3MprVec3Sub2(&a, x0, P); + + t = -1.f * b3MprVec3Dot(&a, &d); + t /= b3MprVec3Len2(&d); + + if (t < 0.f || b3MprIsZero(t)){ + dist = b3MprVec3Dist2(x0, P); + if (witness) + b3MprVec3Copy(witness, x0); + }else if (t > 1.f || b3MprEq(t, 1.f)){ + dist = b3MprVec3Dist2(b, P); + if (witness) + b3MprVec3Copy(witness, b); + }else{ + if (witness){ + b3MprVec3Copy(witness, &d); + b3MprVec3Scale(witness, t); + b3MprVec3Add(witness, x0); + dist = b3MprVec3Dist2(witness, P); + }else{ + // recycling variables + b3MprVec3Scale(&d, t); + b3MprVec3Add(&d, &a); + dist = b3MprVec3Len2(&d); + } + } + + return dist; +} + + +inline float b3MprVec3PointTriDist2(const b3Float4 *P, + const b3Float4 *x0, const b3Float4 *B, + const b3Float4 *C, + b3Float4 *witness) +{ + // Computation comes from analytic expression for triangle (x0, B, C) + // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and + // Then equation for distance is: + // D(s, t) = | T(s, t) - P |^2 + // This leads to minimization of quadratic function of two variables. + // The solution from is taken only if s is between 0 and 1, t is + // between 0 and 1 and t + s < 1, otherwise distance from segment is + // computed. + + b3Float4 d1, d2, a; + float u, v, w, p, q, r; + float s, t, dist, dist2; + b3Float4 witness2; + + b3MprVec3Sub2(&d1, B, x0); + b3MprVec3Sub2(&d2, C, x0); + b3MprVec3Sub2(&a, x0, P); + + u = b3MprVec3Dot(&a, &a); + v = b3MprVec3Dot(&d1, &d1); + w = b3MprVec3Dot(&d2, &d2); + p = b3MprVec3Dot(&a, &d1); + q = b3MprVec3Dot(&a, &d2); + r = b3MprVec3Dot(&d1, &d2); + + s = (q * r - w * p) / (w * v - r * r); + t = (-s * r - q) / w; + + if ((b3MprIsZero(s) || s > 0.f) + && (b3MprEq(s, 1.f) || s < 1.f) + && (b3MprIsZero(t) || t > 0.f) + && (b3MprEq(t, 1.f) || t < 1.f) + && (b3MprEq(t + s, 1.f) || t + s < 1.f)){ + + if (witness){ + b3MprVec3Scale(&d1, s); + b3MprVec3Scale(&d2, t); + b3MprVec3Copy(witness, x0); + b3MprVec3Add(witness, &d1); + b3MprVec3Add(witness, &d2); + + dist = b3MprVec3Dist2(witness, P); + }else{ + dist = s * s * v; + dist += t * t * w; + dist += 2.f * s * t * r; + dist += 2.f * s * p; + dist += 2.f * t * q; + dist += u; + } + }else{ + dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + b3MprVec3Copy(witness, &witness2); + } + + dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + b3MprVec3Copy(witness, &witness2); + } + } + + return dist; +} + + +B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + b3MprSimplex_t *portal, + float *depth, b3Float4 *pdir, b3Float4 *pos) +{ + b3Float4 dir; + b3MprSupport_t v4; + unsigned long iterations; + + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + + iterations = 1UL; + for (int i=0;i find penetration info + if (portalReachTolerance(portal, &v4, &dir) + || iterations ==B3_MPR_MAX_ITERATIONS) + { + *depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir); + *depth = B3_MPR_SQRT(*depth); + + if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z)) + { + + *pdir = dir; + } + b3MprVec3Normalize(pdir); + + // barycentric coordinates: + b3FindPos(portal, pos); + + + return; + } + + b3ExpandPortal(portal, &v4); + + iterations++; + } +} + +B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos) +{ + // Touching contact on portal's v1 - so depth is zero and direction + // is unimportant and pos can be guessed + *depth = 0.f; + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + + b3MprVec3Copy(dir, b3mpr_vec3_origin); + + b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1); + b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2); + b3MprVec3Scale(pos, 0.5); +} + +B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal, + float *depth, b3Float4 *dir, b3Float4 *pos) +{ + + // Origin lies on v0-v1 segment. + // Depth is distance to v1, direction also and position must be + // computed + + b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1); + b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2); + b3MprVec3Scale(pos, 0.5f); + + + b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v); + *depth = B3_MPR_SQRT(b3MprVec3Len2(dir)); + b3MprVec3Normalize(dir); +} + + + +inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB, + b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + __global int* hasSepAxis, + float *depthOut, b3Float4* dirOut, b3Float4* posOut) +{ + + b3MprSimplex_t portal; + + +// if (!hasSepAxis[pairIndex]) + // return -1; + + hasSepAxis[pairIndex] = 0; + int res; + + // Phase 1: Portal discovery + res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal); + + + //sepAxis[pairIndex] = *pdir;//or -dir? + + switch (res) + { + case 0: + { + // Phase 2: Portal refinement + + res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal); + if (res < 0) + return -1; + + // Phase 3. Penetration info + b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut); + hasSepAxis[pairIndex] = 1; + sepAxis[pairIndex] = -*dirOut; + break; + } + case 1: + { + // Touching contact on portal's v1. + b3FindPenetrTouch(&portal, depthOut, dirOut, posOut); + break; + } + case 2: + { + + b3FindPenetrSegment( &portal, depthOut, dirOut, posOut); + break; + } + default: + { + hasSepAxis[pairIndex]=0; + //if (res < 0) + //{ + // Origin isn't inside portal - no collision. + return -1; + //} + } + }; + + return 0; +}; + + + +#endif //B3_MPR_PENETRATION_H diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h new file mode 100644 index 0000000000..718222ebca --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h @@ -0,0 +1,196 @@ + +#ifndef B3_NEW_CONTACT_REDUCTION_H +#define B3_NEW_CONTACT_REDUCTION_H + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +#define GET_NPOINTS(x) (x).m_worldNormalOnB.w + + +int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg 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;i0) + { + + __global b3Float4* pointsIn = &worldVertsB2[pairIndex*vertexFaceCapacity]; + b3Float4 normal = -separatingNormals[i]; + + int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx); + + int dstIdx; + dstIdx = b3AtomicInc( nGlobalContactsOut); + +//#if 0 + b3Assert(dstIdx < contactCapacity); + if (dstIdx < contactCapacity) + { + + __global struct b3Contact4Data* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = -normal; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + int bodyA = pairs[pairIndex].x; + int bodyB = pairs[pairIndex].y; + + pairs[pairIndex].w = dstIdx; + + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB; + c->m_childIndexA =-1; + c->m_childIndexB =-1; + + switch (nReducedContacts) + { + case 4: + c->m_worldPosB[3] = pointsIn[contactIdx.w]; + case 3: + c->m_worldPosB[2] = pointsIn[contactIdx.z]; + case 2: + c->m_worldPosB[1] = pointsIn[contactIdx.y]; + case 1: + c->m_worldPosB[0] = pointsIn[contactIdx.x]; + default: + { + } + }; + + GET_NPOINTS(*c) = nReducedContacts; + + } + + +//#endif + + }// if (numContactsOut>0) + }// if (hasSeparatingAxis[i]) + }// if (im_escapeIndexOrTriangleIndex&~(y)); +} + +inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode) +{ + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0; +} + +inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode) +{ + return -rootNode->m_escapeIndexOrTriangleIndex; +} + +inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization) +{ + b3Float4 clampedPoint = b3MaxFloat4(point2,bvhAabbMin); + clampedPoint = b3MinFloat4 (clampedPoint, bvhAabbMax); + + b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization; + if (isMax) + { + out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1)); + out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1)); + out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1)); + } else + { + out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe)); + out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe)); + out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe)); + } + +} + + +inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow( + const unsigned short int* aabbMin1, + const unsigned short int* aabbMax1, + const unsigned short int* aabbMin2, + const unsigned short int* aabbMax2) +{ + //int overlap = 1; + if (aabbMin1[0] > aabbMax2[0]) + return 0; + if (aabbMax1[0] < aabbMin2[0]) + return 0; + if (aabbMin1[1] > aabbMax2[1]) + return 0; + if (aabbMax1[1] < aabbMin2[1]) + return 0; + if (aabbMin1[2] > aabbMax2[2]) + return 0; + if (aabbMax1[2] < aabbMin2[2]) + return 0; + return 1; + //overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap; + //overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap; + //overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap; + //return overlap; +} + + +#endif //B3_QUANTIZED_BVH_NODE_H diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h new file mode 100644 index 0000000000..35b5197006 --- /dev/null +++ b/thirdparty/bullet/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_t localAabb = localShapeAABB[collidableIndex]; + b3Aabb_t worldAabb; + + b3Float4 aabbAMinOut,aabbAMaxOut; + float margin = 0.f; + b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&aabbAMinOut,&aabbAMaxOut); + + worldAabb.m_minVec =aabbAMinOut; + worldAabb.m_minIndices[3] = bodyId; + worldAabb.m_maxVec = aabbAMaxOut; + worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass==0.f? 0 : 1; + worldAabbs[bodyId] = worldAabb; + } +} + +#endif //B3_UPDATE_AABBS_H -- cgit v1.2.3