Skip to content
Snippets Groups Projects
Commit b24934ca authored by Noah Schick's avatar Noah Schick Committed by René Paris
Browse files

fix(CollisionPostCrash): Fix unnormed vector in post-crash collision detection

Correct documentation of Vector2d<T>::Norm & Add Vector2d<T>::Normalize
parent 134368b1
No related branches found
No related tags found
3 merge requests!266Merge develop into main for v1.2,!263Fix unnormed vector in post-crash collision detection,!261Resolve "Unit types not being enforced in Common::Vector2d"
Pipeline #61507 running
......@@ -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
*
......
......@@ -98,13 +98,9 @@ std::vector<Common::Vector2d<units::length::meter_t>> CollisionDetectionPostCras
auto indices12 = (i1 + 1) % corners1.size(); // 1st polygon, 2nd corner
// calculate normalized normal on polygon edge and distance for Hesse normal form
Common::Vector2d<units::length::meter_t> normal1;
normal1.x = corners1[indices12].y - corners1[indices11].y;
normal1.y = -(corners1[indices12].x - corners1[indices11].x);
auto tmpNormalizedVector = normal1.Norm();
normal1.x = units::length::meter_t(tmpNormalizedVector.x.value());
normal1.y = units::length::meter_t(tmpNormalizedVector.y.value());
Common::Vector2d<units::length::meter_t> normal1{corners1[indices12].y - corners1[indices11].y,
-(corners1[indices12].x - corners1[indices11].x)};
normal1.Normalize();
auto distance1 = normal1.Dot(corners1[indices11]);
......@@ -114,10 +110,9 @@ std::vector<Common::Vector2d<units::length::meter_t>> CollisionDetectionPostCras
auto indices22 = (i2 + 1) % corners2.size(); // 2nd polygon, 2nd corner
// calculate normalized normal on polygon edge and distance for Hesse normal form
Common::Vector2d<units::length::meter_t> normal2;
normal2.x = corners2[indices22].y - corners2[indices21].y;
normal2.y = -(corners2[indices22].x - corners2[indices21].x);
normal2.Norm();
Common::Vector2d<units::length::meter_t> normal2{corners2[indices22].y - corners2[indices21].y,
-(corners2[indices22].x - corners2[indices21].x)};
normal2.Normalize();
auto distance2 = normal2.Dot(corners2[indices21]);
// check if edges are parallel
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment