1219 lines
50 KiB
C++
1219 lines
50 KiB
C++
|
/* Copyright 2017-2018 The MathWorks, Inc. */
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @file
|
||
|
|
* Functors used for Reeds-Shepp motion primitive calculations.
|
||
|
|
* To fully support code generation, note that this file needs to be fully
|
||
|
|
* compliant with the C++98 standard.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#ifndef AUTONOMOUSCODEGEN_REEDS_SHEPP_PRIMITIVES_H_
|
||
|
|
#define AUTONOMOUSCODEGEN_REEDS_SHEPP_PRIMITIVES_H_
|
||
|
|
|
||
|
|
#include <algorithm>
|
||
|
|
#include <vector>
|
||
|
|
|
||
|
|
#ifdef BUILDING_LIBMWAUTONOMOUSCODEGEN
|
||
|
|
#include "autonomouscodegen/autonomouscodegen_constants.hpp" // for pi, twoPi, tooSmall
|
||
|
|
#include "autonomouscodegen/autonomouscodegen_reeds_shepp_constants.hpp" // for TotalNumPaths
|
||
|
|
#include "autonomouscodegen/autonomouscodegen_trig.hpp" // for wrapToPi, toPolar, abs
|
||
|
|
#include "autonomouscodegen/autonomouscodegen_util.hpp" // for data types
|
||
|
|
#else
|
||
|
|
// To deal with the fact that PackNGo has no include file hierarchy during test
|
||
|
|
#include "autonomouscodegen_constants.hpp"
|
||
|
|
#include "autonomouscodegen_reeds_shepp_constants.hpp"
|
||
|
|
#include "autonomouscodegen_trig.hpp"
|
||
|
|
#include "autonomouscodegen_util.hpp"
|
||
|
|
#endif
|
||
|
|
|
||
|
|
namespace autonomous {
|
||
|
|
namespace reedsshepp {
|
||
|
|
|
||
|
|
inline void tauOmega(const real64_T u,
|
||
|
|
const real64_T v,
|
||
|
|
const real64_T xi,
|
||
|
|
const real64_T eta,
|
||
|
|
const real64_T phi,
|
||
|
|
real64_T& tau,
|
||
|
|
real64_T& omega) {
|
||
|
|
real64_T delta = autonomous::wrapToPi(u - v);
|
||
|
|
real64_T A = sin(u) - sin(delta);
|
||
|
|
real64_T B = cos(u) - cos(delta) - 1.0;
|
||
|
|
|
||
|
|
real64_T t1 = atan2(eta * A - xi * B, xi * A + eta * B);
|
||
|
|
real64_T t2 = 2.0 * (cos(delta) - cos(v) - cos(u)) + 3;
|
||
|
|
|
||
|
|
tau = (t2 < 0) ? autonomous::wrapToPi(t1 + autonomous::pi) : autonomous::wrapToPi(t1);
|
||
|
|
omega = autonomous::wrapToPi(tau - u + v - phi);
|
||
|
|
}
|
||
|
|
|
||
|
|
// formula 8.1 in Reeds-Shepp paper
|
||
|
|
inline boolean_T computeLpSpLpPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
real64_T& t,
|
||
|
|
real64_T& u,
|
||
|
|
real64_T& v) {
|
||
|
|
autonomous::toPolar(x - sin(phi), y - 1.0 + cos(phi), u, t);
|
||
|
|
|
||
|
|
if (t >= -autonomous::tooSmall) {
|
||
|
|
v = autonomous::wrapToPi(phi - t);
|
||
|
|
return (v >= -autonomous::tooSmall);
|
||
|
|
}
|
||
|
|
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// formula 8.2
|
||
|
|
inline boolean_T computeLpSpRpPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
real64_T& t,
|
||
|
|
real64_T& u,
|
||
|
|
real64_T& v) {
|
||
|
|
real64_T t1, u1;
|
||
|
|
autonomous::toPolar(x + sin(phi), y - 1.0 - cos(phi), u1, t1);
|
||
|
|
|
||
|
|
u1 = u1 * u1;
|
||
|
|
|
||
|
|
if (u1 >= 4.0) {
|
||
|
|
real64_T theta;
|
||
|
|
|
||
|
|
u = sqrt(u1 - 4.);
|
||
|
|
theta = atan2(2., u);
|
||
|
|
|
||
|
|
t = autonomous::wrapToPi(t1 + theta);
|
||
|
|
v = autonomous::wrapToPi(t - phi);
|
||
|
|
|
||
|
|
return t >= -autonomous::tooSmall && v >= -autonomous::tooSmall;
|
||
|
|
}
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// formula 8.3 / 8.4 *** TYPO IN PAPER ***
|
||
|
|
inline boolean_T computeLpRmLPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
real64_T& t,
|
||
|
|
real64_T& u,
|
||
|
|
real64_T& v) {
|
||
|
|
real64_T xi = x - sin(phi);
|
||
|
|
real64_T eta = y - 1.0 + cos(phi);
|
||
|
|
|
||
|
|
real64_T u1, theta;
|
||
|
|
autonomous::toPolar(xi, eta, u1, theta);
|
||
|
|
|
||
|
|
if (u1 <= 4.0) {
|
||
|
|
u = -2.0 * asin(0.25 * u1);
|
||
|
|
|
||
|
|
t = autonomous::wrapToPi(theta + 0.5 * u + autonomous::pi);
|
||
|
|
v = autonomous::wrapToPi(phi - t + u);
|
||
|
|
|
||
|
|
return t >= -autonomous::tooSmall && u <= autonomous::tooSmall;
|
||
|
|
}
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// formula 8.7
|
||
|
|
inline boolean_T computeLpRupLumRmPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
real64_T& t,
|
||
|
|
real64_T& u,
|
||
|
|
real64_T& v) {
|
||
|
|
real64_T xi = x + sin(phi);
|
||
|
|
real64_T eta = y - 1.0 - cos(phi);
|
||
|
|
real64_T rho = 0.25 * (2.0 + sqrt(xi * xi + eta * eta));
|
||
|
|
|
||
|
|
if (rho <= 1.) {
|
||
|
|
u = acos(rho);
|
||
|
|
|
||
|
|
tauOmega(u, -u, xi, eta, phi, t, v);
|
||
|
|
|
||
|
|
return t >= -autonomous::tooSmall && v <= autonomous::tooSmall;
|
||
|
|
}
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// formula 8.8
|
||
|
|
inline boolean_T computeLpRumLumRpPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
real64_T& t,
|
||
|
|
real64_T& u,
|
||
|
|
real64_T& v) {
|
||
|
|
real64_T xi = x + sin(phi);
|
||
|
|
real64_T eta = y - 1.0 - cos(phi);
|
||
|
|
real64_T rho = (20.0 - xi * xi - eta * eta) / 16.0;
|
||
|
|
|
||
|
|
if (rho >= 0 && rho <= 1) {
|
||
|
|
u = -acos(rho);
|
||
|
|
|
||
|
|
if (u >= -.5 * autonomous::pi) {
|
||
|
|
tauOmega(u, u, xi, eta, phi, t, v);
|
||
|
|
|
||
|
|
return t >= -autonomous::tooSmall && v >= -autonomous::tooSmall;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// formula 8.9
|
||
|
|
inline boolean_T computeLpRmSmLmPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
real64_T& t,
|
||
|
|
real64_T& u,
|
||
|
|
real64_T& v) {
|
||
|
|
real64_T xi = x - sin(phi);
|
||
|
|
real64_T eta = y - 1.0 + cos(phi);
|
||
|
|
|
||
|
|
real64_T rho, theta;
|
||
|
|
autonomous::toPolar(xi, eta, rho, theta);
|
||
|
|
|
||
|
|
if (rho >= 2.0) {
|
||
|
|
real64_T r = sqrt(rho * rho - 4.0);
|
||
|
|
u = 2.0 - r;
|
||
|
|
|
||
|
|
t = autonomous::wrapToPi(theta + atan2(r, -2.0));
|
||
|
|
v = autonomous::wrapToPi(phi - 0.5 * autonomous::pi - t);
|
||
|
|
|
||
|
|
return t >= -autonomous::tooSmall && u <= autonomous::tooSmall && v <= autonomous::tooSmall;
|
||
|
|
}
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// formula 8.10
|
||
|
|
inline boolean_T computeLpRmSmRmPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
real64_T& t,
|
||
|
|
real64_T& u,
|
||
|
|
real64_T& v) {
|
||
|
|
real64_T xi = x + sin(phi);
|
||
|
|
real64_T eta = y - 1.0 - cos(phi);
|
||
|
|
|
||
|
|
real64_T rho, theta;
|
||
|
|
autonomous::toPolar(-eta, xi, rho, theta);
|
||
|
|
|
||
|
|
if (rho >= 2.0) {
|
||
|
|
t = theta;
|
||
|
|
|
||
|
|
u = 2.0 - rho;
|
||
|
|
v = autonomous::wrapToPi(t + 0.5 * autonomous::pi - phi);
|
||
|
|
|
||
|
|
return t >= -autonomous::tooSmall && u <= autonomous::tooSmall && v <= autonomous::tooSmall;
|
||
|
|
}
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// formula 8.11 *** TYPO IN PAPER ***
|
||
|
|
inline boolean_T computeLpRmSLmRpPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
real64_T& t,
|
||
|
|
real64_T& u,
|
||
|
|
real64_T& v) {
|
||
|
|
real64_T xi = x + sin(phi);
|
||
|
|
real64_T eta = y - 1.0 - cos(phi);
|
||
|
|
|
||
|
|
real64_T rho, theta;
|
||
|
|
autonomous::toPolar(xi, eta, rho, theta);
|
||
|
|
|
||
|
|
if (rho >= 2.0) {
|
||
|
|
u = 4.0 - sqrt(rho * rho - 4.0);
|
||
|
|
|
||
|
|
if (u <= autonomous::tooSmall) {
|
||
|
|
t = autonomous::wrapToPi(atan2((4 - u) * xi - 2 * eta, -2 * xi + (u - 4) * eta));
|
||
|
|
v = autonomous::wrapToPi(t - phi);
|
||
|
|
|
||
|
|
return t >= -autonomous::tooSmall && v >= -autonomous::tooSmall;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* RSPathType - Enumeration for holding path type.
|
||
|
|
*/
|
||
|
|
enum RSPathType {
|
||
|
|
LRLNN = 0,
|
||
|
|
RLRNN,
|
||
|
|
LRLRN,
|
||
|
|
RLRLN,
|
||
|
|
LRSLN,
|
||
|
|
RLSRN,
|
||
|
|
LSRLN,
|
||
|
|
RSLRN,
|
||
|
|
LRSRN,
|
||
|
|
RLSLN,
|
||
|
|
RSRLN,
|
||
|
|
LSLRN,
|
||
|
|
LSRNN,
|
||
|
|
RSLNN,
|
||
|
|
LSLNN,
|
||
|
|
RSRNN,
|
||
|
|
LRSLR,
|
||
|
|
RLSRL
|
||
|
|
};
|
||
|
|
|
||
|
|
/*
|
||
|
|
* ReedsSheppPath - Encapsulate normalized Reeds-Shepp path.
|
||
|
|
*/
|
||
|
|
class ReedsSheppPath {
|
||
|
|
|
||
|
|
public:
|
||
|
|
ReedsSheppPath(const real64_T firstSegment = inf,
|
||
|
|
const real64_T secondSegment = inf,
|
||
|
|
const real64_T thirdSegment = inf,
|
||
|
|
const real64_T fourthSegment = inf,
|
||
|
|
const real64_T fifthSegment = inf,
|
||
|
|
const RSPathType pathType = autonomous::reedsshepp::LRLNN,
|
||
|
|
const real64_T forwardCost = 1.0,
|
||
|
|
const real64_T reverseCost = 1.0)
|
||
|
|
: pathType_(pathType)
|
||
|
|
, reverseCost_(reverseCost)
|
||
|
|
, forwardCost_(forwardCost) {
|
||
|
|
|
||
|
|
segmentLengths_[0] = firstSegment;
|
||
|
|
segmentLengths_[1] = secondSegment;
|
||
|
|
segmentLengths_[2] = thirdSegment;
|
||
|
|
segmentLengths_[3] = fourthSegment;
|
||
|
|
segmentLengths_[4] = fifthSegment;
|
||
|
|
}
|
||
|
|
|
||
|
|
real64_T length() const {
|
||
|
|
return autonomous::abs(segmentLengths_[0]) + autonomous::abs(segmentLengths_[1]) +
|
||
|
|
autonomous::abs(segmentLengths_[2]) + autonomous::abs(segmentLengths_[3]) +
|
||
|
|
autonomous::abs(segmentLengths_[4]);
|
||
|
|
}
|
||
|
|
|
||
|
|
real64_T cost() const {
|
||
|
|
real64_T len = 0.0;
|
||
|
|
for (uint32_T i = 0; i < 5; ++i) {
|
||
|
|
if (std::signbit(segmentLengths_[i])) {
|
||
|
|
len += autonomous::abs(reverseCost_ * segmentLengths_[i]);
|
||
|
|
} else {
|
||
|
|
len += autonomous::abs(forwardCost_ * segmentLengths_[i]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return len;
|
||
|
|
}
|
||
|
|
|
||
|
|
RSPathType getPathType() const {
|
||
|
|
return pathType_;
|
||
|
|
}
|
||
|
|
|
||
|
|
const real64_T* getSegmentLengths() const {
|
||
|
|
|
||
|
|
return &segmentLengths_[0];
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
RSPathType pathType_;
|
||
|
|
real64_T segmentLengths_[5];
|
||
|
|
real64_T reverseCost_;
|
||
|
|
real64_T forwardCost_;
|
||
|
|
};
|
||
|
|
|
||
|
|
// Compute for shortest path types
|
||
|
|
/*
|
||
|
|
* computeCSCPath - compute ReedsSheppPath for CSCPath.
|
||
|
|
*/
|
||
|
|
inline void computeCSCPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
const real64_T reverseCost,
|
||
|
|
ReedsSheppPath& path) {
|
||
|
|
real64_T t, u, v, L;
|
||
|
|
real64_T Lmin = path.length();
|
||
|
|
|
||
|
|
if (computeLpSpLpPath(x, y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, u, v, 0.0, 0.0, autonomous::reedsshepp::LSLNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (computeLpSpLpPath(-x, y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, -u, -v, 0.0, 0.0, autonomous::reedsshepp::LSLNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (computeLpSpLpPath(x, -y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, u, v, 0.0, 0.0, autonomous::reedsshepp::RSRNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (computeLpSpLpPath(-x, -y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, -u, -v, 0.0, 0.0, autonomous::reedsshepp::RSRNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (computeLpSpRpPath(x, y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, u, v, 0.0, 0.0, autonomous::reedsshepp::LSRNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (computeLpSpRpPath(-x, y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, -u, -v, 0.0, 0.0, autonomous::reedsshepp::LSRNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (computeLpSpRpPath(x, -y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, u, v, 0.0, 0.0, autonomous::reedsshepp::RSLNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (computeLpSpRpPath(-x, -y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, -u, -v, 0.0, 0.0, autonomous::reedsshepp::RSLNN, 1, reverseCost);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* computeCCCPath - compute ReedsSheppPath for CCCPath.
|
||
|
|
*/
|
||
|
|
inline void computeCCCPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
const real64_T reverseCost,
|
||
|
|
ReedsSheppPath& path) {
|
||
|
|
real64_T t, u, v, L;
|
||
|
|
real64_T Lmin = path.length();
|
||
|
|
|
||
|
|
if (computeLpRmLPath(x, y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, u, v, 0.0, 0.0, autonomous::reedsshepp::LRLNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (computeLpRmLPath(-x, y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, -u, -v, 0.0, 0.0, autonomous::reedsshepp::LRLNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (computeLpRmLPath(x, -y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, u, v, 0.0, 0.0, autonomous::reedsshepp::RLRNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (computeLpRmLPath(-x, -y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, -u, -v, 0.0, 0.0, autonomous::reedsshepp::RLRNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// backwards
|
||
|
|
real64_T xb = x * cos(phi) + y * sin(phi);
|
||
|
|
real64_T yb = x * sin(phi) - y * cos(phi);
|
||
|
|
|
||
|
|
if (computeLpRmLPath(xb, yb, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(v, u, t, 0.0, 0.0, autonomous::reedsshepp::LRLNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (computeLpRmLPath(-xb, yb, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-v, -u, -t, 0.0, 0.0, autonomous::reedsshepp::LRLNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (computeLpRmLPath(xb, -yb, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(v, u, t, 0.0, 0.0, autonomous::reedsshepp::RLRNN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (computeLpRmLPath(-xb, -yb, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-v, -u, -t, 0.0, 0.0, autonomous::reedsshepp::RLRNN, 1, reverseCost);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* computeCCCCPath - compute ReedsSheppPath for CCCCPath.
|
||
|
|
*/
|
||
|
|
inline void computeCCCCPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
const real64_T reverseCost,
|
||
|
|
ReedsSheppPath& path) {
|
||
|
|
real64_T t, u, v, L;
|
||
|
|
real64_T Lmin = path.length();
|
||
|
|
|
||
|
|
if (computeLpRupLumRmPath(x, y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + 2.0 * autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, u, -u, v, 0.0, autonomous::reedsshepp::LRLRN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (computeLpRupLumRmPath(-x, y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + 2.0 * autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, -u, u, -v, 0.0, autonomous::reedsshepp::LRLRN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (computeLpRupLumRmPath(x, -y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + 2.0 * autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, u, -u, v, 0.0, autonomous::reedsshepp::RLRLN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (computeLpRupLumRmPath(-x, -y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + 2.0 * autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, -u, u, -v, 0.0, autonomous::reedsshepp::RLRLN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (computeLpRumLumRpPath(x, y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + 2.0 * autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, u, u, v, 0.0, autonomous::reedsshepp::LRLRN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (computeLpRumLumRpPath(-x, y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + 2.0 * autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, -u, -u, -v, 0.0, autonomous::reedsshepp::LRLRN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (computeLpRumLumRpPath(x, -y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + 2.0 * autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, u, u, v, 0.0, autonomous::reedsshepp::RLRLN, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (computeLpRumLumRpPath(-x, -y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + 2.0 * autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, -u, -u, -v, 0.0, autonomous::reedsshepp::RLRLN, 1, reverseCost);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* computeCCSCPath - compute ReedsSheppPath for CCSCPath.
|
||
|
|
*/
|
||
|
|
inline void computeCCSCPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
const real64_T reverseCost,
|
||
|
|
ReedsSheppPath& path) {
|
||
|
|
real64_T t, u, v, L;
|
||
|
|
real64_T Lmin = path.length() - 0.5 * autonomous::pi;
|
||
|
|
|
||
|
|
if (computeLpRmSmLmPath(x, y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, -0.5 * autonomous::pi, u, v, 0.0, autonomous::reedsshepp::LRSLN, 1,
|
||
|
|
reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (computeLpRmSmLmPath(-x, y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, 0.5 * autonomous::pi, -u, -v, 0.0, autonomous::reedsshepp::LRSLN,
|
||
|
|
1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (computeLpRmSmLmPath(x, -y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, -0.5 * autonomous::pi, u, v, 0.0, autonomous::reedsshepp::RLSRN, 1,
|
||
|
|
reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (computeLpRmSmLmPath(-x, -y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, 0.5 * autonomous::pi, -u, -v, 0.0, autonomous::reedsshepp::RLSRN,
|
||
|
|
1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (computeLpRmSmRmPath(x, y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, -0.5 * autonomous::pi, u, v, 0.0, autonomous::reedsshepp::LRSRN, 1,
|
||
|
|
reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (computeLpRmSmRmPath(-x, y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, 0.5 * autonomous::pi, -u, -v, 0.0, autonomous::reedsshepp::LRSRN,
|
||
|
|
1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (computeLpRmSmRmPath(x, -y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, -0.5 * autonomous::pi, u, v, 0.0, autonomous::reedsshepp::RLSLN, 1,
|
||
|
|
reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (computeLpRmSmRmPath(-x, -y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, 0.5 * autonomous::pi, -u, -v, 0.0, autonomous::reedsshepp::RLSLN,
|
||
|
|
1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// backwards
|
||
|
|
real64_T xb = x * cos(phi) + y * sin(phi);
|
||
|
|
real64_T yb = x * sin(phi) - y * cos(phi);
|
||
|
|
|
||
|
|
if (computeLpRmSmLmPath(xb, yb, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(v, u, -0.5 * autonomous::pi, t, 0.0, autonomous::reedsshepp::LSRLN, 1,
|
||
|
|
reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (computeLpRmSmLmPath(-xb, yb, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-v, -u, 0.5 * autonomous::pi, -t, 0.0, autonomous::reedsshepp::LSRLN,
|
||
|
|
1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (computeLpRmSmLmPath(xb, -yb, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(v, u, -0.5 * autonomous::pi, t, 0.0, autonomous::reedsshepp::RSLRN, 1,
|
||
|
|
reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (computeLpRmSmLmPath(-xb, -yb, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-v, -u, 0.5 * autonomous::pi, -t, 0.0, autonomous::reedsshepp::RSLRN,
|
||
|
|
1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (computeLpRmSmRmPath(xb, yb, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(v, u, -0.5 * autonomous::pi, t, 0.0, autonomous::reedsshepp::RSRLN, 1,
|
||
|
|
reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (computeLpRmSmRmPath(-xb, yb, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-v, -u, 0.5 * autonomous::pi, -t, 0.0, autonomous::reedsshepp::RSRLN,
|
||
|
|
1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (computeLpRmSmRmPath(xb, -yb, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(v, u, -0.5 * autonomous::pi, t, 0.0, autonomous::reedsshepp::LSLRN, 1,
|
||
|
|
reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (computeLpRmSmRmPath(-xb, -yb, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-v, -u, 0.5 * autonomous::pi, -t, 0.0, autonomous::reedsshepp::LSLRN,
|
||
|
|
1, reverseCost);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* computeCCSCCPath - compute ReedsSheppPath for CCSCCPath.
|
||
|
|
*/
|
||
|
|
inline void computeCCSCCPath(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
const real64_T reverseCost,
|
||
|
|
ReedsSheppPath& path) {
|
||
|
|
real64_T t, u, v, L;
|
||
|
|
real64_T Lmin = path.length() - autonomous::pi;
|
||
|
|
|
||
|
|
if (computeLpRmSLmRpPath(x, y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, -0.5 * autonomous::pi, u, -0.5 * autonomous::pi, v,
|
||
|
|
autonomous::reedsshepp::LRSLR, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (computeLpRmSLmRpPath(-x, y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, 0.5 * autonomous::pi, -u, 0.5 * autonomous::pi, -v,
|
||
|
|
autonomous::reedsshepp::LRSLR, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (computeLpRmSLmRpPath(x, -y, -phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(t, -0.5 * autonomous::pi, u, -0.5 * autonomous::pi, v,
|
||
|
|
autonomous::reedsshepp::RLSRL, 1, reverseCost);
|
||
|
|
Lmin = L;
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (computeLpRmSLmRpPath(-x, -y, phi, t, u, v) &&
|
||
|
|
Lmin > (L = autonomous::abs(t) + autonomous::abs(u) + autonomous::abs(v))) {
|
||
|
|
path = ReedsSheppPath(-t, 0.5 * autonomous::pi, -u, 0.5 * autonomous::pi, -v,
|
||
|
|
autonomous::reedsshepp::RLSRL, 1, reverseCost);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* Compute all path types.
|
||
|
|
*/
|
||
|
|
|
||
|
|
/*
|
||
|
|
* computeCSCPath - compute ReedsSheppPath for CSCPath.
|
||
|
|
*/
|
||
|
|
inline void computeCSCPathAll(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
const real64_T forwardCost,
|
||
|
|
const real64_T reverseCost,
|
||
|
|
const boolean_T allPathTypes[TotalNumPaths],
|
||
|
|
std::vector<ReedsSheppPath>* path) {
|
||
|
|
real64_T t, u, v;
|
||
|
|
|
||
|
|
if (allPathTypes[0]) {
|
||
|
|
computeLpSpLpPath(x, y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, u, v, 0.0, 0.0, autonomous::reedsshepp::LSLNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, inf, 0.0, 0.0, autonomous::reedsshepp::LSLNN,
|
||
|
|
forwardCost, reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (allPathTypes[1]) {
|
||
|
|
computeLpSpLpPath(-x, y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, -u, -v, 0.0, 0.0, autonomous::reedsshepp::LSLNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, -inf, 0.0, 0.0,
|
||
|
|
autonomous::reedsshepp::LSLNN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (allPathTypes[2]) {
|
||
|
|
computeLpSpLpPath(x, -y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, u, v, 0.0, 0.0, autonomous::reedsshepp::RSRNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, inf, 0.0, 0.0, autonomous::reedsshepp::RSRNN,
|
||
|
|
forwardCost, reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (allPathTypes[3]) {
|
||
|
|
computeLpSpLpPath(-x, -y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, -u, -v, 0.0, 0.0, autonomous::reedsshepp::RSRNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, -inf, 0.0, 0.0,
|
||
|
|
autonomous::reedsshepp::RSRNN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (allPathTypes[4]) {
|
||
|
|
computeLpSpRpPath(x, y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, u, v, 0.0, 0.0, autonomous::reedsshepp::LSRNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, inf, 0.0, 0.0, autonomous::reedsshepp::LSRNN,
|
||
|
|
forwardCost, reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (allPathTypes[5]) {
|
||
|
|
computeLpSpRpPath(-x, y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, -u, -v, 0.0, 0.0, autonomous::reedsshepp::LSRNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, -inf, 0.0, 0.0,
|
||
|
|
autonomous::reedsshepp::LSRNN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (allPathTypes[6]) {
|
||
|
|
computeLpSpRpPath(x, -y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, u, v, 0.0, 0.0, autonomous::reedsshepp::RSLNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, inf, 0.0, 0.0, autonomous::reedsshepp::RSLNN,
|
||
|
|
forwardCost, reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (allPathTypes[7]) {
|
||
|
|
computeLpSpRpPath(-x, -y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, -u, -v, 0.0, 0.0, autonomous::reedsshepp::RSLNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, -inf, 0.0, 0.0,
|
||
|
|
autonomous::reedsshepp::RSLNN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* computeCCCPath - compute ReedsSheppPath for CCCPath.
|
||
|
|
*/
|
||
|
|
inline void computeCCCPathAll(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
const real64_T forwardCost,
|
||
|
|
const real64_T reverseCost,
|
||
|
|
const boolean_T allPathTypes[TotalNumPaths],
|
||
|
|
std::vector<ReedsSheppPath>* path) {
|
||
|
|
real64_T t, u, v;
|
||
|
|
|
||
|
|
if (allPathTypes[8]) {
|
||
|
|
computeLpRmLPath(x, y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, u, v, 0.0, 0.0, autonomous::reedsshepp::LRLNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, inf, 0.0, 0.0, autonomous::reedsshepp::LRLNN,
|
||
|
|
forwardCost, reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (allPathTypes[9]) {
|
||
|
|
computeLpRmLPath(-x, y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, -u, -v, 0.0, 0.0, autonomous::reedsshepp::LRLNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, -inf, 0.0, 0.0,
|
||
|
|
autonomous::reedsshepp::LRLNN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (allPathTypes[10]) {
|
||
|
|
computeLpRmLPath(x, -y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, u, v, 0.0, 0.0, autonomous::reedsshepp::RLRNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, inf, 0.0, 0.0, autonomous::reedsshepp::RLRNN,
|
||
|
|
forwardCost, reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (allPathTypes[11]) {
|
||
|
|
computeLpRmLPath(-x, -y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, -u, -v, 0.0, 0.0, autonomous::reedsshepp::RLRNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, -inf, 0.0, 0.0,
|
||
|
|
autonomous::reedsshepp::RLRNN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// backwards
|
||
|
|
real64_T xb = x * cos(phi) + y * sin(phi);
|
||
|
|
real64_T yb = x * sin(phi) - y * cos(phi);
|
||
|
|
|
||
|
|
if (allPathTypes[12]) {
|
||
|
|
computeLpRmLPath(xb, yb, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(v, u, t, 0.0, 0.0, autonomous::reedsshepp::LRLNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, inf, 0.0, 0.0, autonomous::reedsshepp::LRLNN,
|
||
|
|
forwardCost, reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (allPathTypes[13]) {
|
||
|
|
computeLpRmLPath(-xb, yb, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-v, -u, -t, 0.0, 0.0, autonomous::reedsshepp::LRLNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, -inf, 0.0, 0.0,
|
||
|
|
autonomous::reedsshepp::LRLNN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (allPathTypes[14]) {
|
||
|
|
computeLpRmLPath(xb, -yb, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(v, u, t, 0.0, 0.0, autonomous::reedsshepp::RLRNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, inf, 0.0, 0.0, autonomous::reedsshepp::RLRNN,
|
||
|
|
forwardCost, reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (allPathTypes[15]) {
|
||
|
|
computeLpRmLPath(-xb, -yb, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-v, -u, -t, 0.0, 0.0, autonomous::reedsshepp::RLRNN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, -inf, 0.0, 0.0,
|
||
|
|
autonomous::reedsshepp::RLRNN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* computeCCCCPath - compute ReedsSheppPath for CCCCPath.
|
||
|
|
*/
|
||
|
|
inline void computeCCCCPathAll(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
const real64_T forwardCost,
|
||
|
|
const real64_T reverseCost,
|
||
|
|
const boolean_T allPathTypes[TotalNumPaths],
|
||
|
|
std::vector<ReedsSheppPath>* path) {
|
||
|
|
real64_T t, u, v;
|
||
|
|
|
||
|
|
if (allPathTypes[16]) {
|
||
|
|
computeLpRupLumRmPath(x, y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, u, -u, v, 0.0, autonomous::reedsshepp::LRLRN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, -inf, inf, 0.0,
|
||
|
|
autonomous::reedsshepp::LRLRN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (allPathTypes[17]) {
|
||
|
|
computeLpRupLumRmPath(-x, y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, -u, u, -v, 0.0, autonomous::reedsshepp::LRLRN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, inf, -inf, 0.0,
|
||
|
|
autonomous::reedsshepp::LRLRN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (allPathTypes[18]) {
|
||
|
|
computeLpRupLumRmPath(x, -y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, u, -u, v, 0.0, autonomous::reedsshepp::RLRLN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, -inf, inf, 0.0,
|
||
|
|
autonomous::reedsshepp::RLRLN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (allPathTypes[19]) {
|
||
|
|
computeLpRupLumRmPath(-x, -y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, -u, u, -v, 0.0, autonomous::reedsshepp::RLRLN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, inf, -inf, 0.0,
|
||
|
|
autonomous::reedsshepp::RLRLN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (allPathTypes[20]) {
|
||
|
|
computeLpRumLumRpPath(x, y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, u, u, v, 0.0, autonomous::reedsshepp::LRLRN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, inf, inf, 0.0, autonomous::reedsshepp::LRLRN,
|
||
|
|
forwardCost, reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (allPathTypes[21]) {
|
||
|
|
computeLpRumLumRpPath(-x, y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, -u, -u, -v, 0.0, autonomous::reedsshepp::LRLRN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, -inf, -inf, 0.0,
|
||
|
|
autonomous::reedsshepp::LRLRN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (allPathTypes[22]) {
|
||
|
|
computeLpRumLumRpPath(x, -y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, u, u, v, 0.0, autonomous::reedsshepp::RLRLN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, inf, inf, 0.0, autonomous::reedsshepp::RLRLN,
|
||
|
|
forwardCost, reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (allPathTypes[23]) {
|
||
|
|
computeLpRumLumRpPath(-x, -y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, -u, -u, -v, 0.0, autonomous::reedsshepp::RLRLN,
|
||
|
|
forwardCost, reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, -inf, -inf, 0.0,
|
||
|
|
autonomous::reedsshepp::RLRLN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* computeCCSCPath - compute ReedsSheppPath for CCSCPath.
|
||
|
|
*/
|
||
|
|
inline void computeCCSCPathAll(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
const real64_T forwardCost,
|
||
|
|
const real64_T reverseCost,
|
||
|
|
const boolean_T allPathTypes[TotalNumPaths],
|
||
|
|
std::vector<ReedsSheppPath>* path) {
|
||
|
|
real64_T t, u, v;
|
||
|
|
|
||
|
|
if (allPathTypes[24]) {
|
||
|
|
computeLpRmSmLmPath(x, y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, -0.5 * autonomous::pi, u, v, 0.0,
|
||
|
|
autonomous::reedsshepp::LRSLN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, -0.5 * autonomous::pi, inf, inf, 0.0,
|
||
|
|
autonomous::reedsshepp::LRSLN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (allPathTypes[25]) {
|
||
|
|
computeLpRmSmLmPath(-x, y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, 0.5 * autonomous::pi, -u, -v, 0.0,
|
||
|
|
autonomous::reedsshepp::LRSLN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, 0.5 * autonomous::pi, -inf, -inf, 0.0,
|
||
|
|
autonomous::reedsshepp::LRSLN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (allPathTypes[26]) {
|
||
|
|
computeLpRmSmLmPath(x, -y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, -0.5 * autonomous::pi, u, v, 0.0,
|
||
|
|
autonomous::reedsshepp::RLSRN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, -0.5 * autonomous::pi, inf, inf, 0.0,
|
||
|
|
autonomous::reedsshepp::RLSRN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (allPathTypes[27]) {
|
||
|
|
computeLpRmSmLmPath(-x, -y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, 0.5 * autonomous::pi, -u, -v, 0.0,
|
||
|
|
autonomous::reedsshepp::RLSRN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, 0.5 * autonomous::pi, -inf, -inf, 0.0,
|
||
|
|
autonomous::reedsshepp::RLSRN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (allPathTypes[28]) {
|
||
|
|
computeLpRmSmRmPath(x, y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, -0.5 * autonomous::pi, u, v, 0.0,
|
||
|
|
autonomous::reedsshepp::LRSRN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, -0.5 * autonomous::pi, inf, inf, 0.0,
|
||
|
|
autonomous::reedsshepp::LRSRN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (allPathTypes[29]) {
|
||
|
|
computeLpRmSmRmPath(-x, y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, 0.5 * autonomous::pi, -u, -v, 0.0,
|
||
|
|
autonomous::reedsshepp::LRSRN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, 0.5 * autonomous::pi, -inf, -inf, 0.0,
|
||
|
|
autonomous::reedsshepp::LRSRN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (allPathTypes[30]) {
|
||
|
|
computeLpRmSmRmPath(x, -y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, -0.5 * autonomous::pi, u, v, 0.0,
|
||
|
|
autonomous::reedsshepp::RLSLN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, -0.5 * autonomous::pi, inf, inf, 0.0,
|
||
|
|
autonomous::reedsshepp::RLSLN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (allPathTypes[31]) {
|
||
|
|
computeLpRmSmRmPath(-x, -y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, 0.5 * autonomous::pi, -u, -v, 0.0,
|
||
|
|
autonomous::reedsshepp::RLSLN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, 0.5 * autonomous::pi, -inf, -inf, 0.0,
|
||
|
|
autonomous::reedsshepp::RLSLN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// backwards
|
||
|
|
real64_T xb = x * cos(phi) + y * sin(phi);
|
||
|
|
real64_T yb = x * sin(phi) - y * cos(phi);
|
||
|
|
|
||
|
|
if (allPathTypes[32]) {
|
||
|
|
computeLpRmSmLmPath(xb, yb, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(v, u, -0.5 * autonomous::pi, t, 0.0,
|
||
|
|
autonomous::reedsshepp::LSRLN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, -0.5 * autonomous::pi, inf, 0.0,
|
||
|
|
autonomous::reedsshepp::LSRLN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (allPathTypes[33]) {
|
||
|
|
computeLpRmSmLmPath(-xb, yb, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-v, -u, 0.5 * autonomous::pi, -t, 0.0,
|
||
|
|
autonomous::reedsshepp::LSRLN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, 0.5 * autonomous::pi, -inf, 0.0,
|
||
|
|
autonomous::reedsshepp::LSRLN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (allPathTypes[34]) {
|
||
|
|
computeLpRmSmLmPath(xb, -yb, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(v, u, -0.5 * autonomous::pi, t, 0.0,
|
||
|
|
autonomous::reedsshepp::RSLRN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, -0.5 * autonomous::pi, inf, 0.0,
|
||
|
|
autonomous::reedsshepp::RSLRN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (allPathTypes[35]) {
|
||
|
|
computeLpRmSmLmPath(-xb, -yb, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-v, -u, 0.5 * autonomous::pi, -t, 0.0,
|
||
|
|
autonomous::reedsshepp::RSLRN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, 0.5 * autonomous::pi, -inf, 0.0,
|
||
|
|
autonomous::reedsshepp::RSLRN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (allPathTypes[36]) {
|
||
|
|
computeLpRmSmRmPath(xb, yb, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(v, u, -0.5 * autonomous::pi, t, 0.0,
|
||
|
|
autonomous::reedsshepp::RSRLN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, -0.5 * autonomous::pi, inf, 0.0,
|
||
|
|
autonomous::reedsshepp::RSRLN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (allPathTypes[37]) {
|
||
|
|
computeLpRmSmRmPath(-xb, yb, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-v, -u, 0.5 * autonomous::pi, -t, 0.0,
|
||
|
|
autonomous::reedsshepp::RSRLN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, 0.5 * autonomous::pi, -inf, 0.0,
|
||
|
|
autonomous::reedsshepp::RSRLN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (allPathTypes[38]) {
|
||
|
|
computeLpRmSmRmPath(xb, -yb, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(v, u, -0.5 * autonomous::pi, t, 0.0,
|
||
|
|
autonomous::reedsshepp::LSLRN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, inf, -0.5 * autonomous::pi, inf, 0.0,
|
||
|
|
autonomous::reedsshepp::LSLRN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (allPathTypes[39]) {
|
||
|
|
computeLpRmSmRmPath(-xb, -yb, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-v, -u, 0.5 * autonomous::pi, -t, 0.0,
|
||
|
|
autonomous::reedsshepp::LSLRN, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, -inf, 0.5 * autonomous::pi, -inf, 0.0,
|
||
|
|
autonomous::reedsshepp::LSLRN, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* computeCCSCCPath - compute ReedsSheppPath for CCSCCPath.
|
||
|
|
*/
|
||
|
|
inline void computeCCSCCPathAll(const real64_T x,
|
||
|
|
const real64_T y,
|
||
|
|
const real64_T phi,
|
||
|
|
real64_T const forwardCost,
|
||
|
|
const real64_T reverseCost,
|
||
|
|
const boolean_T allPathTypes[TotalNumPaths],
|
||
|
|
std::vector<ReedsSheppPath>* path) {
|
||
|
|
real64_T t, u, v;
|
||
|
|
|
||
|
|
if (allPathTypes[40]) {
|
||
|
|
computeLpRmSLmRpPath(x, y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, -0.5 * autonomous::pi, u, -0.5 * autonomous::pi, v,
|
||
|
|
autonomous::reedsshepp::LRSLR, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, -0.5 * autonomous::pi, inf, -0.5 * autonomous::pi,
|
||
|
|
inf, autonomous::reedsshepp::LRSLR, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip
|
||
|
|
if (allPathTypes[41]) {
|
||
|
|
computeLpRmSLmRpPath(-x, y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, 0.5 * autonomous::pi, -u, 0.5 * autonomous::pi, -v,
|
||
|
|
autonomous::reedsshepp::LRSLR, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, 0.5 * autonomous::pi, -inf, 0.5 * autonomous::pi,
|
||
|
|
-inf, autonomous::reedsshepp::LRSLR, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// reflect
|
||
|
|
if (allPathTypes[42]) {
|
||
|
|
computeLpRmSLmRpPath(x, -y, -phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(t, -0.5 * autonomous::pi, u, -0.5 * autonomous::pi, v,
|
||
|
|
autonomous::reedsshepp::RLSRL, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(inf, -0.5 * autonomous::pi, inf, -0.5 * autonomous::pi,
|
||
|
|
inf, autonomous::reedsshepp::RLSRL, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
|
||
|
|
// time flip + reflect
|
||
|
|
if (allPathTypes[43]) {
|
||
|
|
computeLpRmSLmRpPath(-x, -y, phi, t, u, v)
|
||
|
|
? path->push_back(ReedsSheppPath(-t, 0.5 * autonomous::pi, -u, 0.5 * autonomous::pi, -v,
|
||
|
|
autonomous::reedsshepp::RLSRL, forwardCost,
|
||
|
|
reverseCost))
|
||
|
|
: path->push_back(ReedsSheppPath(-inf, 0.5 * autonomous::pi, -inf, 0.5 * autonomous::pi,
|
||
|
|
-inf, autonomous::reedsshepp::RLSRL, forwardCost,
|
||
|
|
reverseCost));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
} // namespace reedsshepp
|
||
|
|
} // namespace autonomous
|
||
|
|
|
||
|
|
#endif /* AUTONOMOUSCODEGEN_REEDS_SHEPP_PRIMITIVES_H_ */
|