bool CollisionBox::overlaps(CollisionBox b1, CollisionBox b2, const Vector& axis)
{
precision halfProjection1 = projectToAxis(b1, axis);
precision halfProjection2 = projectToAxis(b2, axis);
// Take the centres and project the third dimension onto axis
precision p = std::abs((b2.centre - b1.centre).scalarProduct(axis));
return (p < halfProjection1 + halfProjection2);
}
float
Cylinder::getDistOnAxis(const PointI &point)
{
PointI point_proj = projectToAxis(point);
PointI start;
start.x = values[0];
start.y = values[1];
start.z = values[2];
PointI end;
end.x = values[0] + values[3];
end.y = values[1] + values[4];
end.z = values[2] + values[5];
float distStartToEnd = pcl::euclideanDistance<PointI, PointI> (start, end);
float distToStart = pcl::euclideanDistance<PointI, PointI> (start, point_proj);
float distToEnd = pcl::euclideanDistance<PointI, PointI> (end, point_proj);
float distOnAxis = 0;
if (distStartToEnd > distToStart && distStartToEnd > distToEnd)
{
distOnAxis = 0;
}
else
{
distOnAxis = std::min<float> (distToEnd, distToStart);
}
return distOnAxis;
}
