Commit aea29e11 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add CollisionRequest::updateDistanceLowerBound

parent f2d4f45c
......@@ -223,9 +223,16 @@ public:
public:
CollisionResult()
: distance_lower_bound (std::numeric_limits<FCL_REAL>::max())
{
}
/// @brief Update the lower bound only if the distance in inferior.
inline void updateDistanceLowerBound (const FCL_REAL& distance_lower_bound_)
{
if (distance_lower_bound_ < distance_lower_bound)
distance_lower_bound = distance_lower_bound_;
}
/// @brief add one contact into result structure
inline void addContact(const Contact& c)
......
......@@ -89,7 +89,6 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
solver.cached_guess = request.cached_gjk_guess;
const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
result.distance_lower_bound = -1;
std::size_t res;
if(request.num_max_contacts == 0)
{
......
......@@ -109,7 +109,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
}
return 1;
}
result.distance_lower_bound = distance;
result.updateDistanceLowerBound (distance);
return 0;
}
......
......@@ -60,7 +60,7 @@ void collide(CollisionTraversalNodeBase* node,
collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
else
collisionNonRecurse(node, front_list, sqrDistLowerBound);
result.distance_lower_bound = sqrt (sqrDistLowerBound);
result.updateDistanceLowerBound (sqrt (sqrDistLowerBound));
}
}
......
......@@ -132,7 +132,7 @@ namespace fcl {
result.addContact (contact);
return 1;
}
result.distance_lower_bound = -penetrationDepth;
result.updateDistanceLowerBound (-penetrationDepth);
return 0;
}
} // namespace fcl
......
......@@ -399,7 +399,7 @@ void propagateBVHFrontListCollisionRecurse
}
}
}
result.distance_lower_bound = sqrt (sqrDistLowerBound);
result.updateDistanceLowerBound (sqrt (sqrDistLowerBound));
}
......
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