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; }