void btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex) { //BTREVIEW 0.6% btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]); triangleShape.setMargin(m_triangleCollisionMargin); btVoronoiSimplexSolver simplexSolver; btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; //#define USE_SUBSIMPLEX_CONVEX_CAST 1 //if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below #ifdef USE_SUBSIMPLEX_CONVEX_CAST btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver); #else //btGjkConvexCast convexCaster(m_convexShape,&triangleShape,&simplexSolver); btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,&gjkEpaPenetrationSolver); #endif //#USE_SUBSIMPLEX_CONVEX_CAST btConvexCast::CastResult castResult; castResult.m_fraction = btScalar(1.); castResult.m_allowedPenetration = m_allowedPenetration; 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) { /* btContinuousConvexCast's normal is already in world space */ /* #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, castResult.m_hitPoint, castResult.m_fraction, partId, triangleIndex); } } } }
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { (void)partId; (void)triangleIndex; //do a swept sphere for now btTransform ident; ident.setIdentity(); btConvexCast::CastResult castResult; castResult.m_fraction = m_hitFraction; btSphereShape pointShape(m_ccdSphereRadius); btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); btVoronoiSimplexSolver simplexSolver; btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); //local space? if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, ident,ident,castResult)) { if (m_hitFraction > castResult.m_fraction) m_hitFraction = castResult.m_fraction; } }
bool Raytracer::lowlevelRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint) { btScalar closestHitResults = 1.f; bool hasHit = false; btConvexCast::CastResult rayResult; btSphereShape pointShape(0.0f); btTransform rayFromTrans; btTransform rayToTrans; rayFromTrans.setIdentity(); rayFromTrans.setOrigin(rayFrom); rayToTrans.setIdentity(); rayToTrans.setOrigin(rayTo); for (int s=0;s<numObjects;s++) { //do some culling, ray versus aabb btVector3 aabbMin,aabbMax; shapePtr[s]->getAabb(transforms[s],aabbMin,aabbMax); btScalar hitLambda = 1.f; btVector3 hitNormal; btCollisionObject tmpObj; tmpObj.setWorldTransform(transforms[s]); if (btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal)) { //reset previous result //choose the continuous collision detection method btSubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver); //btGjkConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver); //btContinuousConvexCollision convexCaster(&pointShape,shapePtr[s],&simplexSolver,0); if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult)) { if (rayResult.m_fraction < closestHitResults) { closestHitResults = rayResult.m_fraction; worldNormal = transforms[s].getBasis() *rayResult.m_normal; worldNormal.normalize(); hasHit = true; } } } } return hasHit; }
void Raytracer::displayCallback() { updateCamera(); for (int i=0;i<numObjects;i++) { transforms[i].setIdentity(); SimdVector3 pos(-3.5f+i*2.5f,0.f,0.f); transforms[i].setOrigin( pos ); SimdQuaternion orn; if (i < 2) { orn.setEuler(yaw,pitch,roll); transforms[i].setRotation(orn); } } myMink.SetTransformA(SimdTransform(transforms[0].getRotation())); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glDisable(GL_LIGHTING); if (once) { glGenTextures(1, &glTextureId); glBindTexture(GL_TEXTURE_2D,glTextureId ); once = 0; glPixelStorei(GL_UNPACK_ALIGNMENT, 1); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); } glDisable(GL_TEXTURE_2D); glDisable(GL_BLEND); #define RAYTRACER #ifdef RAYTRACER SimdVector4 rgba(1.f,0.f,0.f,0.5f); float top = 1.f; float bottom = -1.f; float nearPlane = 1.f; float tanFov = (top-bottom)*0.5f / nearPlane; float fov = 2.0 * atanf (tanFov); SimdVector3 rayFrom = getCameraPosition(); SimdVector3 rayForward = getCameraTargetPosition()-getCameraPosition(); rayForward.normalize(); float farPlane = 600.f; rayForward*= farPlane; SimdVector3 rightOffset; SimdVector3 vertical(0.f,1.f,0.f); SimdVector3 hor; hor = rayForward.cross(vertical); hor.normalize(); vertical = hor.cross(rayForward); vertical.normalize(); float tanfov = tanf(0.5f*fov); hor *= 2.f * farPlane * tanfov; vertical *= 2.f * farPlane * tanfov; SimdVector3 rayToCenter = rayFrom + rayForward; SimdVector3 dHor = hor * 1.f/float(screenWidth); SimdVector3 dVert = vertical * 1.f/float(screenHeight); SimdTransform rayFromTrans; rayFromTrans.setIdentity(); rayFromTrans.setOrigin(rayFrom); SimdTransform rayFromLocal; SimdTransform rayToLocal; SphereShape pointShape(0.0f); ///clear texture for (int x=0;x<screenWidth;x++) { for (int y=0;y<screenHeight;y++) { SimdVector4 rgba(0.f,0.f,0.f,0.f); raytracePicture->SetPixel(x,y,rgba); } } ConvexCast::CastResult rayResult; SimdTransform rayToTrans; rayToTrans.setIdentity(); SimdVector3 rayTo; for (int x=0;x<screenWidth;x++) { for (int y=0;y<screenHeight;y++) { rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; rayTo += x * dHor; rayTo -= y * dVert; rayToTrans.setOrigin(rayTo); for (int s=0;s<numObjects;s++) { // rayFromLocal = transforms[s].inverse()* rayFromTrans; // rayToLocal = transforms[s].inverse()* rayToTrans; //choose the continuous collision detection method SubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver); //GjkConvexCast convexCaster(&pointShape,shapePtr[0],&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,shapePtr[0],&simplexSolver,0); // BU_Simplex1to4 ptShape(SimdVector3(0,0,0));//algebraic needs features, doesnt use 'supporting vertex' // BU_CollisionPair convexCaster(&ptShape,shapePtr[0]); //reset previous result rayResult.m_fraction = 1.f; if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult)) { //float fog = 1.f - 0.1f * rayResult.m_fraction; rayResult.m_normal.normalize(); SimdVector3 worldNormal; worldNormal = transforms[s].getBasis() *rayResult.m_normal; float light = worldNormal.dot(SimdVector3(0.4f,-1.f,-0.4f)); if (light < 0.2f) light = 0.2f; if (light > 1.f) light = 1.f; rgba = SimdVector4(light,light,light,1.f); raytracePicture->SetPixel(x,y,rgba); } else { //clear is already done //rgba = SimdVector4(0.f,0.f,0.f,0.f); //raytracePicture->SetPixel(x,y,rgba); } } } } #define TEST_PRINTF #ifdef TEST_PRINTF extern BMF_FontData BMF_font_helv10; raytracePicture->Printf("CCD RAYTRACER",&BMF_font_helv10); char buffer[256]; sprintf(buffer,"%d RAYS / Frame",screenWidth*screenHeight*numObjects); raytracePicture->Printf(buffer,&BMF_font_helv10,0,10); #endif //TEST_PRINTF glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glFrustum(-1.0,1.0,-1.0,1.0,3,2020.0); glMatrixMode(GL_MODELVIEW); glPushMatrix(); glLoadIdentity(); // Reset The Modelview Matrix glTranslatef(0.0f,0.0f,-3.0f); // Move Into The Screen 5 Units glEnable(GL_TEXTURE_2D); glBindTexture(GL_TEXTURE_2D,glTextureId ); const unsigned char *ptr = raytracePicture->GetBuffer(); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, raytracePicture->GetWidth(),raytracePicture->GetHeight(), 0, GL_RGBA, GL_UNSIGNED_BYTE, ptr); glEnable (GL_BLEND); glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glColor4f (1,1,1,1); // alpha=0.5=half visible glBegin(GL_QUADS); glTexCoord2f(0.0f, 0.0f); glVertex2f(-1,1); glTexCoord2f(1.0f, 0.0f); glVertex2f(1,1); glTexCoord2f(1.0f, 1.0f); glVertex2f(1,-1); glTexCoord2f(0.0f, 1.0f); glVertex2f(-1,-1); glEnd(); glMatrixMode(GL_MODELVIEW); glPopMatrix(); glMatrixMode(GL_PROJECTION); glPopMatrix(); glMatrixMode(GL_MODELVIEW); #endif //RAYRACER glDisable(GL_TEXTURE_2D); glDisable(GL_DEPTH_TEST); GL_ShapeDrawer::DrawCoordSystem(); glPushMatrix(); /* /// normal opengl rendering float m[16]; int i; for (i=0;i<numObjects;i++) { transA.getOpenGLMatrix( m ); /// draw the simplex GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1)); /// calculate closest point from simplex to the origin, and draw this vector simplex.CalcClosest(m); } */ glPopMatrix(); pitch += 0.005f; yaw += 0.01f; glFlush(); glutSwapBuffers(); }
void LinearConvexCastDemo::displayCallback(void) { updateCamera(); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glDisable(GL_LIGHTING); GL_ShapeDrawer::drawCoordSystem(); static btScalar angle = 0.f; angle+=getDeltaTimeMicroseconds()/1000000.0; tr[1].setRotation(btQuaternion(btVector3(1,0,0),angle)); btTransform toA, toB; toA = tr[0]; toA.setOrigin( btVector3( 0.0f, 0.f, 0.0f ) ); toB = tr[1]; toB.setOrigin( btVector3( 0.0f, 0.0f, 0.0f ) ); gGjkSimplexSolver.reset(); btVector3 worldBoundsMin(-1000,-1000,-1000); btVector3 worldBoundsMax(1000,1000,1000); //btGjkConvexCast convexCaster(shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver ); btSubsimplexConvexCast convexCaster( shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver ); btConvexCast::CastResult result; result.m_hitPoint.setValue(0,0,0); convexCaster.calcTimeOfImpact( tr[ 0 ], toA, tr[ 1 ], toB, result ); btScalar m1[16], m2[16],m3[16]; tr[ 0 ].getOpenGLMatrix( m1 ); tr[ 1 ].getOpenGLMatrix( m2 ); btSphereShape sphere(0.2); btTransform tmp = tr[0]; tmp.setOrigin(result.m_hitPoint); tmp.getOpenGLMatrix(m3); m_shapeDrawer.drawOpenGL( m3, &sphere, btVector3( 1, 0, 1 ), getDebugMode() ,worldBoundsMin,worldBoundsMax); m_shapeDrawer.drawOpenGL( m1, shapePtr[ 0 ], btVector3( 1, 0, 0 ), getDebugMode() ,worldBoundsMin,worldBoundsMax); m_shapeDrawer.drawOpenGL( m2, shapePtr[ 1 ], btVector3( 1, 0, 0 ), getDebugMode() ,worldBoundsMin,worldBoundsMax); btVector3 originA, originB; originA.setInterpolate3( tr[ 0 ].getOrigin(), toA.getOrigin(), result.m_fraction ); originB.setInterpolate3( tr[ 1 ].getOrigin(), toB.getOrigin(), result.m_fraction ); btTransform A = tr[ 0 ]; A.setOrigin( originA ); btTransform B = tr[ 1 ]; B.setOrigin( originB ); A.getOpenGLMatrix( m1 ); B.getOpenGLMatrix( m2 ); m_shapeDrawer.drawOpenGL( m1, shapePtr[ 0 ], btVector3( 1, 1, 0 ), getDebugMode() ,worldBoundsMin,worldBoundsMax); m_shapeDrawer.drawOpenGL( m2, shapePtr[ 1 ], btVector3( 1, 1, 0 ), getDebugMode() ,worldBoundsMin,worldBoundsMax); glFlush(); glutSwapBuffers(); }
void CollisionWorld::RayTestSingle(const SimdTransform& rayFromTrans,const SimdTransform& rayToTrans, CollisionObject* collisionObject, const CollisionShape* collisionShape, const SimdTransform& colObjWorldTransform, RayResultCallback& resultCallback) { SphereShape pointShape(0.0f); if (collisionShape->IsConvex()) { ConvexCast::CastResult castResult; castResult.m_fraction = 1.f;//?? ConvexShape* convexShape = (ConvexShape*) collisionShape; VoronoiSimplexSolver simplexSolver; SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) { //add hit if (castResult.m_normal.length2() > 0.0001f) { castResult.m_normal.normalize(); if (castResult.m_fraction < resultCallback.m_closestHitFraction) { CollisionWorld::LocalRayResult localRayResult ( collisionObject, 0, castResult.m_normal, castResult.m_fraction ); resultCallback.AddSingleResult(localRayResult); } } } } else { if (collisionShape->IsConcave()) { TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionShape; SimdTransform worldTocollisionObject = colObjWorldTransform.inverse(); SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); //ConvexCast::CastResult struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback { CollisionWorld::RayResultCallback* m_resultCallback; CollisionObject* m_collisionObject; TriangleMeshShape* m_triangleMesh; BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to, CollisionWorld::RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape* triangleMesh): TriangleRaycastCallback(from,to), m_resultCallback(resultCallback), m_collisionObject(collisionObject), m_triangleMesh(triangleMesh) { } virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) { CollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; CollisionWorld::LocalRayResult rayResult (m_collisionObject, &shapeInfo, hitNormalLocal, hitFraction); return m_resultCallback->AddSingleResult(rayResult); } }; BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); rcb.m_hitFraction = resultCallback.m_closestHitFraction; SimdVector3 rayAabbMinLocal = rayFromLocal; rayAabbMinLocal.setMin(rayToLocal); SimdVector3 rayAabbMaxLocal = rayFromLocal; rayAabbMaxLocal.setMax(rayToLocal); triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); } else { //todo: use AABB tree or other BVH acceleration structure! if (collisionShape->IsCompound()) { const CompoundShape* compoundShape = static_cast<const CompoundShape*>(collisionShape); int i=0; for (i=0;i<compoundShape->GetNumChildShapes();i++) { SimdTransform childTrans = compoundShape->GetChildTransform(i); const CollisionShape* childCollisionShape = compoundShape->GetChildShape(i); SimdTransform childWorldTrans = colObjWorldTransform * childTrans; RayTestSingle(rayFromTrans,rayToTrans, collisionObject, childCollisionShape, childWorldTrans, resultCallback); } } } } }
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) { 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(rayFromTrans,rayToTrans,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 = rayFromTrans.getBasis() * castResult.m_normal; #endif //USE_SUBSIMPLEX_CONVEX_CAST 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()) { btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)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; btVector3 rayAabbMinLocal = rayFromLocal; rayAabbMinLocal.setMin(rayToLocal); btVector3 rayAabbMaxLocal = rayFromLocal; rayAabbMaxLocal.setMax(rayToLocal); triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); } else { //todo: use AABB tree or other BVH acceleration structure! if (collisionShape->isCompound()) { const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); int i=0; for (i=0;i<compoundShape->getNumChildShapes();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); } } } } }
void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, RayResultCallback& resultCallback) { btSphereShape pointShape(btScalar(0.0)); pointShape.setMargin(0.f); const btConvexShape* castShape = &pointShape; if (collisionShape->isConvex()) { // BT_PROFILE("rayTestConvex"); btConvexCast::CastResult castResult; castResult.m_fraction = resultCallback.m_closestHitFraction; 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(rayFromTrans,rayToTrans,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 = rayFromTrans.getBasis() * castResult.m_normal; #endif //USE_SUBSIMPLEX_CONVEX_CAST 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()) { // BT_PROFILE("rayTestConcave"); if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { ///optimized version for btBvhTriangleMeshShape 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 { //generic (slower) case btConcaveShape* concaveShape = (btConcaveShape*)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; btConcaveShape* m_triangleMesh; BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* 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,concaveShape); rcb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 rayAabbMinLocal = rayFromLocal; rayAabbMinLocal.setMin(rayToLocal); btVector3 rayAabbMaxLocal = rayFromLocal; rayAabbMaxLocal.setMax(rayToLocal); concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); } } else { // BT_PROFILE("rayTestCompound"); ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt if (collisionShape->isCompound()) { const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); int i=0; for (i=0;i<compoundShape->getNumChildShapes();i++) { btTransform childTrans = compoundShape->getChildTransform(i); const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); btTransform childWorldTrans = colObjWorldTransform * childTrans; // replace collision shape so that callback can determine the triangle btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape(); collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); rayTestSingle(rayFromTrans,rayToTrans, collisionObject, childCollisionShape, childWorldTrans, resultCallback); // restore collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); } } } } }
void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, RayResultCallback& resultCallback) { btSphereShape pointShape(btScalar(0.0)); pointShape.setMargin(0.f); const btConvexShape* castShape = &pointShape; if (collisionShape->isConvex()) { // BT_PROFILE("rayTestConvex"); btConvexCast::CastResult castResult; castResult.m_fraction = resultCallback.m_closestHitFraction; 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(rayFromTrans,rayToTrans,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 = rayFromTrans.getBasis() * castResult.m_normal; #endif //USE_SUBSIMPLEX_CONVEX_CAST 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()) { // BT_PROFILE("rayTestConcave"); if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { ///optimized version for btBvhTriangleMeshShape 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; btTransform m_colObjWorldTransform; BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh,const btTransform& colObjWorldTransform): //@BP Mod btTriangleRaycastCallback(from,to, resultCallback->m_flags), m_resultCallback(resultCallback), m_collisionObject(collisionObject), m_triangleMesh(triangleMesh), m_colObjWorldTransform(colObjWorldTransform) { } virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) { btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; btCollisionWorld::LocalRayResult rayResult (m_collisionObject, &shapeInfo, hitNormalWorld, hitFraction); bool normalInWorldSpace = true; return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); } }; BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); } else { //generic (slower) case btConcaveShape* concaveShape = (btConcaveShape*)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; btConcaveShape* m_triangleMesh; btTransform m_colObjWorldTransform; BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform): //@BP Mod btTriangleRaycastCallback(from,to, resultCallback->m_flags), m_resultCallback(resultCallback), m_collisionObject(collisionObject), m_triangleMesh(triangleMesh), m_colObjWorldTransform(colObjWorldTransform) { } virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) { btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; btCollisionWorld::LocalRayResult rayResult (m_collisionObject, &shapeInfo, hitNormalWorld, hitFraction); bool normalInWorldSpace = true; return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); } }; BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 rayAabbMinLocal = rayFromLocal; rayAabbMinLocal.setMin(rayToLocal); btVector3 rayAabbMaxLocal = rayFromLocal; rayAabbMaxLocal.setMax(rayToLocal); concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); } } else { // BT_PROFILE("rayTestCompound"); if (collisionShape->isCompound()) { struct LocalInfoAdder2 : public RayResultCallback { RayResultCallback* m_userCallback; int m_i; LocalInfoAdder2 (int i, RayResultCallback *user) : m_userCallback(user), m_i(i) { m_closestHitFraction = m_userCallback->m_closestHitFraction; } virtual bool needsCollision(btBroadphaseProxy* p) const { return m_userCallback->needsCollision(p); } virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b) { btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = -1; shapeInfo.m_triangleIndex = m_i; if (r.m_localShapeInfo == NULL) r.m_localShapeInfo = &shapeInfo; const btScalar result = m_userCallback->addSingleResult(r, b); m_closestHitFraction = m_userCallback->m_closestHitFraction; return result; } }; struct RayTester : btDbvt::ICollide { btCollisionObject* m_collisionObject; const btCompoundShape* m_compoundShape; const btTransform& m_colObjWorldTransform; const btTransform& m_rayFromTrans; const btTransform& m_rayToTrans; RayResultCallback& m_resultCallback; RayTester(btCollisionObject* collisionObject, const btCompoundShape* compoundShape, const btTransform& colObjWorldTransform, const btTransform& rayFromTrans, const btTransform& rayToTrans, RayResultCallback& resultCallback): m_collisionObject(collisionObject), m_compoundShape(compoundShape), m_colObjWorldTransform(colObjWorldTransform), m_rayFromTrans(rayFromTrans), m_rayToTrans(rayToTrans), m_resultCallback(resultCallback) { } void Process(int i) { const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i); const btTransform& childTrans = m_compoundShape->getChildTransform(i); btTransform childWorldTrans = m_colObjWorldTransform * childTrans; // replace collision shape so that callback can determine the triangle btCollisionShape* saveCollisionShape = m_collisionObject->getCollisionShape(); m_collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); LocalInfoAdder2 my_cb(i, &m_resultCallback); rayTestSingle( m_rayFromTrans, m_rayToTrans, m_collisionObject, childCollisionShape, childWorldTrans, my_cb); // restore m_collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); } void Process(const btDbvtNode* leaf) { Process(leaf->dataAsInt); } }; const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); const btDbvt* dbvt = compoundShape->getDynamicAabbTree(); RayTester rayCB( collisionObject, compoundShape, colObjWorldTransform, rayFromTrans, rayToTrans, resultCallback); #ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION if (dbvt) { btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin(); btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin(); btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB); } else #endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION { for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i) { rayCB.Process(i); } } } } } }
void btContinuousConvexCollisionDemo::displayCallback(void) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glDisable(GL_LIGHTING); //GL_ShapeDrawer::drawCoordSystem(); btScalar m[16]; int i; btVector3 worldBoundsMin(-1000,-1000,-1000); btVector3 worldBoundsMax(1000,1000,1000); /*for (i=0;i<numObjects;i++) { fromTrans[i].getOpenGLMatrix( m ); m_shapeDrawer.drawOpenGL(m,shapePtr[i]); } */ if (getDebugMode()==btIDebugDraw::DBG_DrawAabb) { i=0;//for (i=1;i<numObjects;i++) { //for each object, subdivide the from/to transform in 10 equal steps int numSubSteps = 10; for (int s=0;s<10;s++) { btScalar subStep = s * 1.f/(float)numSubSteps; btTransform interpolatedTrans; btTransformUtil::integrateTransform(fromTrans[i],linVels[i],angVels[i],subStep,interpolatedTrans); //fromTrans[i].getOpenGLMatrix(m); //m_shapeDrawer.drawOpenGL(m,shapePtr[i]); //toTrans[i].getOpenGLMatrix(m); //m_shapeDrawer.drawOpenGL(m,shapePtr[i]); interpolatedTrans.getOpenGLMatrix( m ); m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,0,1),getDebugMode(),worldBoundsMin,worldBoundsMax); } } } btMatrix3x3 mat; mat.setEulerZYX(yaw,pitch,roll); btQuaternion orn; mat.getRotation(orn); orn.setEuler(yaw,pitch,roll); fromTrans[1].setRotation(orn); toTrans[1].setRotation(orn); if (m_stepping || m_singleStep) { m_singleStep = false; pitch += 0.005f; // yaw += 0.01f; } // btVector3 fromA(-25,11,0); // btVector3 toA(-15,11,0); // btQuaternion ornFromA(0.f,0.f,0.f,1.f); // btQuaternion ornToA(0.f,0.f,0.f,1.f); // btTransform rayFromWorld(ornFromA,fromA); // btTransform rayToWorld(ornToA,toA); btTransform rayFromWorld = fromTrans[0]; btTransform rayToWorld = toTrans[0]; if (drawLine) { glBegin(GL_LINES); glColor3f(0, 0, 1); glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z()); glVertex3d(rayToWorld.getOrigin().x(),rayToWorld.getOrigin().y(),rayToWorld.getOrigin().z()); glEnd(); } //now perform a raycast on the shapes, in local (shape) space gGjkSimplexSolver.reset(); //choose one of the following lines for (i=0;i<numObjects;i++) { fromTrans[i].getOpenGLMatrix(m); m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax); } btDebugCastResult rayResult1(fromTrans[0],shapePtr[0],linVels[0],angVels[0],&m_shapeDrawer); for (i=1;i<numObjects;i++) { btConvexCast::CastResult rayResult2; btConvexCast::CastResult* rayResultPtr; if (btIDebugDraw::DBG_DrawAabb) { rayResultPtr = &rayResult1; } else { rayResultPtr = &rayResult2; } //GjkConvexCast convexCaster(&gGjkSimplexSolver); //SubsimplexConvexCast convexCaster(&gGjkSimplexSolver); //optional btConvexPenetrationDepthSolver* penetrationDepthSolver = 0; btContinuousConvexCollision convexCaster(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,penetrationDepthSolver ); gGjkSimplexSolver.reset(); if (convexCaster.calcTimeOfImpact(fromTrans[0],toTrans[0],fromTrans[i] ,toTrans[i] ,*rayResultPtr)) { glDisable(GL_DEPTH_TEST); btTransform hitTrans; btTransformUtil::integrateTransform(fromTrans[0],linVels[0],angVels[0],rayResultPtr->m_fraction,hitTrans); hitTrans.getOpenGLMatrix(m); m_shapeDrawer.drawOpenGL(m,shapePtr[0],btVector3(0,1,0),getDebugMode(),worldBoundsMin,worldBoundsMax); btTransformUtil::integrateTransform(fromTrans[i],linVels[i],angVels[i],rayResultPtr->m_fraction,hitTrans); hitTrans.getOpenGLMatrix(m); m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(0,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax); } } glFlush(); glutSwapBuffers(); }