TEST(KDTree, itSplitsXthenYthenZ) { KDTree tree; vector<RTShape*> shapes; RTSphere sphere0(Vector(5,5,0), 1); RTSphere sphere1(Vector(5,-5,0), 1); RTSphere sphere2(Vector(-5,5,0), 1); RTSphere sphere3(Vector(-5,-5,0), 1); shapes.push_back(&sphere0); shapes.push_back(&sphere1); shapes.push_back(&sphere2); shapes.push_back(&sphere3); BoundingBox box(Vector(-6,-6,-6), Vector(12,12,12)); tree.setBoundingBox(box); tree.setTerminationCondition(1); tree.build(shapes, 0); CHECK_EQUAL( 1, tree.getLeft()->getLeft()->size() ); CHECK_EQUAL( 1, tree.getLeft()->getRight()->size() ); CHECK_EQUAL( 1, tree.getRight()->getLeft()->size() ); CHECK_EQUAL( 1, tree.getRight()->getRight()->size() ); }
TEST(KDTree, shouldSearchInTree) { KDTree tree; vector<RTShape*> shapes; RTSphere sphere0(Vector(2.5, 2.5, 2.5), 2.4); RTSphere sphere1(Vector(2.5, 2.5, 7.5), 2.4); RTSphere sphere2(Vector(2.5, 7.5, 7.5), 2.4); RTSphere sphere3(Vector(2.5, 7.5, 2.5), 2.4); RTSphere sphere4(Vector(7.5, 2.5, 2.5), 2.4); RTSphere sphere5(Vector(7.5, 2.5, 7.5), 2.4); RTSphere sphere6(Vector(7.5, 7.5, 7.5), 2.4); RTSphere sphere7(Vector(7.5, 7.5, 2.5), 2.4); shapes.push_back(&sphere0); shapes.push_back(&sphere1); shapes.push_back(&sphere2); shapes.push_back(&sphere3); shapes.push_back(&sphere4); shapes.push_back(&sphere5); shapes.push_back(&sphere6); shapes.push_back(&sphere7); BoundingBox box(Vector(0,0,0), Vector(10,10,10)); tree.setBoundingBox(box); tree.setTerminationCondition(1); tree.build(shapes, 0); Ray ray(Vector(7.5, 7.5, -2 ), Vector(0,0,1)); IntersectionPtr intersection = tree.intersect(ray); CHECK( intersection != nullptr ); CHECK( intersection->getShape() == &sphere7 ); }
TEST(KDTree, shouldSupportIntersectionSearchForRegularNodes) { KDTree tree; vector<RTShape*> shapes; RTSphere sphere0(Vector(5,5,0), 1); RTSphere sphere1(Vector(5,-5,0), 1); RTSphere sphere2(Vector(-5,5,0), 1); RTSphere sphere3(Vector(-5,-5,0), 1); shapes.push_back(&sphere0); shapes.push_back(&sphere1); shapes.push_back(&sphere2); shapes.push_back(&sphere3); BoundingBox box(Vector(-6,-6,-6), Vector(12,12,12)); tree.setBoundingBox(box); tree.setTerminationCondition(1); tree.build(shapes, 0); Ray ray(Vector(-10, 5, 0 ), Vector(1,0,0)); IntersectionPtr intersection = tree.intersect(ray); CHECK( intersection != nullptr ); CHECK( intersection->getShape() == &sphere2 ); }
TEST(KDTree, shapesCanBeInTwoVoxelsIfOnEdge) { KDTree tree; vector<RTShape*> shapes; RTSphere sphere0(Vector(-5,0,0), 1); RTSphere sphere1(Vector(5,0,0), 1); RTSphere sphere2(Vector(0,0,0), 1); shapes.push_back(&sphere0); shapes.push_back(&sphere1); shapes.push_back(&sphere2); BoundingBox box(Vector(-6, -1, -1), Vector(12, 2, 2)); tree.setBoundingBox(box); tree.setTerminationCondition(2); tree.build(shapes, 0); CHECK_EQUAL( 2, tree.getLeft()->size() ); CHECK_EQUAL( 2, tree.getRight()->size() ); }
btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold ///col0->m_worldTransform, btScalar resultFraction = btScalar(1.); btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); if (squareMot0 < col0->getCcdSquareMotionThreshold() && squareMot1 < col1->getCcdSquareMotionThreshold()) return resultFraction; if (disableCcd) return btScalar(1.); //An adhoc way of testing the Continuous Collision Detection algorithms //One object is approximated as a sphere, to simplify things //Starting in penetration should report no time of impact //For proper CCD, better accuracy and handling of 'allowed' penetration should be added //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) /// Convex0 against sphere for Convex1 { btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape()); btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) { //store result.m_fraction in both bodies if (col0->getHitFraction()> result.m_fraction) col0->setHitFraction( result.m_fraction ); if (col1->getHitFraction() > result.m_fraction) col1->setHitFraction( result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; } } /// Sphere (for convex0) against Convex1 { btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape()); btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) { //store result.m_fraction in both bodies if (col0->getHitFraction() > result.m_fraction) col0->setHitFraction( result.m_fraction); if (col1->getHitFraction() > result.m_fraction) col1->setHitFraction( result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; } } return resultFraction; }
float ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo) { ///Rather then checking ALL pairs, only calculate TOI when motion exceeds treshold ///Linear motion for one of objects needs to exceed m_ccdSquareMotionTreshold ///col0->m_worldTransform, float resultFraction = 1.f; CollisionObject* col1 = static_cast<CollisionObject*>(m_box1.m_clientObject); CollisionObject* col0 = static_cast<CollisionObject*>(m_box0.m_clientObject); float squareMot0 = (col0->m_interpolationWorldTransform.getOrigin() - col0->m_worldTransform.getOrigin()).length2(); float squareMot1 = (col1->m_interpolationWorldTransform.getOrigin() - col1->m_worldTransform.getOrigin()).length2(); if (squareMot0 < col0->m_ccdSquareMotionTreshold && squareMot0 < col0->m_ccdSquareMotionTreshold) return resultFraction; if (disableCcd) return 1.f; CheckPenetrationDepthSolver(); //An adhoc way of testing the Continuous Collision Detection algorithms //One object is approximated as a sphere, to simplify things //Starting in penetration should report no time of impact //For proper CCD, better accuracy and handling of 'allowed' penetration should be added //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) bool needsCollision = m_dispatcher->NeedsCollision(m_box0,m_box1); if (!needsCollision) return 1.f; /// Convex0 against sphere for Convex1 { ConvexShape* convex0 = static_cast<ConvexShape*>(col0->m_collisionShape); SphereShape sphere1(col1->m_ccdSweptShereRadius); //todo: allow non-zero sphere sizes, for better approximation ConvexCast::CastResult result; VoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere GjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform, col1->m_worldTransform,col1->m_interpolationWorldTransform,result)) { //store result.m_fraction in both bodies if (col0->m_hitFraction > result.m_fraction) col0->m_hitFraction = result.m_fraction; if (col1->m_hitFraction > result.m_fraction) col1->m_hitFraction = result.m_fraction; if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; } } /// Sphere (for convex0) against Convex1 { ConvexShape* convex1 = static_cast<ConvexShape*>(col1->m_collisionShape); SphereShape sphere0(col0->m_ccdSweptShereRadius); //todo: allow non-zero sphere sizes, for better approximation ConvexCast::CastResult result; VoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere GjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform, col1->m_worldTransform,col1->m_interpolationWorldTransform,result)) { //store result.m_fraction in both bodies if (col0->m_hitFraction > result.m_fraction) col0->m_hitFraction = result.m_fraction; if (col1->m_hitFraction > result.m_fraction) col1->m_hitFraction = result.m_fraction; if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; } } return resultFraction; }