/**
* 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;
}
/// Sets the pointer to the inboard link for this joint (and updates the spatial axes, if the outboard link has been set)
void Joint::set_inboard_link(RigidBodyPtr inboard, bool update_pose)
{
_inboard_link = inboard;
if (!inboard)
return;
// add this joint to the outer joints
inboard->_outer_joints.insert(get_this());
// setup F's pose relative to the inboard
set_inboard_pose(inboard->get_pose(), update_pose);
// update articulated body pointers, if possible
if (!inboard->get_articulated_body() && !_abody.expired())
inboard->set_articulated_body(ArticulatedBodyPtr(_abody));
else if (inboard->get_articulated_body() && _abody.expired())
set_articulated_body(ArticulatedBodyPtr(inboard->get_articulated_body()));
// the articulated body pointers must now be equal; it is
// conceivable that the user is updating the art. body pointers in an
// unorthodox manner, but we'll look for this anwyway...
if (!_abody.expired())
{
ArticulatedBodyPtr abody1(inboard->get_articulated_body());
ArticulatedBodyPtr abody2(_abody);
assert(abody1 == abody2);
}
}
/// Determines (and sets) the value of Q from the axes and the inboard link and outboard link transforms
void UniversalJoint::determine_q(VectorNd& q)
{
const unsigned X = 0, Y = 1, Z = 2;
// get the outboard link
RigidBodyPtr outboard = get_outboard_link();
// verify that the outboard link is set
if (!outboard)
throw std::runtime_error("determine_q() called on NULL outboard link!");
// set proper size for q
this->q.resize(num_dof());
// get the poses of the joint and outboard link
shared_ptr<const Pose3d> Fj = get_pose();
shared_ptr<const Pose3d> Fo = outboard->get_pose();
// compute transforms
Transform3d wTo = Pose3d::calc_relative_pose(Fo, GLOBAL);
Transform3d jTw = Pose3d::calc_relative_pose(GLOBAL, Fj);
Transform3d jTo = jTw * wTo;
// determine the joint transformation
Matrix3d R = jTo.q;
// determine q1 and q2 -- they are uniquely determined by examining the rotation matrix
// (see get_rotation())
q.resize(num_dof());
q[DOF_1] = std::atan2(R(Z,Y), R(Y,Y));
q[DOF_2] = std::atan2(R(X,Z), R(X,X));
}
/// Sets the location of this joint with specified inboard and outboard links
void Joint::set_location(const Point3d& point, RigidBodyPtr inboard, RigidBodyPtr outboard)
{
assert(inboard && outboard);
// convert p to the inboard and outboard links' frames
Point3d pi = Pose3d::transform_point(inboard->get_pose(), point);
Point3d po = Pose3d::transform_point(outboard->get_pose(), point);
// set _F's and Fb's origins
_F->x = Origin3d(pi);
_Fb->x = Origin3d(po);
// invalidate all outboard pose vectors
outboard->invalidate_pose_vectors();
// set inboard and outboard links
set_inboard_link(inboard, false);
set_outboard_link(outboard, false);
}
/// Gets the farthest point from this geometry
double CollisionGeometry::get_farthest_point_distance() const
{
// get the primitive from this
PrimitivePtr primitive = get_geometry();
// get the vertices
vector<Point3d> verts;
get_vertices(verts);
if (verts.empty())
return 0.0;
// get the rigid body pose in P's frame
RigidBodyPtr rb = dynamic_pointer_cast<RigidBody>(get_single_body());
Point3d rbX = Pose3d::transform_point(verts.front().pose, Point3d(0,0,0,rb->get_pose()));
// find which point is closest
double max_dist = 0.0;
for (unsigned i=0; i< verts.size(); i++)
max_dist = std::max(max_dist, (verts[i] - rbX).norm());
return max_dist;
}
