diff --git a/examples/Collision/CollisionSdkC_Api.cpp b/examples/Collision/CollisionSdkC_Api.cpp index 5ef7eec9b..81e6fde0e 100644 --- a/examples/Collision/CollisionSdkC_Api.cpp +++ b/examples/Collision/CollisionSdkC_Api.cpp @@ -60,7 +60,7 @@ void plDeleteShape(plCollisionSdkHandle collisionSdkHandle, plCollisionWorldHand plCollisionObjectHandle plCreateCollisionObject( plCollisionSdkHandle collisionSdkHandle, plCollisionWorldHandle worldHandle, void* userData, int userIndex, plCollisionShapeHandle cshape ,plVector3 childPos,plQuaternion childOrn) { CollisionSdkInterface* sdk = (CollisionSdkInterface*) collisionSdkHandle; - return sdk->createCollisionObject(userData, userIndex, cshape, childPos, childOrn); + return sdk->createCollisionObject(worldHandle, userData, userIndex, cshape, childPos, childOrn); } @@ -73,7 +73,7 @@ void plDeleteCollisionObject(plCollisionSdkHandle collisionSdkHandle, plCollisio void plSetCollisionObjectTransform( plCollisionSdkHandle collisionSdkHandle, plCollisionWorldHandle worldHandle, plCollisionObjectHandle objHandle, plVector3 position,plQuaternion orientation) { CollisionSdkInterface* sdk = (CollisionSdkInterface*) collisionSdkHandle; - sdk->setCollisionObjectTransform(objHandle,position,orientation); + sdk->setCollisionObjectTransform(worldHandle,objHandle,position,orientation); } void plAddCollisionObject(plCollisionSdkHandle collisionSdkHandle, plCollisionWorldHandle world, plCollisionObjectHandle object) diff --git a/examples/Collision/Internal/Bullet2CollisionSdk.cpp b/examples/Collision/Internal/Bullet2CollisionSdk.cpp index 825d93885..27d0da9bd 100644 --- a/examples/Collision/Internal/Bullet2CollisionSdk.cpp +++ b/examples/Collision/Internal/Bullet2CollisionSdk.cpp @@ -92,7 +92,7 @@ void Bullet2CollisionSdk::removeCollisionObject(plCollisionWorldHandle worldHand } } -plCollisionObjectHandle Bullet2CollisionSdk::createCollisionObject( void* userPointer, int userIndex, plCollisionShapeHandle shapeHandle , +plCollisionObjectHandle Bullet2CollisionSdk::createCollisionObject( plCollisionWorldHandle worldHandle, void* userPointer, int userIndex, plCollisionShapeHandle shapeHandle , plVector3 startPosition,plQuaternion startOrientation ) { @@ -118,7 +118,7 @@ void Bullet2CollisionSdk::deleteCollisionObject(plCollisionObjectHandle bodyHand btCollisionObject* colObj = (btCollisionObject*) bodyHandle; delete colObj; } -void Bullet2CollisionSdk::setCollisionObjectTransform(plCollisionObjectHandle bodyHandle, +void Bullet2CollisionSdk::setCollisionObjectTransform(plCollisionWorldHandle /*worldHandle*/, plCollisionObjectHandle bodyHandle, plVector3 position,plQuaternion orientation ) { btCollisionObject* colObj = (btCollisionObject*) bodyHandle; diff --git a/examples/Collision/Internal/Bullet2CollisionSdk.h b/examples/Collision/Internal/Bullet2CollisionSdk.h index b7a93c661..38418b54b 100644 --- a/examples/Collision/Internal/Bullet2CollisionSdk.h +++ b/examples/Collision/Internal/Bullet2CollisionSdk.h @@ -24,10 +24,10 @@ public: virtual void addCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object); virtual void removeCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object); - virtual plCollisionObjectHandle createCollisionObject( void* userPointer, int userIndex, plCollisionShapeHandle cshape , + virtual plCollisionObjectHandle createCollisionObject( plCollisionWorldHandle worldHandle, void* userPointer, int userIndex, plCollisionShapeHandle cshape , plVector3 startPosition,plQuaternion startOrientation ); virtual void deleteCollisionObject(plCollisionObjectHandle body); - virtual void setCollisionObjectTransform(plCollisionObjectHandle body, + virtual void setCollisionObjectTransform(plCollisionWorldHandle world, plCollisionObjectHandle body, plVector3 position,plQuaternion orientation ); virtual int collide(plCollisionWorldHandle world,plCollisionObjectHandle colA, plCollisionObjectHandle colB, diff --git a/examples/Collision/Internal/CollisionSdkInterface.h b/examples/Collision/Internal/CollisionSdkInterface.h index d7065e2c0..25f844ef5 100644 --- a/examples/Collision/Internal/CollisionSdkInterface.h +++ b/examples/Collision/Internal/CollisionSdkInterface.h @@ -22,10 +22,10 @@ public: virtual void addCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object)=0; virtual void removeCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object)=0; - virtual plCollisionObjectHandle createCollisionObject( void* userPointer, int userIndex, plCollisionShapeHandle cshape , + virtual plCollisionObjectHandle createCollisionObject( plCollisionWorldHandle worldHandle, void* userPointer, int userIndex, plCollisionShapeHandle cshape , plVector3 startPosition,plQuaternion startOrientation )=0; virtual void deleteCollisionObject(plCollisionObjectHandle body)=0; - virtual void setCollisionObjectTransform(plCollisionObjectHandle body, + virtual void setCollisionObjectTransform(plCollisionWorldHandle world, plCollisionObjectHandle body, plVector3 position,plQuaternion orientation )=0; virtual int collide(plCollisionWorldHandle world,plCollisionObjectHandle colA, plCollisionObjectHandle colB, diff --git a/examples/Collision/Internal/RealTimeBullet3CollisionSdk.cpp b/examples/Collision/Internal/RealTimeBullet3CollisionSdk.cpp index 27b52fe19..3686e2005 100644 --- a/examples/Collision/Internal/RealTimeBullet3CollisionSdk.cpp +++ b/examples/Collision/Internal/RealTimeBullet3CollisionSdk.cpp @@ -1,18 +1,39 @@ +#define BLAAT #include "RealTimeBullet3CollisionSdk.h" #include "Bullet3Common/b3AlignedObjectArray.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" + +enum RTB3ShapeTypes +{ + RTB3_SHAPE_SPHERE=0, + RTB3_SHAPE_PLANE, +// RTB3_SHAPE_CAPSULE, + MAX_NUM_SINGLE_SHAPE_TYPES, + RTB3_SHAPE_COMPOUND_INTERNAL + + +}; + +//we start at 1, so that the 0 index is 'invalid' just like a nullptr +#define START_COLLIDABLE_INDEX 1 struct RTB3CollisionWorld { + b3AlignedObjectArray m_collidableUserPointers; + b3AlignedObjectArray m_collidableUserIndices; + b3AlignedObjectArray m_collidablePositions; + b3AlignedObjectArray m_collidableOrientations; b3AlignedObjectArray m_collidables; b3AlignedObjectArray m_childShapes; + b3AlignedObjectArray m_planeFaces; b3AlignedObjectArray m_compoundOverlappingPairs; int m_nextFreeShapeIndex; int m_nextFreeCollidableIndex; RTB3CollisionWorld() - :m_nextFreeCollidableIndex(0), - m_nextFreeShapeIndex(0) + :m_nextFreeCollidableIndex(START_COLLIDABLE_INDEX), + m_nextFreeShapeIndex(START_COLLIDABLE_INDEX) { } }; @@ -40,6 +61,10 @@ plCollisionWorldHandle RealTimeBullet3CollisionSdk::createCollisionWorld(int max { RTB3CollisionWorld* world = new RTB3CollisionWorld(); world->m_collidables.resize(maxNumObjsCapacity); + world->m_collidablePositions.resize(maxNumObjsCapacity); + world->m_collidableOrientations.resize(maxNumObjsCapacity); + world->m_collidableUserPointers.resize(maxNumObjsCapacity); + world->m_collidableUserIndices.resize(maxNumObjsCapacity); world->m_childShapes.resize(maxNumShapesCapacity); world->m_compoundOverlappingPairs.resize(maxNumPairsCapacity); @@ -61,12 +86,22 @@ void RealTimeBullet3CollisionSdk::deleteCollisionWorld(plCollisionWorldHandle wo plCollisionShapeHandle RealTimeBullet3CollisionSdk::createSphereShape(plCollisionWorldHandle worldHandle, plReal radius) { - int index = 10; - return (plCollisionShapeHandle) index; + RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle; + if (world->m_nextFreeShapeIndexm_childShapes.size()) + { + b3GpuChildShape& shape = world->m_childShapes[world->m_nextFreeShapeIndex]; + shape.m_childPosition.setZero(); + shape.m_childOrientation.setValue(0,0,0,1); + shape.m_radius = radius; + shape.m_shapeType = RTB3_SHAPE_SPHERE; + return (plCollisionShapeHandle) world->m_nextFreeShapeIndex++; + } + return 0; } void RealTimeBullet3CollisionSdk::deleteShape(plCollisionWorldHandle worldHandle, plCollisionShapeHandle shape) { + ///todo //deleting shapes would involve a garbage collection phase, and mess up all user indices //this would be solved by one more in-direction, at some performance penalty for certain operations //for now, we don't delete and eventually run out-of-shapes @@ -74,36 +109,238 @@ void RealTimeBullet3CollisionSdk::deleteShape(plCollisionWorldHandle worldHandle void RealTimeBullet3CollisionSdk::addCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object) { + ///createCollisionObject already adds it to the world } void RealTimeBullet3CollisionSdk::removeCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object) { + ///todo, see deleteShape } -plCollisionObjectHandle RealTimeBullet3CollisionSdk::createCollisionObject( void* userPointer, int userIndex, plCollisionShapeHandle cshape , +plCollisionObjectHandle RealTimeBullet3CollisionSdk::createCollisionObject( plCollisionWorldHandle worldHandle, void* userPointer, + int userIndex, plCollisionShapeHandle shapeHandle , plVector3 startPosition,plQuaternion startOrientation ) { + RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle; + if (world->m_nextFreeCollidableIndex < world->m_collidables.size()) + { + b3Collidable& collidable = world->m_collidables[world->m_nextFreeCollidableIndex]; + world->m_collidablePositions[world->m_nextFreeCollidableIndex].setValue(startPosition[0],startPosition[1],startPosition[2]); + world->m_collidableOrientations[world->m_nextFreeCollidableIndex].setValue(startOrientation[0],startOrientation[1],startOrientation[2],startOrientation[3]); + world->m_collidableUserPointers[world->m_nextFreeCollidableIndex] = userPointer; + world->m_collidableUserIndices[world->m_nextFreeCollidableIndex] = userIndex; + int shapeIndex = (int)shapeHandle; + collidable.m_shapeIndex = shapeIndex; + b3GpuChildShape& shape = world->m_childShapes[shapeIndex]; + collidable.m_shapeType = shape.m_shapeType; + collidable.m_numChildShapes = 1; + + switch (collidable.m_shapeType) + { + case RTB3_SHAPE_SPHERE: + { + break; + } + case RTB3_SHAPE_PLANE: + { + b3Assert(0); + break; + } + case RTB3_SHAPE_COMPOUND_INTERNAL: + { + b3Assert(0); + break; + } + default: + { + b3Assert(0); + } + } + + /*case SHAPE_COMPOUND_OF_CONVEX_HULLS: + case SHAPE_COMPOUND_OF_SPHERES: + case SHAPE_COMPOUND_OF_CAPSULES: + { + collidable.m_numChildShapes = shape.m_numChildShapes; + collidable.m_shapeIndex = shape.m_shapeIndex; + break; + */ + return (plCollisionObjectHandle)world->m_nextFreeCollidableIndex++; + } return 0; } void RealTimeBullet3CollisionSdk::deleteCollisionObject(plCollisionObjectHandle body) { + ///todo, see deleteShape } -void RealTimeBullet3CollisionSdk::setCollisionObjectTransform(plCollisionObjectHandle body, +void RealTimeBullet3CollisionSdk::setCollisionObjectTransform(plCollisionWorldHandle world, plCollisionObjectHandle body, plVector3 position,plQuaternion orientation ) { } - -int RealTimeBullet3CollisionSdk::collide(plCollisionWorldHandle world,plCollisionObjectHandle colA, plCollisionObjectHandle colB, - lwContactPoint* pointsOut, int pointCapacity) + +struct plContactCache { + lwContactPoint* pointsOut; + int pointCapacity; + int numAddedPoints; +}; + +typedef void (*plDetectCollisionFunc)(RTB3CollisionWorld* world,int colA, int shapeIndexA, int colB, int shapeIndexB, + plContactCache* contactCache); + +void detectCollisionDummy(RTB3CollisionWorld* world,int colA, int shapeIndexA, int colB, int shapeIndexB, + plContactCache* contactCache) +{ + (void)world; + (void)colA,(void)colB; + (void)contactCache; +} + +void plVecCopy(b3Scalar* dst,const b3Vector3& src) +{ + dst[0] = src.x; + dst[1] = src.y; + dst[2] = src.z; +} + +void ComputeClosestPointsPlaneSphere(const b3Vector3& planeNormalWorld, b3Scalar planeConstant, const b3Vector3& spherePosWorld,b3Scalar sphereRadius, plContactCache* contactCache) +{ + if (contactCache->numAddedPoints < contactCache->pointCapacity) + { + lwContactPoint& pointOut = contactCache->pointsOut[contactCache->numAddedPoints]; + b3Scalar t = -(spherePosWorld.dot(-planeNormalWorld)+planeConstant); + b3Vector3 intersectionPoint = spherePosWorld+t*-planeNormalWorld; + b3Scalar distance = t-sphereRadius; + if (distance<=0) + { + pointOut.m_distance = distance; + plVecCopy(pointOut.m_ptOnBWorld,intersectionPoint); + plVecCopy(pointOut.m_ptOnAWorld,spherePosWorld+sphereRadius*-planeNormalWorld); + plVecCopy(pointOut.m_normalOnB,planeNormalWorld); + contactCache->numAddedPoints++; + } + } +} + +void ComputeClosestPointsSphereSphere(b3Scalar sphereARadius, const b3Vector3& sphereAPosWorld, b3Scalar sphereBRadius, const b3Vector3& sphereBPosWorld, plContactCache* contactCache) { + + if (contactCache->numAddedPoints < contactCache->pointCapacity) + { + lwContactPoint& pointOut = contactCache->pointsOut[contactCache->numAddedPoints]; + b3Vector3 diff = sphereAPosWorld-sphereBPosWorld; + b3Scalar len = diff.length(); + pointOut.m_distance = len - (sphereARadius+sphereBRadius); + if (pointOut.m_distance<=0) + { + b3Vector3 normOnB = b3MakeVector3(1,0,0); + if (len > B3_EPSILON) { + normOnB = diff / len; + } + plVecCopy(pointOut.m_normalOnB,normOnB); + b3Vector3 ptAWorld = sphereAPosWorld - sphereARadius*normOnB; + plVecCopy(pointOut.m_ptOnAWorld,ptAWorld); + plVecCopy(pointOut.m_ptOnBWorld,ptAWorld-normOnB*pointOut.m_distance); + contactCache->numAddedPoints++; + } + } +} + +void detectCollisionSphereSphere(RTB3CollisionWorld* world,int colA, int shapeIndexA, int colB, int shapeIndexB, + plContactCache* contactCache) +{ + b3Scalar radiusA = world->m_childShapes[shapeIndexA].m_radius; + b3Scalar radiusB = world->m_childShapes[shapeIndexB].m_radius; + b3Transform trA(world->m_collidableOrientations[colA],world->m_collidablePositions[colA]); + const b3Vector3& sphereALocalPos = world->m_childShapes[shapeIndexA].m_childPosition; + b3Vector3 spherePosAWorld = trA(sphereALocalPos); + b3Transform trB(world->m_collidableOrientations[colB],world->m_collidablePositions[colB]); + const b3Vector3& sphereBLocalPos = world->m_childShapes[shapeIndexB].m_childPosition; + b3Vector3 spherePosBWorld = trB(sphereALocalPos); + ComputeClosestPointsSphereSphere(radiusA,spherePosAWorld,radiusB,spherePosBWorld,contactCache); +} + +void detectCollisionSpherePlane(RTB3CollisionWorld* world,int colA, int shapeIndexA, int colB, int shapeIndexB, + plContactCache* contactCache) +{ + (void)world; + (void)colA,(void)shapeIndexA,(void)colB,(void)shapeIndexB; + (void)contactCache; +} + +void detectCollisionPlaneSphere(RTB3CollisionWorld* world,int colA, int shapeIndexA, int colB, int shapeIndexB, + plContactCache* contactCache) +{ + (void)world; + (void)colA,(void)shapeIndexA,(void)colB,(void)shapeIndexB; + (void)contactCache; +} + + + +#ifdef RTB3_SHAPE_CAPSULE +plDetectCollisionFunc funcTbl_detectCollision[MAX_NUM_SINGLE_SHAPE_TYPES,][MAX_NUM_SINGLE_SHAPE_TYPES,] = { + {detectCollisionSphereSphere ,detectCollisionSpherePlane ,detectCollisionSphereCapsule}, + {detectCollisionPlaneSphere ,detectCollisionDummy ,detectCollisionPlaneCapsule}, + {detectCollisionCapsuleSphere ,detectCollisionCapsulePlane ,detectCollisionCapsuleCapsule}, +}; +#else +plDetectCollisionFunc funcTbl_detectCollision[MAX_NUM_SINGLE_SHAPE_TYPES][MAX_NUM_SINGLE_SHAPE_TYPES] = { + {detectCollisionSphereSphere ,detectCollisionSpherePlane}, + {detectCollisionPlaneSphere ,detectCollisionDummy }, +}; + +#endif + +int RealTimeBullet3CollisionSdk::collide(plCollisionWorldHandle worldHandle,plCollisionObjectHandle colAHandle, plCollisionObjectHandle colBHandle, + lwContactPoint* pointsOutOrg, int pointCapacity) +{ + RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle; + int colAIndex = (int) colAHandle; + int colBIndex = (int) colBHandle; + const b3Collidable& colA = world->m_collidables[colAIndex]; + const b3Collidable& colB = world->m_collidables[colBIndex]; + + plContactCache contactCache; + contactCache.pointCapacity = pointCapacity; + contactCache.pointsOut = pointsOutOrg; + contactCache.numAddedPoints = 0; + int remainingCapacity = pointCapacity; + + for (int i=0;im_childShapes[colA.m_shapeIndex+i].m_shapeType] + [world->m_childShapes[colB.m_shapeIndex+j].m_shapeType](world,colAIndex,colA.m_shapeIndex+i,colBIndex,colB.m_shapeIndex+j,&contactCache); + } + } + return contactCache.numAddedPoints; + } + return 0; } -void RealTimeBullet3CollisionSdk::collideWorld( plCollisionWorldHandle world, +void RealTimeBullet3CollisionSdk::collideWorld( plCollisionWorldHandle worldHandle, plNearCallback filter, void* userData) { + RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle; + if (filter) + { + for (int i=START_COLLIDABLE_INDEX;im_nextFreeCollidableIndex;i++) + { + for (int j=i+1;jm_nextFreeCollidableIndex;j++) + { + plCollisionObjectHandle colA = (plCollisionObjectHandle)i; + plCollisionObjectHandle colB = (plCollisionObjectHandle)j; + filter((plCollisionSdkHandle)this,worldHandle,userData,colA,colB); + } + } + } } plCollisionSdkHandle RealTimeBullet3CollisionSdk::createRealTimeBullet3CollisionSdkHandle() diff --git a/examples/Collision/Internal/RealTimeBullet3CollisionSdk.h b/examples/Collision/Internal/RealTimeBullet3CollisionSdk.h index b153f50c3..b04f24d7b 100644 --- a/examples/Collision/Internal/RealTimeBullet3CollisionSdk.h +++ b/examples/Collision/Internal/RealTimeBullet3CollisionSdk.h @@ -23,10 +23,10 @@ public: virtual void addCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object); virtual void removeCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object); - virtual plCollisionObjectHandle createCollisionObject( void* userPointer, int userIndex, plCollisionShapeHandle cshape , + virtual plCollisionObjectHandle createCollisionObject( plCollisionWorldHandle worldHandle, void* userPointer, int userIndex, plCollisionShapeHandle cshape , plVector3 startPosition,plQuaternion startOrientation ); virtual void deleteCollisionObject(plCollisionObjectHandle body); - virtual void setCollisionObjectTransform(plCollisionObjectHandle body, + virtual void setCollisionObjectTransform(plCollisionWorldHandle world, plCollisionObjectHandle body, plVector3 position,plQuaternion orientation ); virtual int collide(plCollisionWorldHandle world,plCollisionObjectHandle colA, plCollisionObjectHandle colB, diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h index 0ef4d97a7..5808bdff0 100644 --- a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h +++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h @@ -15,9 +15,6 @@ enum b3ShapeTypes SHAPE_CONCAVE_TRIMESH=5, SHAPE_COMPOUND_OF_CONVEX_HULLS=6, SHAPE_SPHERE=7, - SHAPE_CAPSULE=8, - SHAPE_COMPOUND_OF_SPHERES=9, - SHAPE_COMPOUND_OF_CAPSULES=10, MAX_NUM_SHAPE_TYPES, }; @@ -37,7 +34,11 @@ struct b3Collidable }; int m_shapeType; - int m_shapeIndex; + union + { + int m_shapeIndex; + float m_height; + }; }; typedef struct b3GpuChildShape b3GpuChildShape_t; @@ -46,9 +47,17 @@ struct b3GpuChildShape b3Float4 m_childPosition; b3Quat m_childOrientation; int m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS - float m_radius;//used for SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES - float m_height;//used for SHAPE_COMPOUND_OF_CAPSULES - int m_unused2; + union + { + float m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES + int m_numChildShapes;//used for compound shape + }; + union + { + float m_height;//used for childshape of SHAPE_COMPOUND_OF_CAPSULES + int m_collidableShapeIndex; + }; + int m_shapeType; }; struct b3CompoundOverlappingPair