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

Simplify code: Remove computation of cost.

parent 5bcf569d
......@@ -129,64 +129,6 @@ struct Contact
}
};
/// @brief Cost source describes an area with a cost. The area is described by an AABB region.
struct CostSource
{
/// @brief aabb lower bound
Vec3f aabb_min;
/// @brief aabb upper bound
Vec3f aabb_max;
/// @brief cost density in the AABB region
FCL_REAL cost_density;
FCL_REAL total_cost;
CostSource(const Vec3f& aabb_min_, const Vec3f& aabb_max_, FCL_REAL cost_density_) : aabb_min(aabb_min_),
aabb_max(aabb_max_),
cost_density(cost_density_)
{
total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
}
CostSource(const AABB& aabb, FCL_REAL cost_density_) : aabb_min(aabb.min_),
aabb_max(aabb.max_),
cost_density(cost_density_)
{
total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
}
CostSource() {}
bool operator < (const CostSource& other) const
{
if(total_cost < other.total_cost)
return false;
if(total_cost > other.total_cost)
return true;
if(cost_density < other.cost_density)
return false;
if(cost_density > other.cost_density)
return true;
for(size_t i = 0; i < 3; ++i)
if(aabb_min[i] != other.aabb_min[i])
return aabb_min[i] < other.aabb_min[i];
return false;
}
bool operator == (const CostSource& other) const
{
return aabb_min == other.aabb_min
&& aabb_max == other.aabb_max
&& cost_density == other.cost_density
&& total_cost == other.total_cost;
}
};
struct CollisionResult;
/// @brief request to the collision algorithm
......@@ -201,15 +143,6 @@ struct CollisionRequest
/// Whether a lower bound on distance is returned when objects are disjoint
bool enable_distance_lower_bound;
/// @brief The maximum number of cost sources will return
size_t num_max_cost_sources;
/// @brief whether the cost sources will be computed
bool enable_cost;
/// @brief whether the cost computation is approximated
bool use_approximate_cost;
/// @brief narrow phase solver
GJKSolverType gjk_solver_type;
......@@ -235,9 +168,6 @@ struct CollisionRequest
num_max_contacts(num_max_contacts_),
enable_contact(enable_contact_),
enable_distance_lower_bound (enable_distance_lower_bound_),
num_max_cost_sources(num_max_cost_sources_),
enable_cost(enable_cost_),
use_approximate_cost(use_approximate_cost_),
gjk_solver_type(gjk_solver_type_),
security_margin (0),
break_distance (1e-3)
......@@ -256,9 +186,6 @@ private:
/// @brief contact information
std::vector<Contact> contacts;
/// @brief cost sources
std::set<CostSource> cost_sources;
public:
Vec3f cached_gjk_guess;
......@@ -278,19 +205,10 @@ public:
contacts.push_back(c);
}
/// @brief add one cost source into result structure
inline void addCostSource(const CostSource& c, std::size_t num_max_cost_sources)
{
cost_sources.insert(c);
while (cost_sources.size() > num_max_cost_sources)
cost_sources.erase(--cost_sources.end());
}
/// @brief whether two CollisionResult are the same or not
inline bool operator ==(const CollisionResult& other) const
{
return contacts == other.contacts
&& cost_sources == other.cost_sources
&& distance_lower_bound == other.distance_lower_bound;
}
......@@ -306,12 +224,6 @@ public:
return contacts.size();
}
/// @brief number of cost sources found
size_t numCostSources() const
{
return cost_sources.size();
}
/// @brief get the i-th contact calculated
const Contact& getContact(size_t i) const
{
......@@ -328,18 +240,10 @@ public:
std::copy(contacts.begin(), contacts.end(), contacts_.begin());
}
/// @brief get all the cost sources
void getCostSources(std::vector<CostSource>& cost_sources_)
{
cost_sources_.resize(cost_sources.size());
std::copy(cost_sources.begin(), cost_sources.end(), cost_sources_.begin());
}
/// @brief clear the results obtained
void clear()
{
contacts.clear();
cost_sources.clear();
}
/// @brief reposition Contact objects when fcl inverts them
......
......@@ -231,26 +231,6 @@ public:
this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
}
}
if(is_intersect && this->request.enable_cost)
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
{
if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
}
......@@ -333,26 +313,6 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting
result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
}
}
if(is_intersect && request.enable_cost)
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(model2, tf2, shape_aabb);
/* bool res = */ AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(shape_aabb, overlap_part);
result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
}
}
else if((!model1->isFree() || model2.isFree()) && request.enable_cost)
{
if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL))
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(model2, tf2, shape_aabb);
/* bool res = */ AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(shape_aabb, overlap_part);
result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
}
}
}
......@@ -534,25 +494,6 @@ public:
}
}
if(is_intersect && this->request.enable_cost)
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
{
if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
}
......
......@@ -224,23 +224,7 @@ public:
}
}
}
if(is_intersect && this->request.enable_cost)
{
AABB overlap_part;
AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
{
if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
{
AABB overlap_part;
AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
}
/// @brief Whether the traversal process can stop early
......
......@@ -305,7 +305,6 @@ private:
computeBV<AABB, Box>(box, box_tf, aabb1);
computeBV<AABB, S>(s, tf2, aabb2);
aabb1.overlap(aabb2, overlap_part);
cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * s.cost_density), crequest->num_max_cost_sources);
}
}
......@@ -347,41 +346,10 @@ private:
}
}
if(is_intersect && crequest->enable_cost)
{
AABB overlap_part;
AABB aabb1, aabb2;
computeBV<AABB, Box>(box, box_tf, aabb1);
computeBV<AABB, S>(s, tf2, aabb2);
aabb1.overlap(aabb2, overlap_part);
}
return crequest->isSatisfied(*cresult);
}
else return false;
}
else if(!tree1->isNodeFree(root1) && !s.isFree() && crequest->enable_cost) // uncertain area
{
OBB obb1;
convertBV(bv1, tf1, obb1);
if(obb1.overlap(obb2))
{
Box box;
Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);
if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
{
AABB overlap_part;
AABB aabb1, aabb2;
computeBV<AABB, Box>(box, box_tf, aabb1);
computeBV<AABB, S>(s, tf2, aabb2);
aabb1.overlap(aabb2, overlap_part);
}
}
return false;
}
else // free area
return false;
}
......@@ -390,7 +358,7 @@ private:
/// 2) at least of one the nodes is free; OR
/// 2) (two uncertain nodes or one node occupied and one node uncertain) AND cost not required
if(tree1->isNodeFree(root1) || s.isFree()) return false;
else if((tree1->isNodeUncertain(root1) || s.isUncertain()) && !crequest->enable_cost) return false;
else if((tree1->isNodeUncertain(root1) || s.isUncertain())) return false;
else
{
OBB obb1;
......@@ -409,14 +377,6 @@ private:
if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2))
return true;
}
else if(!s.isFree() && crequest->enable_cost)
{
AABB child_bv;
computeChildBV(bv1, i, child_bv);
if(OcTreeShapeIntersectRecurse(tree1, NULL, child_bv, s, obb2, tf1, tf2))
return true;
}
}
return false;
......@@ -540,7 +500,6 @@ private:
computeBV<AABB, Box>(box, box_tf, aabb1);
AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3));
aabb1.overlap(aabb2, overlap_part);
cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * tree2->cost_density), crequest->num_max_cost_sources);
}
}
......@@ -600,51 +559,11 @@ private:
}
}
if(is_intersect && crequest->enable_cost)
{
AABB overlap_part;
AABB aabb1;
computeBV<AABB, Box>(box, box_tf, aabb1);
AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3));
aabb1.overlap(aabb2, overlap_part);
cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources);
}
return crequest->isSatisfied(*cresult);
}
else
return false;
}
else if(!tree1->isNodeFree(root1) && !tree2->isFree() && crequest->enable_cost) // uncertain area
{
OBB obb1, obb2;
convertBV(bv1, tf1, obb1);
convertBV(tree2->getBV(root2).bv, tf2, obb2);
if(obb1.overlap(obb2))
{
Box box;
Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);
int primitive_id = tree2->getBV(root2).primitiveId();
const Triangle& tri_id = tree2->tri_indices[primitive_id];
const Vec3f& p1 = tree2->vertices[tri_id[0]];
const Vec3f& p2 = tree2->vertices[tri_id[1]];
const Vec3f& p3 = tree2->vertices[tri_id[2]];
if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL))
{
AABB overlap_part;
AABB aabb1;
computeBV<AABB, Box>(box, box_tf, aabb1);
AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3));
aabb1.overlap(aabb2, overlap_part);
cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources);
}
}
return false;
}
else // free area
return false;
}
......@@ -653,7 +572,8 @@ private:
/// 2) at least one of the nodes is free; OR
/// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required
if(tree1->isNodeFree(root1) || tree2->isFree()) return false;
else if((tree1->isNodeUncertain(root1) || tree2->isUncertain()) && !crequest->enable_cost) return false;
else if((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
return false;
else
{
OBB obb1, obb2;
......@@ -675,14 +595,6 @@ private:
if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
return true;
}
else if(!tree2->isFree() && crequest->enable_cost)
{
AABB child_bv;
computeChildBV(bv1, i, child_bv);
if(OcTreeMeshIntersectRecurse(tree1, NULL, child_bv, tree2, root2, tf1, tf2))
return true;
}
}
}
else
......@@ -801,7 +713,6 @@ private:
computeBV<AABB, Box>(box1, box1_tf, aabb1);
computeBV<AABB, Box>(box2, box2_tf, aabb2);
aabb1.overlap(aabb2, overlap_part);
cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * tree2->getOccupancyThres()), crequest->num_max_cost_sources);
}
return false;
......@@ -904,46 +815,8 @@ private:
}
}
if(is_intersect && crequest->enable_cost)
{
Box box1, box2;
Transform3f box1_tf, box2_tf;
constructBox(bv1, tf1, box1, box1_tf);
constructBox(bv2, tf2, box2, box2_tf);
AABB overlap_part;
AABB aabb1, aabb2;
computeBV<AABB, Box>(box1, box1_tf, aabb1);
computeBV<AABB, Box>(box2, box2_tf, aabb2);
aabb1.overlap(aabb2, overlap_part);
cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources);
}
return crequest->isSatisfied(*cresult);
}
else if(!tree1->isNodeFree(root1) && !tree2->isNodeFree(root2) && crequest->enable_cost) // uncertain area (here means both are uncertain or one uncertain and one occupied)
{
OBB obb1, obb2;
convertBV(bv1, tf1, obb1);
convertBV(bv2, tf2, obb2);
if(obb1.overlap(obb2))
{
Box box1, box2;
Transform3f box1_tf, box2_tf;
constructBox(bv1, tf1, box1, box1_tf);
constructBox(bv2, tf2, box2, box2_tf);
AABB overlap_part;
AABB aabb1, aabb2;
computeBV<AABB, Box>(box1, box1_tf, aabb1);
computeBV<AABB, Box>(box2, box2_tf, aabb2);
aabb1.overlap(aabb2, overlap_part);
cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources);
}
return false;
}
else // free area (at least one node is free)
return false;
}
......@@ -952,7 +825,8 @@ private:
/// 2) at least one of the nodes is free; OR
/// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required
if(tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) return false;
else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)) && !crequest->enable_cost) return false;
else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)))
return false;
else
{
OBB obb1, obb2;
......@@ -976,16 +850,6 @@ private:
tf1, tf2))
return true;
}
else if(!tree2->isNodeFree(root2) && crequest->enable_cost)
{
AABB child_bv;
computeChildBV(bv1, i, child_bv);
if(OcTreeIntersectRecurse(tree1, NULL, child_bv,
tree2, root2, bv2,
tf1, tf2))
return true;
}
}
}
else
......@@ -1003,16 +867,6 @@ private:
tf1, tf2))
return true;
}
else if(!tree1->isNodeFree(root1) && crequest->enable_cost)
{
AABB child_bv;
computeChildBV(bv2, i, child_bv);
if(OcTreeIntersectRecurse(tree1, root1, bv1,
tree2, NULL, child_bv,
tf1, tf2))
return true;
}
}
}
......
......@@ -101,28 +101,6 @@ public:
result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE));
}
}
if(is_collision && request.enable_cost)
{
AABB aabb1, aabb2;
computeBV<AABB, S1>(*model1, tf1, aabb1);
computeBV<AABB, S2>(*model2, tf2, aabb2);
AABB overlap_part;
aabb1.overlap(aabb2, overlap_part);
result->addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
}
}
else if((!model1->isFree() && !model2->isFree()) && request.enable_cost)
{
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
{
AABB aabb1, aabb2;
computeBV<AABB, S1>(*model1, tf1, aabb1);
computeBV<AABB, S2>(*model2, tf2, aabb2);
AABB overlap_part;
aabb1.overlap(aabb2, overlap_part);
result->addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
}
}
}
......
......@@ -42,7 +42,7 @@ namespace fcl
bool CollisionRequest::isSatisfied(const CollisionResult& result) const
{
return (!enable_cost) && result.isCollision() && (num_max_contacts <= result.numContacts());
return result.isCollision() && (num_max_contacts <= result.numContacts());
}
bool DistanceRequest::isSatisfied(const DistanceResult& result) const
......
......@@ -108,41 +108,13 @@ std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3f& tf1
{
if(request.isSatisfied(result)) return result.numContacts();
if(request.enable_cost && request.use_approximate_cost)
{
CollisionRequest no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree
no_cost_request.enable_cost = false; // disable cost computation
OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result);
collide(&node, request, result);
Box box;
Transform3f box_tf;
constructBox(obj2->getBV(0).bv, tf2, box, box_tf); // compute the box for BVH's root node
box.cost_density = obj2->cost_density;
box.threshold_occupied = obj2->threshold_occupied;
box.threshold_free = obj2->threshold_free;
CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); // additional cost request, no contacts
OcTreeShapeCollide<Box, NarrowPhaseSolver>(o1, tf1, &box, box_tf, nsolver, only_cost_request, result);
}
else
{
OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
collide(&node, request, result);
}
<