bool CollisionHandlerDefault::IsSoftColliding(const Robot& robot, const Tree& tree)
{
ReachableObstaclesContainerCollide rc(world_, tree, robot);
world_.Accept(rc);
T_Point points = TreeToSegments(robot, tree);
if(points.size() >= 2)
{
if(points.size() >= 3)
{
matrices::Vector3 pt2 = points[points.size()-1];
points.pop_back();
matrices::Vector3 pt1 = points[points.size()-1];
points.push_back(pt1 + (pt2 - pt1) * 0.9);
}
for(ReachableObstaclesContainerCollide::T_ObstaclesCIT it = rc.obstacles_.begin(); it!= rc.obstacles_.end(); ++it)
{
T_Point::const_iterator ptIt = points.begin();;
T_Point::const_iterator ptIt2 = points.begin(); ++ptIt2;
for(;ptIt2 != points.end(); ++ptIt, ++ptIt2)
{
if(intersection_.Intersect(*ptIt, *ptIt2, **it))
{
return true;
}
}
}
}
return false;
}
bool CollisionHandlerDefault::IsColliding(const Robot& robot, const Tree& tree)
{
ReachableObstaclesContainerCollide rc(world_, tree, robot);
world_.Accept(rc);
T_Point points = TreeToSegments(robot, tree);
if(points.size() >= 2)
{
for(ReachableObstaclesContainerCollide::T_ObstaclesCIT it = rc.obstacles_.begin(); it!= rc.obstacles_.end(); ++it)
{
T_Point::const_iterator ptIt = points.begin();;
T_Point::const_iterator ptIt2 = points.begin(); ++ptIt2;
for(;ptIt2 != points.end(); ++ptIt, ++ptIt2)
{
if(intersection_.Intersect(*ptIt, *ptIt2, **it))
{
return true;
}
}
}
}
return false;
}
Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints, const real ub =1.)
{
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), constraints, ub);
}