Add edist to rr, lf method

diff --git a/incl/bcar.hh b/incl/bcar.hh
index 49122977b42e9d1e762a1e985f52a211253c4fbe..658db31042b6bbc3ae116cd92ce36421f3315ea2 100644 (file)
--- 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 8234b6261b85b0b2ed14e44d906471dcd460ade3..350d1afc144ac2954e5ca381c81018c35fecab16 100644 (file)
--- 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 8770234edc8360f9b300e26667e3e6062112de6b..6849630d7b381f22ab9444e2e83f873bf4252d05 100644 (file)
--- 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")