Add line len computation

diff --git a/incl/bcar.hh b/incl/bcar.hh
index 1e0b61bf2c1de59076729a28dac3324a26f7eaf2..faa8470bbe74e2f89445735f4cd77f4636f3f2ee 100644 (file)
--- a/incl/bcar.hh
+++ b/incl/bcar.hh
@@ -100,6 +100,8 @@ public:
         * \param p The point to consider.
         */
        bool is_on_right_side(Point const& p) const;
+
+       double len() const;
 };
 
 /*! Store coordinates `x`, `y`, and heading `h`. */

diff --git a/src/bcar.cc b/src/bcar.cc
index 75f220c9d3668c1462eeb49af00e9c3dcf2ed7ed..96a2c8c82ecc5db43f62683fac03b1171999b24b 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -191,6 +191,14 @@ Line::is_on_right_side(Point const& p) const
        }
 }
 
+double
+Line::len() const
+{
+       double dx = this->lp().x() - this->fp().x();
+       double dy = this->lp().y() - this->fp().y();
+       return sqrt(dx * dx + dy * dy);
+}
+
 Pose::Pose() : Point()
 {
 }