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