diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index 4d129deca..7f7fc6d81 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -397,7 +397,7 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y) case '.': { - shootBox(getCameraTargetPosition()); + shootBox(getRayTo(x,y));//getCameraTargetPosition()); break; } @@ -530,7 +530,7 @@ void DemoApplication::shootBox(const btVector3& destination) btConvexShape* childShape = new btBoxShape(btVector3(1.f,1.f,1.f)); m_shootBoxShape = new btUniformScalingShape(childShape,0.5f); #else - m_shootBoxShape = new btBoxShape(btVector3(1.f,1.f,1.f)); + m_shootBoxShape = new btSphereShape(1.f);//BoxShape(btVector3(1.f,1.f,1.f)); #endif// } diff --git a/Demos/SoftDemo/SoftDemo.cpp b/Demos/SoftDemo/SoftDemo.cpp index 896334a3e..5b814cdb1 100644 --- a/Demos/SoftDemo/SoftDemo.cpp +++ b/Demos/SoftDemo/SoftDemo.cpp @@ -1322,7 +1322,7 @@ void SoftDemo::clientResetScene() m_autocam = false; m_raycast = false; m_cutting = false; - m_results.time = SIMD_INFINITY; + m_results.fraction = 1.f; demofncs[current_demo](this); } @@ -1364,14 +1364,16 @@ void SoftDemo::renderme() const btScalar dist=10; btTransform trs; trs.setOrigin(ps); + btScalar rayLength = 1000.f; + const btScalar angle=m_animtime*0.2; trs.setRotation(btQuaternion(angle,SIMD_PI/4,0)); - const btVector3 dir=trs.getBasis()*btVector3(0,-1,0); + btVector3 dir=trs.getBasis()*btVector3(0,-1,0); trs.setOrigin(ps-dir*dist); btAlignedObjectArray origins; - btAlignedObjectArray times; + btAlignedObjectArray fractions; origins.resize(res*res); - times.resize(res*res,SIMD_INFINITY); + fractions.resize(res*res,1.f); for(int y=0;yrayCast(*org,dir,results,*mint)) + btVector3 rayFrom = *org; + btVector3 rayTo = rayFrom+dir*rayLength; + if(psbs[ib]->rayTest(rayFrom,rayTo,results)) { - *mint=results.time; + *fraction=results.fraction; } } - ++org;++mint; + ++org;++fraction; } long ms=btMax(m_clock.getTimeMilliseconds(),1); long rayperseconds=(1000*(origins.size()*sbs.size()))/ms; @@ -1413,15 +1417,15 @@ void SoftDemo::renderme() idraw->drawLine(c[2],c[0],btVector3(0,0,0)); for(int i=0,ni=origins.size();idrawLine(org,org+dir*tim,btVector3(1,0,0)); + idraw->drawLine(org,org+dir*rayLength*fraction,btVector3(1,0,0)); } else { - idraw->drawLine(org,org-dir*0.1,btVector3(0,0,0)); + idraw->drawLine(org,org-dir*rayLength*0.1,btVector3(0,0,0)); } } #undef RES @@ -1490,7 +1494,7 @@ void SoftDemo::keyboardCallback(unsigned char key, int x, int y) // void SoftDemo::mouseMotionFunc(int x,int y) { -if(m_node&&(m_results.timerayCast(rayFrom,rayDir,res,m_results.time)) + if(psb->rayTest(rayFrom,rayTo,res)) { m_results=res; } } - if(m_results.timegetTotalMass()); m_results.body->refine(&isphere,0.0001,true); printf("Mass after: %f\r\n",m_results.body->getTotalMass()); } - m_results.time=SIMD_INFINITY; + m_results.fraction=1.f; m_drag=false; DemoApplication::mouseFunc(button,state,x,y); break; diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index 66f97dc14..9bbd9b098 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -669,7 +669,7 @@ Benchmarking dbvt... [4] btDbvt::collideTT self: 2814 ms (-20%) [5] btDbvt::collideTT xform: 7379 ms (-1%) [6] btDbvt::collideTT xform,self: 7270 ms (-2%) -[7] btDbvt::collideRAY: 6314 ms (0%),(332143 r/s) +[7] btDbvt::rayTest: 6314 ms (0%),(332143 r/s) [8] insert/remove: 2093 ms (0%),(1001983 ir/s) [9] updates (teleport): 1879 ms (-3%),(1116100 u/s) [10] updates (jitter): 1244 ms (-4%),(1685813 u/s) @@ -809,7 +809,7 @@ bool cfgBenchmark6_Enable = cfgEnable; static const int cfgBenchmark6_Iterations = 512; static const btScalar cfgBenchmark6_OffsetScale = 2; static const int cfgBenchmark6_Reference = 7270; -//[7] btDbvt::collideRAY +//[7] btDbvt::rayTest bool cfgBenchmark7_Enable = cfgEnable; static const int cfgBenchmark7_Passes = 32; static const int cfgBenchmark7_Iterations = 65536; @@ -1017,13 +1017,13 @@ if(cfgBenchmark7_Enable) } btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); dbvt.optimizeTopDown(); - printf("[7] btDbvt::collideRAY: "); + printf("[7] btDbvt::rayTest: "); wallclock.reset(); for(int i=0;i=a.mi.x())&& (b.z()<=a.mx.z())); } -// -DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btVector3& org, - const btVector3& invdir, - const unsigned* signs) -{ -#if 0 -const btVector3 b0((a.mi-org)*invdir); -const btVector3 b1((a.mx-org)*invdir); -const btVector3 tmin(btMin(b0[0],b1[0]),btMin(b0[1],b1[1]),btMin(b0[2],b1[2])); -const btVector3 tmax(btMax(b0[0],b1[0]),btMax(b0[1],b1[1]),btMax(b0[2],b1[2])); -const btScalar tin=btMax(tmin[0],btMax(tmin[1],tmin[2])); -const btScalar tout=btMin(tmax[0],btMin(tmax[1],tmax[2])); -return(tinx()-org[0])*invdir[0]; -btScalar txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0]; -const btScalar tymin=(bounds[ signs[1]]->y()-org[1])*invdir[1]; -const btScalar tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1]; -if((txmin>tymax)||(tymin>txmax)) return(false); -if(tymin>txmin) txmin=tymin; -if(tymaxz()-org[2])*invdir[2]; -const btScalar tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2]; -if((txmin>tzmax)||(tzmin>txmax)) return(false); -if(tzmin>txmin) txmin=tzmin; -if(tzmax0); -#endif -} + + + + +////////////////////////////////////// + // DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, @@ -875,31 +848,51 @@ if(root) } } + // DBVT_PREFIX -inline void btDbvt::collideRAY( const btDbvtNode* root, - const btVector3& origin, - const btVector3& direction, +inline void btDbvt::rayTest( const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, DBVT_IPOLICY) { DBVT_CHECKTYPE -if(root) + if(root) { - const btVector3 normal=direction.normalized(); - const btVector3 invdir( 1/normal.x(), - 1/normal.y(), - 1/normal.z()); - const unsigned signs[]={ direction.x()<0, - direction.y()<0, - direction.z()<0}; + btVector3 rayDir = (rayTo-rayFrom); + rayDir.normalize (); + + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + btVector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; + + + btVector3 resultNormal; + + btAlignedObjectArray stack; stack.reserve(SIMPLE_STACKSIZE); stack.push_back(root); do { const btDbvtNode* node=stack[stack.size()-1]; stack.pop_back(); - if(Intersect(node->volume,origin,invdir,signs)) - { + + btVector3 bounds[2] = {node->volume.Mins(),node->volume.Maxs()}; + btScalar lambda_max = rayDir.dot(rayTo-rayFrom); + btScalar tmin=1.f,lambda_min=0.f; + bool result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + +#ifdef COMPARE_BTRAY_AABB2 + btScalar param=1.f; + bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); + btAssert(result1 == result2); +#endif //TEST_BTRAY_AABB2 + + if(result1) + { if(node->isinternal()) { stack.push_back(node->childs[0]); diff --git a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index 4743b564d..5be3006f3 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -229,12 +229,12 @@ void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, BroadphaseRayTester callback(rayCallback); - m_sets[0].collideRAY( m_sets[0].m_root, + m_sets[0].rayTest( m_sets[0].m_root, rayFrom, rayTo, callback); - m_sets[1].collideRAY( m_sets[1].m_root, + m_sets[1].rayTest( m_sets[1].m_root, rayFrom, rayTo, callback); diff --git a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp index 14ee0a546..9b3087486 100644 --- a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -468,14 +468,15 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall #ifdef RAYAABB2 btVector3 rayFrom = raySource; - btVector3 rayDirection = (rayTarget-raySource); - rayDirection.normalize (); - lambda_max = rayDirection.dot(rayTarget-raySource); + btVector3 rayDir = (rayTarget-raySource); + rayDir.normalize (); + lambda_max = rayDir.dot(rayTarget-raySource); ///what about division by zero? --> just set rayDirection[i] to 1.0 - rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[0]; - rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[1]; - rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[2]; - unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; + btVector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; #endif btVector3 bounds[2]; @@ -501,7 +502,7 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall ///careful with this check: need to check division by zero (above) and fix the unQuantize method ///thanks Joerg/hiker for the reproduction case! ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 - rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max) : false; + rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false; #else btVector3 normal; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 401768e0a..ec9f0e2a7 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -1222,17 +1222,19 @@ return(done); } // -bool btSoftBody::rayCast(const btVector3& org, - const btVector3& dir, - sRayCast& results, - btScalar maxtime) -{ -if(m_faces.size()&&m_fdbvt.empty()) initializeFaceTree(); -results.body = this; -results.time = maxtime; -results.feature = eFeature::None; -results.index = -1; -return(rayCast(org,dir,results.time,results.feature,results.index,false)!=0); +bool btSoftBody::rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results) + { + if(m_faces.size()&&m_fdbvt.empty()) + initializeFaceTree(); + + results.body = this; + results.fraction = 1.f; + results.feature = eFeature::None; + results.index = -1; + + return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0); } // @@ -1497,31 +1499,36 @@ void btSoftBody::integrateMotion() } // - btSoftBody::RayCaster::RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt) + btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt) { -o = org; -d = dir; -mint = mxt; -face = 0; -tests = 0; + m_rayFrom = rayFrom; + m_rayNormalizedDirection = (rayTo-rayFrom); + m_rayTo = rayTo; + m_mint = mxt; + m_face = 0; + m_tests = 0; } // -void btSoftBody::RayCaster::Process(const btDbvtNode* leaf) +void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf) { -btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data; -const btScalar t=rayTriangle( o,d, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); -if((t>0)&&(tdata; + const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + m_mint); + if((t>0)&&(tteps)&&(tceps) && (dot(n,cross(b-hit,c-hit))>ceps) && (dot(n,cross(c-hit,a-hit))>ceps)) @@ -1638,16 +1646,20 @@ void btSoftBody::indicesToPointers(const int* map) } // -int btSoftBody::rayCast(const btVector3& org,const btVector3& dir, +int btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const { int cnt=0; if(bcountonly||m_fdbvt.empty()) {/* Full search */ + btVector3 dir = rayTo-rayFrom; + dir.normalize(); + for(int i=0,ni=m_faces.size();im_x, f.m_n[1]->m_x, f.m_n[2]->m_x, @@ -1666,13 +1678,14 @@ int btSoftBody::rayCast(const btVector3& org,const btVector3& dir, } else {/* Use dbvt */ - RayCaster collider(org,dir,mint); - btDbvt::collideRAY(m_fdbvt.m_root,org,dir,collider); - if(collider.face) + RayFromToCaster collider(rayFrom,rayTo,mint); + + btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider); + if(collider.m_face) { - mint=collider.mint; + mint=collider.m_mint; feature=btSoftBody::eFeature::Face; - index=(int)(collider.face-&m_faces[0]); + index=(int)(collider.m_face-&m_faces[0]); cnt=1; } } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index b0dadaab1..fbb4ddca0 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -134,7 +134,7 @@ public: btSoftBody* body; /// soft body eFeature::_ feature; /// feature type int index; /// feature index - btScalar time; /// time of impact (rayorg+raydir*time) + btScalar fraction; /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction) }; /* ImplicitFn */ @@ -532,18 +532,21 @@ public: btScalar radmrg; // radial margin btScalar updmrg; // Update margin }; - /* RayCaster */ - struct RayCaster : btDbvt::ICollide + /// RayFromToCaster takes a ray from, ray to (instead of direction!) + struct RayFromToCaster : btDbvt::ICollide { - btVector3 o; - btVector3 d; - btScalar mint; - Face* face; - int tests; - RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt); + btVector3 m_rayFrom; + btVector3 m_rayTo; + btVector3 m_rayNormalizedDirection; + btScalar m_mint; + Face* m_face; + int m_tests; + RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt); void Process(const btDbvtNode* leaf); - static inline btScalar rayTriangle(const btVector3& org, - const btVector3& dir, + + static inline btScalar rayFromToTriangle(const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayNormalizedDirection, const btVector3& a, const btVector3& b, const btVector3& c, @@ -744,11 +747,11 @@ public: /* CutLink */ bool cutLink(int node0,int node1,btScalar position); bool cutLink(const Node* node0,const Node* node1,btScalar position); - /* Ray casting */ - bool rayCast(const btVector3& org, - const btVector3& dir, - sRayCast& results, - btScalar maxtime=SIMD_INFINITY); + + ///Ray casting using rayFrom and rayTo in worldspace, (not direction!) + bool rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results); /* Solver presets */ void setSolver(eSolverPresets::_ preset); /* predictMotion */ @@ -798,7 +801,8 @@ public: // void pointersToIndices(); void indicesToPointers(const int* map=0); - int rayCast(const btVector3& org,const btVector3& dir, + + int rayTest(const btVector3& rayFrom,const btVector3& rayTo, btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; void initializeFaceTree(); btVector3 evaluateCom() const;