5 #define _USE_MATH_DEFINES // for MSVC
12 double diff = to - from;
15 }
else if (diff < -
M_PI){
25 std::vector<Configuration> path;
29 double theta = atan2(dy, dx);
39 std::cout << 1 << std::endl;
42 for (
unsigned int i=1;
i<=
n;
i++){
44 std::cout << pos << std::endl;
48 std::cout << 2 << std::endl;
51 for (
unsigned int i=1;
i<=
n;
i++){
53 std::cout << pos << std::endl;
57 std::cout << 3 << std::endl;
60 for (
unsigned int i=1;
i<=
n;
i++){
62 std::cout << pos << std::endl;
78 if (dx == 0 && dy == 0){
87 double theta = atan2(dy, dx);
90 double d1 = cspace->
weight(2)*fabs(dth1);
94 double d2 = sqrt(dx*dx + dy*dy);
97 double d3 = cspace->
weight(2) * fabs(dth2);
99 double d = d1 + d2 + d3;
102 std::cout <<
"theta = " << theta <<
", dth1 = " << dth1
103 <<
", dth2 = " << dth2 << std::endl;
104 std::cout <<
"d1:" << d1 <<
", d2:" << d2 <<
", d3:" << d3 << std::endl;
111 if (ratio >= 0 && ratio*d < d1){
115 cfg.
value(2) = from.
value(2) + ratio*d/d1*dth1;
117 }
else if (ratio*d >= d1 && ratio*d < (d1+d2)){
118 double r = (ratio*d - d1)/d2;
122 cfg.
value(2) = theta;
124 }
else if (ratio*d >= (d1+d2) && ratio <= 1.0){
128 cfg.
value(2) = theta + (ratio*d-d1-d2)/d3*dth2;
131 std::cout <<
"TGT::interpolate() : invalid ratio(" << ratio <<
")"
143 double theta = atan2(dy, dx);
148 if (dx == 0 && dy == 0) {
154 dth1 *= cspace->
weight(2);
157 dth2 *= cspace->
weight(2);
160 return sqrt(dx*dx + dy*dy) + dth1 + dth2;