From 683fa9ec998ee17d2235b5b2b881c5d2be38b34c Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 15 Aug 2022 16:03:35 +0200 Subject: [PATCH] Add edist to rr, lf method --- incl/bcar.hh | 6 ++++++ src/bcar.cc | 12 ++++++++++++ ut/bcar.t.cc | 6 ++++++ 3 files changed, 24 insertions(+) diff --git a/incl/bcar.hh b/incl/bcar.hh index 4912297..658db31 100644 --- a/incl/bcar.hh +++ b/incl/bcar.hh @@ -265,6 +265,12 @@ public: /*! Get front track. */ double ft() const; + /*! Return rear axle center to right rear corner Euclidean distance. */ + double edist_to_rr() const; + + /*! Return rear axle center to left front corner Euclidean distance. */ + double edist_to_lf() const; + /*! \brief Get minimum turning radius. * * Please, note that the method returns really _minimum turning radius_, diff --git a/src/bcar.cc b/src/bcar.cc index 8234b62..350d1af 100644 --- a/src/bcar.cc +++ b/src/bcar.cc @@ -541,6 +541,18 @@ CarSize::ft() const return this->_front_track; } +double +CarSize::edist_to_rr() const +{ + return sqrt(pow(this->w() / 2.0, 2) + pow(this->len() - this->df(), 2)); +} + +double +CarSize::edist_to_lf() const +{ + return sqrt(pow(this->w() / 2.0, 2) + pow(this->df(), 2)); +} + double CarSize::mtr() const { diff --git a/ut/bcar.t.cc b/ut/bcar.t.cc index 8770234..6849630 100644 --- a/ut/bcar.t.cc +++ b/ut/bcar.t.cc @@ -76,6 +76,12 @@ WVTEST_MAIN("bcar basic geometry") WVPASSEQ_DOUBLE(0.0, bc.x(), 0.00001); WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001); WVPASSEQ_DOUBLE(M_PI / 2.0, bc.h(), 0.00001); + + // distance to rr, lf + WVPASSEQ_DOUBLE(0.70710677, bc.edist_to_rr(), 10e-5); + WVPASSEQ_DOUBLE(bc.edist(bc.rr()), bc.edist_to_rr(), 10e-5); + WVPASSEQ_DOUBLE(2.5495098, bc.edist_to_lf(), 10e-5); + WVPASSEQ_DOUBLE(bc.edist(bc.lf()), bc.edist_to_lf(), 10e-5); } WVTEST_MAIN("test collide functions") -- 2.39.2