Protect against more NaNs

Signed-off-by: Louise Poubel <[email protected]>
gazebosim · Jun 23, 2022 · f310470 · f310470
1 parent 120f3a3
commit f310470
Showing 1 changed file with 21 additions and 2 deletions.
diff --git a/ogre2/src/Ogre2RayQuery.cc b/ogre2/src/Ogre2RayQuery.cc
@@ -79,8 +79,27 @@ void Ogre2RayQuery::SetFromCamera(const CameraPtr &_camera,
   Ogre::Ray ray =
       camera->ogreCamera->getCameraToViewportRay(screenPos.X(), screenPos.Y());
 
-  this->origin = Ogre2Conversions::Convert(ray.getOrigin());
-  this->direction = Ogre2Conversions::Convert(ray.getDirection());
+  auto originMath = Ogre2Conversions::Convert(ray.getOrigin());
+  if (originMath.IsFinite())
+  {
+    this->origin = originMath;
+  }
+  else
+  {
+    ignwarn << "Attempted to set non-finite origin from camera ["
+            << camera->Name() << "]" << std::endl;
+  }
+
+  auto directionMath = Ogre2Conversions::Convert(ray.getDirection());
+  if (directionMath.IsFinite())
+  {
+    this->direction = directionMath;
+  }
+  else
+  {
+    ignwarn << "Attempted to set non-finite direction from camera ["
+            << camera->Name() << "]" << std::endl;
+  }
 
   this->dataPtr->camera = camera;