Commit 6f3ca783 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Add some asserts

  to check that if no collision lowed bound of distance is positive.
parent 17874bf7
......@@ -154,10 +154,11 @@ namespace fcl
distance = (w0 - w1).norm();
p1 = w0;
p2 = shape.toshape0.transform(w1);
assert (distance > 0);
}
break;
default:
;
assert (false && "should not reach type part.");
}
return col;
}
......
......@@ -276,6 +276,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting
if(request.num_max_contacts > result.numContacts())
result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE,
p1, -normal, -distance));
assert (result.isCollision ());
return;
}
sqrDistLowerBound = distance * distance;
......@@ -391,9 +392,11 @@ public:
bool BVTesting(int b1, int /*b2*/, FCL_REAL& sqrDistLowerBound) const
{
if(this->enable_statistics) this->num_bv_tests++;
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
this->model2_bv, this->model1->getBV(b1).bv,
this->request, sqrDistLowerBound);
bool res (!overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
this->model2_bv, this->model1->getBV(b1).bv,
this->request, sqrDistLowerBound));
assert (!res || sqrDistLowerBound > 0);
return res;
}
void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
......@@ -402,6 +405,7 @@ public:
(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics,
this->num_leaf_tests, this->request, *(this->result), sqrDistLowerBound);
assert (this->result->isCollision () || sqrDistLowerBound > 0);
}
};
......
......@@ -666,5 +666,7 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
}
return false;
}
assert (false && "should not reach this point");
return false;
}
} // fcl
......@@ -164,8 +164,10 @@ bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
if(enable_statistics) num_bv_tests++;
return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv,
request, sqrDistLowerBound);
bool res (!overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv,
request, sqrDistLowerBound));
assert (!res || sqrDistLowerBound > 0);
return res;
}
namespace details
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment