Fix missing direction of ray (gazebosim#3297)

ffgao · Feb 8, 2023 · 7972255 · 7972255
1 parent 08f01aa
commit 7972255
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 4 deletions.
diff --git a/gazebo/physics/RayShape.cc b/gazebo/physics/RayShape.cc
@@ -74,6 +74,10 @@ void RayShape::SetPoints(const ignition::math::Vector3d &_posStart,
   this->relativeStartPos = _posStart;
   this->relativeEndPos = _posEnd;
 
+  // Compute the relative direction of the ray
+  this->relativeDir  = this->relativeEndPos - this->relativeStartPos;
+  this->relativeDir.Normalize();
+
   if (this->collisionParent)
   {
     this->globalStartPos =
@@ -114,10 +118,7 @@ void RayShape::SetLength(double _len)
 {
   this->contactLen = _len;
 
-  ignition::math::Vector3d dir = this->relativeEndPos - this->relativeStartPos;
-  dir.Normalize();
-
-  this->relativeEndPos = dir * _len + this->relativeStartPos;
+  this->relativeEndPos = this->relativeDir * _len + this->relativeStartPos;
 }
 
 //////////////////////////////////////////////////

diff --git a/gazebo/physics/RayShape.hh b/gazebo/physics/RayShape.hh
@@ -157,6 +157,9 @@ namespace gazebo
 
       /// \brief End position of the ray in global cs
       protected: ignition::math::Vector3d globalEndPos;
+
+      /// \brief Direction of the ray, relative to the body
+      protected: ignition::math::Vector3d relativeDir;
 
       /// \brief Name of the object this ray collided with
       private: std::string collisionName;