/// 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);
}
/**
* The primitive is not cloned, nor is it unaltered; <b>this</b> points to
* the pointer returned by this method (typically <b>primitive</b>).
* \return primitive <i>unless the geometry of the underlying primitive is
* inconsistent, degenerate, or non-convex<i>; in that case,
* a corrected primitive will be returned.
*/
PrimitivePtr CollisionGeometry::set_geometry(PrimitivePtr primitive)
{
Quatd EYE;
if (_single_body.expired())
throw std::runtime_error("CollisionGeometry::set_geometry() called before single body set!");
SingleBodyPtr sb(_single_body);
RigidBodyPtr rb = dynamic_pointer_cast<RigidBody>(sb);
if (rb && !Quatd::rel_equal(rb->get_pose()->q, EYE))
{
std::cerr << "CollisionGeometry::set_primitive() warning - rigid body's orientation is not identity." << std::endl;
std::cerr << " At the rigid body's current orientation (" << AAngled(rb->get_pose()->q) << ")" << std::endl;
std::cerr << " the primitive will have the orientation (" << AAngled(primitive->get_pose()->q) << ")" << std::endl;
}
// save the primitive
_geometry = primitive;
// add this to the primitive
CollisionGeometryPtr cg = dynamic_pointer_cast<CollisionGeometry>(shared_from_this());
primitive->add_collision_geometry(cg);
return primitive;
}
/// 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 (unsigned) distance of a point from this collision geometry
double CollisionGeometry::calc_dist_and_normal(const Point3d& p, std::vector<Vector3d>& normals) const
{
// get the primitive from this
PrimitivePtr primitive = get_geometry();
// get the collision geometry
shared_ptr<const CollisionGeometry> cg_const = dynamic_pointer_cast<const CollisionGeometry>(shared_from_this());
CollisionGeometryPtr cg = const_pointer_cast<CollisionGeometry>(cg_const);
// 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_dist_and_normal(px, normals);
}
/// Gets vertices for a primitive
void CollisionGeometry::get_vertices(std::vector<Point3d>& vertices) const
{
// get the primitive from this
PrimitivePtr primitive = get_geometry();
// get a pointer to this
shared_ptr<const CollisionGeometry> cg_const = dynamic_pointer_cast<const CollisionGeometry>(shared_from_this());
CollisionGeometryPtr cg = const_pointer_cast<CollisionGeometry>(cg_const);
// get the pose for this
shared_ptr<const Pose3d> P = primitive->get_pose(cg);
// get the vertices from the primitive
vertices.clear();
primitive->get_vertices(P, vertices);
}
/// Gets a supporting point for this geometry in a particular direction
Point3d CollisionGeometry::get_supporting_point(const Vector3d& d) const
{
// get the primitive from this
PrimitivePtr primitive = get_geometry();
// get this
shared_ptr<const CollisionGeometry> cg_const = dynamic_pointer_cast<const CollisionGeometry>(shared_from_this());
CollisionGeometryPtr cg = const_pointer_cast<CollisionGeometry>(cg_const);
// get the pose for this
shared_ptr<const Pose3d> P = primitive->get_pose(cg);
// transform the vector
Vector3d dir = Pose3d::transform_vector(P, d);
// get the supporting point from the primitive
return primitive->get_supporting_point(dir);
}