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;
}
