| 61 | } |
| 62 | |
| 63 | double ViewNode::computeCost(const Vector3d& p1, const Vector3d& p2, const double& y1, |
| 64 | const double& y2, const Vector3d& v1, const double& yd1, vector<Vector3d>& path) { |
| 65 | // Cost of position change |
| 66 | double pos_cost = ViewNode::searchPath(p1, p2, path) / vm_; |
| 67 | |
| 68 | // Consider velocity change |
| 69 | if (v1.norm() > 1e-3) { |
| 70 | Vector3d dir = (p2 - p1).normalized(); |
| 71 | Vector3d vdir = v1.normalized(); |
| 72 | double diff = acos(vdir.dot(dir)); |
| 73 | pos_cost += w_dir_ * diff; |
| 74 | // double vc = v1.dot(dir); |
| 75 | // pos_cost += w_dir_ * pow(vm_ - fabs(vc), 2) / (2 * vm_ * am_); |
| 76 | // if (vc < 0) |
| 77 | // pos_cost += w_dir_ * 2 * fabs(vc) / am_; |
| 78 | } |
| 79 | |
| 80 | // Cost of yaw change |
| 81 | double diff = fabs(y2 - y1); |
| 82 | diff = min(diff, 2 * M_PI - diff); |
| 83 | double yaw_cost = diff / yd_; |
| 84 | return max(pos_cost, yaw_cost); |
| 85 | |
| 86 | // // Consider yaw rate change |
| 87 | // if (fabs(yd1) > 1e-3) |
| 88 | // { |
| 89 | // double diff1 = y2 - y1; |
| 90 | // while (diff1 < -M_PI) |
| 91 | // diff1 += 2 * M_PI; |
| 92 | // while (diff1 > M_PI) |
| 93 | // diff1 -= 2 * M_PI; |
| 94 | // double diff2 = diff1 > 0 ? diff1 - 2 * M_PI : 2 * M_PI + diff1; |
| 95 | // } |
| 96 | // else |
| 97 | // { |
| 98 | // } |
| 99 | } |
| 100 | } // namespace fast_planner |