diff --git a/include/ignition/rendering/base/BaseNode.hh b/include/ignition/rendering/base/BaseNode.hh index 549023d25..22e623025 100644 --- a/include/ignition/rendering/base/BaseNode.hh +++ b/include/ignition/rendering/base/BaseNode.hh @@ -336,16 +336,16 @@ namespace ignition template void BaseNode::SetLocalPose(const math::Pose3d &_pose) { - math::Pose3d pose = _pose; - pose.Pos() = pose.Pos() - pose.Rot() * this->origin; - - if (!pose.IsFinite()) + if (!_pose.IsFinite()) { - ignerr << "Unable to set pose of a node: " - << "non-finite (nan, inf) values detected." << std::endl; + ignerr << "Unable to set non-finite pose [" << _pose + << "] to node [" << this->Name() << "]" << std::endl; return; } + math::Pose3d pose = _pose; + pose.Pos() = pose.Pos() - pose.Rot() * this->origin; + if (!initialLocalPoseSet) { this->initialLocalPose = pose; @@ -380,6 +380,13 @@ namespace ignition template void BaseNode::SetLocalPosition(const math::Vector3d &_position) { + if (!_position.IsFinite()) + { + ignerr << "Unable to set non-finite position [" << _position + << "] to node [" << this->Name() << "]" << std::endl; + return; + } + math::Pose3d pose = this->LocalPose(); pose.Pos() = _position; this->SetLocalPose(pose); @@ -411,6 +418,13 @@ namespace ignition template void BaseNode::SetLocalRotation(const math::Quaterniond &_rotation) { + if (!_rotation.IsFinite()) + { + ignerr << "Unable to set non-finite rotation [" << _rotation + << "] to node [" << this->Name() << "]" << std::endl; + return; + } + math::Pose3d pose = this->LocalPose(); pose.Rot() = _rotation; this->SetLocalPose(pose); @@ -525,6 +539,12 @@ namespace ignition template void BaseNode::SetOrigin(const math::Vector3d &_origin) { + if (!_origin.IsFinite()) + { + ignerr << "Unable to set non-finite origin [" << _origin + << "] to node [" << this->Name() << "]" << std::endl; + return; + } this->origin = _origin; } diff --git a/ogre2/src/Ogre2RayQuery.cc b/ogre2/src/Ogre2RayQuery.cc index f46c82ede..552c787fc 100644 --- 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; diff --git a/src/OrbitViewController.cc b/src/OrbitViewController.cc index fb14c0d6c..a752ba3cc 100644 --- a/src/OrbitViewController.cc +++ b/src/OrbitViewController.cc @@ -15,6 +15,8 @@ * */ +#include + #include #include "ignition/rendering/OrbitViewController.hh" @@ -92,6 +94,13 @@ const math::Vector3d &OrbitViewController::Target() const ////////////////////////////////////////////////// void OrbitViewController::Zoom(const double _value) { + if (!std::isfinite(_value)) + { + ignerr << "Failed to zoom by non-finite value [" << _value << "]" + << std::endl; + return; + } + if (!this->dataPtr->camera) { ignerr << "Camera is NULL" << std::endl; @@ -113,12 +122,26 @@ void OrbitViewController::Zoom(const double _value) ////////////////////////////////////////////////// void OrbitViewController::Pan(const math::Vector2d &_value) { + if (!_value.IsFinite()) + { + ignerr << "Failed to pan by non-finite value [" << _value << "]" + << std::endl; + return; + } + if (!this->dataPtr->camera) { ignerr << "Camera is NULL" << std::endl; return; } + if (!this->dataPtr->camera->WorldPosition().IsFinite()) + { + ignerr << "Camera world position isn't finite [" + << this->dataPtr->camera->WorldPosition() << "]" << std::endl; + return; + } + double viewportWidth = this->dataPtr->camera->ImageWidth(); double viewportHeight = this->dataPtr->camera->ImageHeight(); @@ -148,6 +171,13 @@ void OrbitViewController::Pan(const math::Vector2d &_value) ////////////////////////////////////////////////// void OrbitViewController::Orbit(const math::Vector2d &_value) { + if (!_value.IsFinite()) + { + ignerr << "Failed to orbit by non-finite value [" << _value << "]" + << std::endl; + return; + } + if (!this->dataPtr->camera) { ignerr << "Camera is NULL" << std::endl;