Get rid of boost includes

[hubacji1/rrts.git] / src / reeds_shepp.cpp

diff --git a/src/reeds_shepp.cpp b/src/reeds_shepp.cpp
index 95628a19c96555bfd193eca570b05af556e26289..2955312c16b8017336675cff52e89372c629ec1e 100644 (file)
--- a/src/reeds_shepp.cpp
+++ b/src/reeds_shepp.cpp
@@ -1,50 +1,16 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2016, Guan-Horng Liu.
-*  All rights reserved.
-*
-*  Redistribution and use in source and binary forms, with or without
-*  modification, are permitted provided that the following conditions
-*  are met:
-*
-*   * Redistributions of source code must retain the above copyright
-*     notice, this list of conditions and the following disclaimer.
-*   * Redistributions in binary form must reproduce the above
-*     copyright notice, this list of conditions and the following
-*     disclaimer in the documentation and/or other materials provided
-*     with the distribution.
-*   * Neither the name of the the copyright holder nor the names of its
-*     contributors may be used to endorse or promote products derived
-*     from this software without specific prior written permission.
-*
-*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-*  POSSIBILITY OF SUCH DAMAGE.
-*
-* Author:  Guan-Horng Liu
-*********************************************************************/
-
+// Copyright (c) 2016, Guan-Horng Liu.
+// SPDX-FileCopyrightText: 2016 Guan-Horng Liu
+//
+// SPDX-License-Identifier: BSD-3-Clause
 
 #include "reeds_shepp.h"
-#include <boost/math/constants/constants.hpp>
-
+#include <cmath>
 
 namespace
 {
     // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
 
-    const double pi = boost::math::constants::pi<double>();
+    const double pi = M_PI;
     const double twopi = 2. * pi;
     const double RS_EPS = 1e-6;
     const double ZERO = 10*std::numeric_limits<double>::epsilon();
@@ -563,14 +529,14 @@ void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size,
     double dist = rho_ * path.length();
 
     for (double seg=0.0; seg<=dist; seg+=step_size){
-        double qnew[3] = {};
+        double qnew[4] = {};
         interpolate(q0, path, seg/rho_, qnew);
         cb( qnew, user_data);
     }
     return;
 }
 
-void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[3])
+void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[4])
 {
 
     if (seg < 0.0) seg = 0.0;
@@ -617,4 +583,5 @@ void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, doubl
 
     s[0] = s[0] * rho_ + q0[0];
     s[1] = s[1] * rho_ + q0[1];
+    s[3] = v;
 }