From 051a33c5056a9a1c72a8463e39e20a7ee23d2ab6 Mon Sep 17 00:00:00 2001 From: Daniel Santos-Olivan Date: Fri, 23 Apr 2021 12:53:25 +0200 Subject: [PATCH] IMU: Fix some weird cases when the imu gives nan values --- .../Source/Carla/Sensor/InertialMeasurementUnit.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp index 1521737c2..dbf5256a5 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp @@ -167,13 +167,16 @@ float AInertialMeasurementUnit::ComputeCompass() { // Magnetometer: orientation with respect to the North in rad const FVector ForwVect = GetActorForwardVector().GetSafeNormal2D(); - float Compass = std::acos(FVector::DotProduct(CarlaNorthVector, ForwVect)); + const float DotProd = FVector::DotProduct(CarlaNorthVector, ForwVect) + // We check if the dot product is higher than 1.0 due to numerical error + if (DotProd >= 1.00f) + return 0.0f; + + const float Compass = std::acos(DotProd); // Keep the angle between [0, 2pi) if (FVector::CrossProduct(CarlaNorthVector, ForwVect).Z < 0.0f) - { - Compass = carla::geom::Math::Pi2() - Compass; - } + return carla::geom::Math::Pi2() - Compass; return Compass; }