X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/blobdiff_plain/c8fff97c303bf1048f8a8a0a69375ad7d9db027e..90d163f545ab59eb9a286093e4bc6993edfe93c1:/src/rrtext10.cc diff --git a/src/rrtext10.cc b/src/rrtext10.cc index c5b5602..cdb39b8 100644 --- a/src/rrtext10.cc +++ b/src/rrtext10.cc @@ -3,16 +3,16 @@ double RRTExt10::cost_build(RRTNode &f, RRTNode &t) { - double q0[] = {f.x(), f.y(), f.h()}; - double q1[] = {t.x(), t.y(), t.h()}; - ReedsSheppStateSpace rsss(this->bc.mtr()); - return rsss.distance(q0, q1); + double q0[] = {f.x(), f.y(), f.h()}; + double q1[] = {t.x(), t.y(), t.h()}; + ReedsSheppStateSpace rsss(this->bc.mtr()); + return rsss.distance(q0, q1); } double RRTExt10::cost_search(RRTNode &f, RRTNode &t) { - double cost = 0; - cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); - cost += std::abs(t.h() - f.h()); - return cost; + double cost = 0; + cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); + cost += std::abs(t.h() - f.h()); + return cost; }