/* 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_ */