Fix unnormed vector in post-crash collision detection

• The norm of a vector was being calculated but not assigned
• Optimize inefficient zero length test of a vector

Closes #259 (closed)

sim/src/common/vector2d.h

@@ -2,7 +2,7 @@
  * Copyright (c) 2020 HLRS, University of Stuttgart
  *               2017-2021 ITK Engineering GmbH
  *               2018-2020 in-tech GmbH
- *               2021-2023 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ *               2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
  *
  * This program and the accompanying materials are made available under the
  * terms of the Eclipse Public License 2.0 which is available at
@@ -35,6 +35,9 @@ class OPENPASSCOMMONEXPORT Vector2d final
 public:
   static T constexpr EPSILON{1e-9};  //!< set epsilon
 
+  /// unit type of an area in the coordinate space of this vector
+  using SquaredUnit = decltype(std::declval<T>() * std::declval<T>());
+
   /// x dimension
   T x;
   /// y dimension
@@ -145,32 +148,46 @@ public:
     return x * in.y - y * in.x;
   }
 
-  //! @brief Method which checks whether the vector has a length
-  //!
-  //! @return true if the vector has no length
-  bool HasNoLength() { return units::math::abs(Length()) < EPSILON; }
-
   /*!
-   * Normalizes the 2d vector
+   * Returns whether the vector has a squared length of nearly zero.
    *
-   * Each component of the vector is devided by the length of the vector.
+   * \return true if the vector has no length
+   */
+  inline bool HasNoLength() const noexcept { return units::math::abs(Dot(*this)) < Vector2d<SquaredUnit>::EPSILON; }
+
+  /*!
+   * Returns the norm of this 2d vector.
    *
-   * In case of a vector with length 0, the vector cannot be normalized and false is returned.
+   * In case of a vector with length 0, the vector cannot be normalized and a zero vector is returned.
    *
-   * \return returns true if vector could be normalized, false otherwise
+   * \return vector scaled to unit length if it has non-zero length. Otherwise returns a zero length vector.
    */
-  Vector2d<units::dimensionless::scalar_t> Norm()
+  Vector2d<units::dimensionless::scalar_t> Norm() const
   {
-    auto length = Length();
-
-    if (HasNoLength())
+    const auto length{Length()};
+    if (length < EPSILON)
     {
-      throw std::runtime_error("Can't normalize Vector2d with length of 0.0");
+      return {};
     }
-
     return Vector2d<units::dimensionless::scalar_t>(x / length, y / length);
   }
 
+  /*!
+   * Normalizes this vector if its length is greater than the given epsilon.
+   * If not, no change is made to this vector.
+   *
+   * \param epsilon Tolerance threshold to account for rounding errors
+   */
+  void Normalize(SquaredUnit epsilon = Common::Vector2d<SquaredUnit>::EPSILON) noexcept
+  {
+    if (const SquaredUnit squaredLength{this->Dot(*this)}; squaredLength > epsilon)
+    {
+      const units::dimensionless::scalar_t length{units::math::sqrt(squaredLength).template to<double>()};
+      x /= length;
+      y /= length;
+    }
+  }
+
   /*!
    * returns length of the vector
    *