diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 435ffaa84..21c7ee6ab 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -189,140 +189,242 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra const btTransform& colObjWorldTransform, RayResultCallback& resultCallback,short int collisionFilterMask) { - btSphereShape pointShape(btScalar(0.0)); pointShape.setMargin(0.f); - - objectQuerySingle(&pointShape,rayFromTrans,rayToTrans, - collisionObject, - collisionShape, - colObjWorldTransform, - resultCallback,collisionFilterMask); -} - -void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback,short int collisionFilterMask) -{ - + const btConvexShape* castShape = &pointShape; if (collisionShape->isConvex()) - { - btConvexCast::CastResult castResult; - castResult.m_fraction = btScalar(1.);//?? + { + btConvexCast::CastResult castResult; + castResult.m_fraction = btScalar(1.);//?? - btConvexShape* convexShape = (btConvexShape*) collisionShape; - btVoronoiSimplexSolver simplexSolver; -#define USE_SUBSIMPLEX_CONVEX_CAST 1 + btConvexShape* convexShape = (btConvexShape*) collisionShape; + btVoronoiSimplexSolver simplexSolver; +#define USE_SUBSIMPLEX_CONVEX_CAST 1 #ifdef USE_SUBSIMPLEX_CONVEX_CAST btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver); #else - //btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver); - //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); + //btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver); + //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); #endif //#USE_SUBSIMPLEX_CONVEX_CAST - if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < resultCallback.m_closestHitFraction) { - //add hit - if (castResult.m_normal.length2() > btScalar(0.0001)) - { - - if (castResult.m_fraction < resultCallback.m_closestHitFraction) - { #ifdef USE_SUBSIMPLEX_CONVEX_CAST - //rotate normal into worldspace - castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal; + //rotate normal into worldspace + castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal; #endif //USE_SUBSIMPLEX_CONVEX_CAST - castResult.m_normal.normalize(); - btCollisionWorld::LocalRayResult localRayResult + castResult.m_normal.normalize(); + btCollisionWorld::LocalRayResult localRayResult + ( + collisionObject, + 0, + castResult.m_normal, + castResult.m_fraction + ); + + bool normalInWorldSpace = true; + resultCallback.AddSingleResult(localRayResult, normalInWorldSpace); + + } + } + } + } else { + if (collisionShape->isConcave()) + { + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + //ConvexCast::CastResult + struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + { + btCollisionWorld::RayResultCallback* m_resultCallback; + btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; + + BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): + btTriangleRaycastCallback(from,to), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } + + + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + + btCollisionWorld::LocalRayResult rayResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitFraction); + + bool normalInWorldSpace = false; + return m_resultCallback->AddSingleResult(rayResult,normalInWorldSpace); + } + + }; + + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); + } else { + //todo: use AABB tree or other BVH acceleration structure! + if (collisionShape->isCompound()) + { + const btCompoundShape* compoundShape = static_cast(collisionShape); + int i=0; + for (i=0;igetNumChildShapes();i++) + { + btTransform childTrans = compoundShape->getChildTransform(i); + const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); + btTransform childWorldTrans = colObjWorldTransform * childTrans; + rayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + childCollisionShape, + childWorldTrans, + resultCallback, collisionFilterMask); + + } + } + } + } +} + +void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback,short int collisionFilterMask) +{ + if (collisionShape->isConvex()) + { + btConvexCast::CastResult castResult; + castResult.m_fraction = btScalar(1.);//?? + + btConvexShape* convexShape = (btConvexShape*) collisionShape; + btVoronoiSimplexSolver simplexSolver; +#define USE_SUBSIMPLEX_CONVEX_CAST 1 +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver); +#else + //btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver); + //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); +#endif //#USE_SUBSIMPLEX_CONVEX_CAST + + if (convexCaster.calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < resultCallback.m_closestHitFraction) + { +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + //rotate normal into worldspace + castResult.m_normal = convexFromTrans.getBasis() * castResult.m_normal; +#endif //USE_SUBSIMPLEX_CONVEX_CAST + + castResult.m_normal.normalize(); + btCollisionWorld::LocalConvexResult localConvexResult ( collisionObject, 0, castResult.m_normal, + btVector3(6,6,6), // FIXME need real hitpoint castResult.m_fraction ); - bool normalInWorldSpace = true; - resultCallback.AddSingleResult(localRayResult, normalInWorldSpace); + bool normalInWorldSpace = true; + resultCallback.AddSingleResult(localConvexResult, normalInWorldSpace); - } - } } } - else + } + } else { + if (collisionShape->isConcave()) + { + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); + btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); + btTransform rotationXform; + + rotationXform.setIdentity (); // FIXME!!! + + //ConvexCast::CastResult + struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback { - if (collisionShape->isConcave()) - { - btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; - btTransform worldTocollisionObject = colObjWorldTransform.inverse(); - btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); - btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + btCollisionWorld::ConvexResultCallback* m_resultCallback; + btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; - //ConvexCast::CastResult - struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback - { - btCollisionWorld::RayResultCallback* m_resultCallback; - btCollisionObject* m_collisionObject; - btTriangleMeshShape* m_triangleMesh; - - BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): - btTriangleRaycastCallback(from,to), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) - { - } - - - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; - - btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitFraction); - - bool normalInWorldSpace = false; - return m_resultCallback->AddSingleResult(rayResult,normalInWorldSpace); - } - - }; - - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); - rcb.m_hitFraction = resultCallback.m_closestHitFraction; - triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); - } else + BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, + btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): + btTriangleConvexcastCallback(castShape, from,to, triangleToWorld), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) { - //todo: use AABB tree or other BVH acceleration structure! - if (collisionShape->isCompound()) - { - const btCompoundShape* compoundShape = static_cast(collisionShape); - int i=0; - for (i=0;igetNumChildShapes();i++) - { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); - btTransform childWorldTrans = colObjWorldTransform * childTrans; - objectQuerySingle(castShape, rayFromTrans,rayToTrans, - collisionObject, - childCollisionShape, - childWorldTrans, - resultCallback, collisionFilterMask); - - } - - - } } + + + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + + btCollisionWorld::LocalConvexResult convexResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); + + bool normalInWorldSpace = false; + return m_resultCallback->AddSingleResult(convexResult,normalInWorldSpace); + } + + }; + + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform); + tccb.m_hitFraction = resultCallback.m_closestHitFraction; + btVector3 boxMinLocal, boxMaxLocal; + castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); + triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); + } else { + //todo: use AABB tree or other BVH acceleration structure! + if (collisionShape->isCompound()) + { + const btCompoundShape* compoundShape = static_cast(collisionShape); + int i=0; + for (i=0;igetNumChildShapes();i++) + { + btTransform childTrans = compoundShape->getChildTransform(i); + const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); + btTransform childWorldTrans = colObjWorldTransform * childTrans; + objectQuerySingle(castShape, convexFromTrans,convexToTrans, + collisionObject, + childCollisionShape, + childWorldTrans, + resultCallback, collisionFilterMask); + } } + } + } } void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback,short int collisionFilterMask) @@ -362,3 +464,42 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r } } + +void btCollisionWorld::convexTest(const btConvexShape* castShape, const btVector3& convexFromWorld, const btVector3& convexToWorld, ConvexResultCallback& resultCallback,short int collisionFilterMask) +{ + btTransform convexFromTrans,convexToTrans; + convexFromTrans.setIdentity(); + convexFromTrans.setOrigin(convexFromWorld); + convexToTrans.setIdentity(); + convexToTrans.setOrigin(convexToWorld); + btVector3 castShapeAabbMin, castShapeAabbMax; + btTransform I; + I.setIdentity(); + castShape->getAabb (I, castShapeAabbMin, castShapeAabbMax); + + /// go over all objects, and if the ray intersects their aabb + cast shape aabb, + // do a ray-shape query using convexCaster (CCD) + int i; + for (i=0;igetBroadphaseHandle()->m_collisionFilterGroup & collisionFilterMask) { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); + AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 hitNormal; + if (btRayAabb(convexFromWorld,convexToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + { + objectQuerySingle(castShape, convexFromTrans,convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback); + } + } + } + +} diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index bda03cced..ca6ada4c8 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -204,7 +204,83 @@ public: }; - + struct LocalConvexResult + { + LocalConvexResult(btCollisionObject* hitCollisionObject, + LocalShapeInfo* localShapeInfo, + const btVector3& hitNormalLocal, + const btVector3& hitPointLocal, + btScalar hitFraction + ) + :m_hitCollisionObject(hitCollisionObject), + m_localShapeInfo(localShapeInfo), + m_hitNormalLocal(hitNormalLocal), + m_hitPointLocal(hitPointLocal), + m_hitFraction(hitFraction) + { + } + + btCollisionObject* m_hitCollisionObject; + LocalShapeInfo* m_localShapeInfo; + btVector3 m_hitNormalLocal; + btVector3 m_hitPointLocal; + btScalar m_hitFraction; + }; + + ///RayResultCallback is used to report new raycast results + struct ConvexResultCallback + { + virtual ~ConvexResultCallback() + { + } + btScalar m_closestHitFraction; + bool HasHit() + { + return (m_closestHitFraction < btScalar(1.)); + } + + ConvexResultCallback() + :m_closestHitFraction(btScalar(1.)) + { + } + virtual btScalar AddSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0; + }; + + struct ClosestConvexResultCallback : public ConvexResultCallback + { + ClosestConvexResultCallback(const btVector3& convexFromWorld,const btVector3& convexToWorld) + :m_convexFromWorld(convexFromWorld), + m_convexToWorld(convexToWorld), + m_hitCollisionObject(0) + { + } + + btVector3 m_convexFromWorld;//used to calculate hitPointWorld from hitFraction + btVector3 m_convexToWorld; + + btVector3 m_hitNormalWorld; + btVector3 m_hitPointWorld; + btCollisionObject* m_hitCollisionObject; + + virtual btScalar AddSingleResult(LocalConvexResult& rayResult,bool normalInWorldSpace) + { +//caller already does the filter on the m_closestHitFraction + assert(rayResult.m_hitFraction <= m_closestHitFraction); + + m_closestHitFraction = rayResult.m_hitFraction; + m_hitCollisionObject = rayResult.m_hitCollisionObject; + if (normalInWorldSpace) + { + m_hitNormalWorld = rayResult.m_hitNormalLocal; + } else + { + ///need to transform normal into worldspace + m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; + } + m_hitPointWorld.setInterpolate3(m_convexFromWorld,m_convexToWorld,rayResult.m_hitFraction); + return rayResult.m_hitFraction; + } + }; int getNumCollisionObjects() const { @@ -215,6 +291,10 @@ public: /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback, short int collisionFilterMask=-1); + // convexTest performs a linear convex cast on all objects in the btCollisionWorld, and calls the resultCallback + // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback. + void convexTest (const btConvexShape* castShape, const btVector3& from, const btVector3& to, ConvexResultCallback& resultCallback, short int collisionFilterMask=-1); + /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. /// This allows more customization. @@ -229,7 +309,7 @@ public: btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback, short int collisionFilterMask=-1); + ConvexResultCallback& resultCallback, short int collisionFilterMask=-1); void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=1,short int collisionFilterMask=1); diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index 83f1ccf58..e8e0347e9 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -151,6 +151,66 @@ void btBvhTriangleMeshShape::performRaycast (btTriangleRaycastCallback* callback m_bvh->reportRayOverlappingNodex(&myNodeCallback,raySource,rayTarget); } +void btBvhTriangleMeshShape::performConvexcast (btTriangleConvexcastCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) +{ + struct MyNodeOverlapCallback : public btNodeOverlapCallback + { + btStridingMeshInterface* m_meshInterface; + btTriangleConvexcastCallback* m_callback; + + MyNodeOverlapCallback(btTriangleConvexcastCallback* callback,btStridingMeshInterface* meshInterface) + :m_meshInterface(meshInterface), + m_callback(callback) + { + } + + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) + { + btVector3 m_triangle[3]; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + nodeSubPart); + + int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); + btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); + + const btVector3& meshScaling = m_meshInterface->getScaling(); + for (int j=2;j>=0;j--) + { + int graphicsindex = indicestype==PHY_SHORT?((short*)gfxbase)[j]:gfxbase[j]; + + btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + } + + /* Perform ray vs. triangle collision here */ + m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); + } + }; + + MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + + m_bvh->reportBoxCastOverlappingNodex (&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax); +} + //perform bvh tree traversal and report overlapping triangles to 'callback' void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index a73642f5f..1812f1ce6 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -28,7 +28,7 @@ ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape btOptimizedBvh* m_bvh; bool m_useQuantizedAabbCompression; - bool m_ownsBvh; + bool m_ownsBvh; bool m_pad[11];////need padding due to alignment public: @@ -53,6 +53,7 @@ public: void performRaycast (btTriangleRaycastCallback* callback, const btVector3& raySource, const btVector3& rayTarget); + void performConvexcast (btTriangleConvexcastCallback* callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax); virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 1417bcbcc..99f1f69d3 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -741,7 +741,7 @@ void btOptimizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantize -void btOptimizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, int startNodeIndex,int endNodeIndex) const +void btOptimizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const { btAssert(m_useQuantization); @@ -776,6 +776,10 @@ void btOptimizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* rayAabbMin.setMin(rayTarget); rayAabbMax.setMax(rayTarget); + /* Add box cast extents to bounding box */ + rayAabbMin += aabbMin; + rayAabbMax += aabbMax; + unsigned short int quantizedQueryAabbMin[3]; unsigned short int quantizedQueryAabbMax[3]; quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin); @@ -815,6 +819,9 @@ void btOptimizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* btVector3 bounds[2]; bounds[0] = unQuantize(rootNode->m_quantizedAabbMin); bounds[1] = unQuantize(rootNode->m_quantizedAabbMax); + /* Add box cast extents */ + bounds[0] += aabbMin; + bounds[1] += aabbMax; btVector3 normal; #if 0 bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); @@ -831,6 +838,7 @@ void btOptimizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* #endif } + RayBoxOverlap = true; if (isLeafNode && RayBoxOverlap) { nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); @@ -946,7 +954,7 @@ void btOptimizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallb bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; if (fast_path) { - walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, 0, m_curNodeIndex); + walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0), 0, m_curNodeIndex); } else { /* Otherwise fallback to AABB overlap test */ btVector3 aabbMin = raySource; @@ -958,13 +966,16 @@ void btOptimizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallb } -void btOptimizedBvh::reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btOptimizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const { - (void)nodeCallback; - (void)aabbMin; - (void)aabbMax; - //not yet, please use aabb - btAssert(0); + bool supported = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; + if (supported) + { + walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); + } else { + //not yet, please implement different paths + btAssert("Box cast on this type of Bvh Tree is not supported yet" && 0); + } } diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index 6ac30f0ee..d52502700 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -286,7 +286,7 @@ protected: void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; - void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, int startNodeIndex,int endNodeIndex) const; + void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const; ///tree traversal designed for small-memory processors like PS3 SPU @@ -331,7 +331,7 @@ public: void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; void reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const; - void reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const; SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point) const { diff --git a/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index b802ae39d..e0ca5b684 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -13,7 +13,14 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +#include +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" #include "btRaycastCallback.h" btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to) @@ -97,3 +104,55 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, } } } + + +btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld) +{ + m_convexShape = convexShape; + m_convexShapeFrom = convexShapeFrom; + m_convexShapeTo = convexShapeTo; + m_triangleToWorld = triangleToWorld; + m_hitFraction = 1.0; +} + +void +btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex) +{ + btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]); + + btVoronoiSimplexSolver simplexSolver; + + +//#define USE_SUBSIMPLEX_CONVEX_CAST 1 +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver); +#else + //btGjkConvexCast convexCaster(m_convexShape,&triangleShape,&simplexSolver); + btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,NULL); +#endif //#USE_SUBSIMPLEX_CONVEX_CAST + + btConvexCast::CastResult castResult; + castResult.m_fraction = btScalar(1.); + if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult)) + { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < m_hitFraction) + { + +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + //rotate normal into worldspace + castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal; +#endif //USE_SUBSIMPLEX_CONVEX_CAST + castResult.m_normal.normalize(); + + reportHit (castResult.m_normal, + btVector3(6,6,6), + castResult.m_fraction, + partId, + triangleIndex); + } + } + } +} \ No newline at end of file diff --git a/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index 87490a345..0d5eaf8eb 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -17,14 +17,15 @@ subject to the following restrictions: #define RAYCAST_TRI_CALLBACK_H #include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "LinearMath/btTransform.h" struct btBroadphaseProxy; - +class btConvexShape; class btTriangleRaycastCallback: public btTriangleCallback { public: - //input + //input btVector3 m_from; btVector3 m_to; @@ -38,5 +39,21 @@ public: }; +class btTriangleConvexcastCallback : public btTriangleCallback +{ +public: + const btConvexShape* m_convexShape; + btTransform m_convexShapeFrom; + btTransform m_convexShapeTo; + btTransform m_triangleToWorld; + btScalar m_hitFraction; + + btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld); + + virtual void processTriangle (btVector3* triangle, int partId, int triangleIndex); + + virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; +}; + #endif //RAYCAST_TRI_CALLBACK_H diff --git a/src/LinearMath/btAabbUtil2.h b/src/LinearMath/btAabbUtil2.h index 3491ef2d1..8bb6b3af7 100644 --- a/src/LinearMath/btAabbUtil2.h +++ b/src/LinearMath/btAabbUtil2.h @@ -20,7 +20,14 @@ subject to the following restrictions: #include "btVector3.h" #include "btMinMax.h" - +SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin, + btVector3& aabbMax, + const btVector3& expansionMin, + const btVector3& expansionMax) +{ + aabbMin = aabbMin + expansionMin; + aabbMax = aabbMax + expansionMax; +} /// conservative test for overlap between two aabbs