vehicle_platform/cost.cc

   1 /*
   2 This file is part of I am car.
   3
   4 I am car is free software: you can redistribute it and/or modify
   5 it under the terms of the GNU General Public License as published by
   6 the Free Software Foundation, either version 3 of the License, or
   7 (at your option) any later version.
   8
   9 I am car is distributed in the hope that it will be useful,
  10 but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 GNU General Public License for more details.
  13
  14 You should have received a copy of the GNU General Public License
  15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
  16 */
  17
  18 #include <cmath>
  19 #include "cost.h"
  20 #include "reeds_shepp.h"
  21 #include "rrtnode.h"
  22
  23 float co1(RRTNode *init, RRTNode *goal)
  24 {
  25         float dx = goal->x() - init->x();
  26         float dy = goal->y() - init->y();
  27         return pow(pow(dx, 2) + pow(dy, 2), 0.5);
  28 }
  29
  30 float cco1(RRTNode *init, RRTNode *goal)
  31 {
  32         return init->ccost() + co1(init, goal);
  33 }
  34
  35 float co2(RRTNode *init, RRTNode *goal)
  36 {
  37         double q0[] = {init->x(), init->y(), init->h()};
  38         double q1[] = {goal->x(), goal->y(), goal->h()};
  39         ReedsSheppStateSpace rsss(10.82); // TODO const param
  40         return static_cast<float>(rsss.distance(q0, q1));
  41 }
  42
  43 float cco2(RRTNode *init, RRTNode *goal)
  44 {
  45         return init->ccost() + co2(init, goal);
  46 }