/// 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); }