// output a conic int Ttt::my_conic_to( const FT_Vector* control, const FT_Vector* to, void* user ) { P to_pt(to); P ctrl_pt(control); P last_pt(&last_point); P diff = ctrl_pt-last_pt ; my_writer->conic_to( to_pt, diff ); last_point = *to; return 0; }
bool COptimizePath::_LineTo(CLine& line, APointF& to) { #define LOCAL_STRICT_LINE A3DPOINT2 to_pt((int)to.x, (int)to.y); A3DPOINT2 cur_pt((int)line.GetFrom().x, (int)line.GetFrom().y); CMoveMap * pMoveMap = g_MoveAgentManager.GetMoveMap(); assert(pMoveMap); #ifdef LOCAL_STRICT_LINE A3DPOINT2 last_pt(cur_pt); #endif while (cur_pt != to_pt ) { APointF cur(line.Next()); cur_pt.x = (int)cur.x; cur_pt.y = (int)cur.y; if (!pMoveMap->IsPosReachable(cur_pt)) { return false; } #ifdef LOCAL_STRICT_LINE if ((cur_pt.x != last_pt.x && cur_pt.y != last_pt.y) &&(!pMoveMap->IsPosReachable(last_pt.x, cur_pt.y) || !pMoveMap->IsPosReachable(cur_pt.x, last_pt.y)) ) { return false; } last_pt = cur_pt; #endif } #undef LOCAL_STRICT_LINE return true; }
void lqr::glpathCallback(const nav_msgs::Path::ConstPtr& path) { //ROS_INFO_STREAM("new path received "); int num_points = path->poses.size(); inited = 1; glpath.clear(); distance_to_last.clear(); dir_vec.clear(); angle_diff_per_m.clear(); vector <double> last_pt(3,0); for(int i = 0; i<num_points; i++) { //getting the orientation: vector <double> q(4); //quaternion q.at(0) = path->poses.at(i).pose.orientation.w; q.at(1) = path->poses.at(i).pose.orientation.x; q.at(2) = path->poses.at(i).pose.orientation.y; q.at(3) = path->poses.at(i).pose.orientation.z; double zangle = get_z_euler_from_quad(q)/PI*180.0; //getting the path points in x and y: double x = path->poses.at(i).pose.position.x; double y = path->poses.at(i).pose.position.y; /*if(i==0) { ROS_INFO_STREAM("path coords at 0: x" << x << " y: " << y ); ROS_INFO_STREAM("orientation at 0, w: " << q.at(0) << " x: "<< q.at(1)<< " y: "<< q.at(2)<< " z: " << q.at(3)); ROS_INFO_STREAM("euler, z angle: " << get_z_euler_from_quad(q)/PI*180.0); }*/ if(i>0) { distance_to_last.push_back( sqrt(pow(last_pt.at(0)-x,2) + pow(last_pt.at(1)-y,2)) ); double angle_diff = zangle - last_pt.at(2); angle_diff_per_m.push_back(angle_diff/distance_to_last.back()); //ROS_INFO_STREAM("distance_to_last at iter: " << i << " is: " << distance_to_last[i-1] << " angle_diff_per_m: " << angle_diff_per_m.back()); double heading[2] = {cos(zangle*PI/180),sin(zangle*PI/180)}; double shiftvec[2] = {x - last_pt.at(0), y - last_pt.at(1)}; if((shiftvec[0]*heading[0]+ shiftvec[1]*heading[1] ) > 0 ) dir_vec.push_back(1); else dir_vec.push_back(-1); } last_pt.at(0) = x; last_pt.at(1) = y; last_pt.at(2) = zangle; vector <double > pathpoint; pathpoint.push_back(x); pathpoint.push_back(y); pathpoint.push_back(zangle); glpath.push_back(pathpoint); } //ROS_INFO_STREAM("new path saved "); calc_des_speed(); }
unsigned int add_arc_as_cubics(int max_recursion, Builder *B, float tol, vec2 start_pt, vec2 end_pt, vec2 center, float radius, float start_angle, float angle) { /* One way to approximate an arc with a cubic-bezier is as * follows (taken from GLyphy which is likely take its * computations from Cairo). * * D = tan(angle / 4) * p0 = start of arc * p3 = end of arc * vp = p3 - p1 * jp = J(vp) * A = (1 - D^2) / 3 * B = (2 * D) / 3 * p1 = p0 + A * vp - B * jp * p2 = p1 - A * vp - B * jp * * and the error between the arc and [p0, p1, p2, p3] is given by * * error <= |vp| * |D|^5 / (54(1 + D^2)) * * Now, |angle| < 2 * PI, implies |angle| / 4 < PI / 4 thus |D| < 1, giving * * error <= |vp| * |D|^5 / 27 * * thus we need: * * |tan(|angle| / 4)|^5 < (27 * tol) / |vp| * * since, tan() is increasing function, * * |angle| < 4 * pow(atan((27 * tol) / |vp|), 0.20f) * */ vec2 vp(end_pt - start_pt); vec2 jp(-vp.y(), vp.x()); float mag_vp(vp.magnitude()); float goal, angle_max; if (mag_vp < tol) { B->line_to(end_pt); return 1; } /* half the tolerance for the cubic to quadratic and half * the tolerance for arc to cubic. */ tol *= 0.5f; goal = t_min(1.0f, (27.0f * tol) / vp.magnitude()); angle_max = t_min(FASTUIDRAW_PI, 4.0f * std::pow(fastuidraw::t_atan(goal), 0.20f)); float angle_remaining(angle); float current_angle(start_angle); float angle_direction(t_sign(angle)); float angle_advance(angle_max * angle_direction); vec2 last_pt(start_pt); unsigned int return_value(0); while (t_abs(angle_remaining) > angle_max) { float next_angle(current_angle + angle_advance); float cos_next_angle(t_cos(next_angle)); float sin_next_angle(t_sin(next_angle)); vec2 next_delta, next_pt; next_pt.x() = center.x() + cos_next_angle * radius; next_pt.y() = center.y() + sin_next_angle * radius; return_value += add_arc_as_single_cubic(max_recursion, B, tol, last_pt, next_pt, angle_advance); current_angle += angle_advance; angle_remaining -= angle_advance; last_pt = next_pt; } return_value += add_arc_as_single_cubic(max_recursion, B, tol, last_pt, end_pt, angle_remaining); return return_value; }