void Correspondence::draw(Viewer &viewer, double radius)
{
// draw the edge plane
viewer.drawEdgePlaneDetails(p,Vec3d(0,0,0),Quaternion(),radius,2,6); // color by camera ID
// draw the 3D line
if (l == NULL)
return;
viewer.drawLine(l->_a->getPosition(),l->_b->getPosition(),PINK,6); // color by camera ID
// for debug info, draw the projection line in yellow
double lambda;
Vec3d m = intersectRayPlane(l->_a->getPosition(),p._normal,p._s,p._normal,lambda);
viewer.drawLine(l->_a->getPosition(),m,YELLOW,3);
m = intersectRayPlane(l->_b->getPosition(),p._normal,p._s,p._normal,lambda);
viewer.drawLine(l->_b->getPosition(),m,YELLOW,3);
double distance = p.distance(l);
}
bool projectRayToGround(const tf::Transformer& listener,
const tf::Stamped<tf::Point> camera_ray,
Eigen::Vector4d ground_plane, std::string ground_frame,
tf::Stamped<tf::Point>* world_point)
{
tf::StampedTransform camera_transform;
// This is a static link, so Time(0) should be fine
if (!listener.canTransform(ground_frame, camera_ray.frame_id_, camera_ray.stamp_)) {
ROS_INFO("Couldn't transform %s to %s\n",camera_ray.frame_id_.c_str(),
ground_frame.c_str());
return false;
}
listener.lookupTransform(ground_frame, camera_ray.frame_id_,ros::Time(0),camera_transform);
tf::Stamped<tf::Point> ground_frame_ray;
listener.transformVector(ground_frame, camera_ray, ground_frame_ray);
Eigen::Vector3d ray, ray_origin;
tf::vectorTFToEigen(ground_frame_ray, ray);
tf::vectorTFToEigen(camera_transform.getOrigin(), ray_origin);
Eigen::Vector3d ground_v = intersectRayPlane(ray, ray_origin, ground_plane);
tf::vectorEigenToTF(ground_v, *world_point);
world_point->frame_id_ = ground_frame;
world_point->stamp_ = camera_ray.stamp_;
return true;
}
//-----------------------------------------------------------------------------
// Name: collideWithWorld()
// Desc: Recursive part of the collision response. This function is the
// one who actually calls the collision check on the meshes
//-----------------------------------------------------------------------------
Fvector msimulator_CollideWithWorld(SCollisionData& cl, Fvector position, Fvector velocity, WORD cnt)
{
//
msimulator_ResolveStuck(cl,position);
// do we need to worry ?
// if (fsimilar(position.x,target.x,EPS_L)&&fsimilar(position.z,target.z,EPS_L))
if (velocity.magnitude()<EPS_L)
{
cl.vVelocity.set(0,0,0);
return position;
}
if (cnt>psCollideActDepth) return cl.vLastSafePosition;
Fvector ret_pos;
Fvector destinationPoint;
destinationPoint.add(position,velocity);
// reset the collision package we send to the mesh
cl.vVelocity.set (velocity);
cl.vSourcePoint.set (position);
cl.bFoundCollision = FALSE;
cl.bStuck = FALSE;
cl.fNearestDistance = -1;
// Check collision
msimulator_CheckCollision (cl);
// check return value here, and possibly call recursively
if (cl.bFoundCollision == FALSE && !cl.bStuck)
{
// if no collision move very close to the desired destination.
float l = velocity.magnitude();
Fvector V;
V.mul(velocity,(l-cl_epsilon)/l);
// return the final position
ret_pos.add(position,V);
// update the last safe position for future error recovery
cl.vLastSafePosition.set(ret_pos);
return ret_pos;
} else {
// There was a collision
// OK, first task is to move close to where we hit something :
Fvector newSourcePoint;
// If we are stuck, we just back up to last safe position
if (cl.bStuck) {
return cl.vLastSafePosition;
}
// only update if we are not already very close
if (cl.fNearestDistance >= cl_epsilon) {
Fvector V;
V.set(velocity);
V.set_length(cl.fNearestDistance-cl_epsilon);
newSourcePoint.add(cl.vSourcePoint,V);
} else {
newSourcePoint.set(cl.vSourcePoint);
}
// Now we must calculate the sliding plane
Fvector slidePlaneOrigin;
slidePlaneOrigin.set(cl.vNearestPolygonIntersectionPoint);
Fvector slidePlaneNormal;
slidePlaneNormal.sub(newSourcePoint,cl.vNearestPolygonIntersectionPoint);
// We now project the destination point onto the sliding plane
float l = intersectRayPlane(destinationPoint, slidePlaneNormal, slidePlaneOrigin, slidePlaneNormal);
// We can now calculate a _new destination point on the sliding plane
Fvector newDestinationPoint;
newDestinationPoint.mad(destinationPoint,slidePlaneNormal,l);
// Generate the slide xr_vector, which will become our _new velocity xr_vector
// for the next iteration
Fvector newVelocityVector;
newVelocityVector.sub(newDestinationPoint, cl.vNearestPolygonIntersectionPoint);
// now we recursively call the function with the _new position and velocity
cl.vLastSafePosition.set(position);
return msimulator_CollideWithWorld(cl, newSourcePoint, newVelocityVector,cnt+1);
}
}
inline Vec3f intersectRayPlane (const Vec3f& o, const Vec3f& d, const Vec4f& plane ) { float t; return intersectRayPlane(t, o,d,plane); }
