/// Calculates the signed distance for a primitive
double CollisionGeometry::calc_signed_dist(const Point3d& p)
{
// get the primitive from this
PrimitivePtr primitive = get_geometry();
// get the collision geometry
CollisionGeometryPtr cg = dynamic_pointer_cast<CollisionGeometry>(shared_from_this());
// setup a new point with a new pose
Point3d px = Pose3d::transform_point(primitive->get_pose(cg), p);
// call the primitive function
return primitive->calc_signed_dist(px);
}
/// Calculates the signed distances between two geometries and returns closest points if geometries are not interpenetrating
double CollisionGeometry::calc_signed_dist(CollisionGeometryPtr gA, CollisionGeometryPtr gB, Point3d& pA, Point3d& pB)
{
// get the two primitives
PrimitivePtr primA = gA->get_geometry();
PrimitivePtr primB = gB->get_geometry();
// setup poses for the points
pA.pose = primA->get_pose(gA);
pB.pose = primB->get_pose(gB);
FILE_LOG(LOG_COLDET) << "CollisionGeometry::calc_signed_dist() - computing signed distance between " << gA->get_single_body()->id << " and " << gB->get_single_body()->id << std::endl;
// now compute the signed distance
return primA->calc_signed_dist(primB, pA, pB);
}