From 7972255593747fb22c9b870573fd6de5ccd251d1 Mon Sep 17 00:00:00 2001
From: ffgao <50094080+ffgao@users.noreply.github.com>
Date: Wed, 8 Feb 2023 04:02:41 +0000
Subject: [PATCH] Fix missing direction of ray (#3297)

---
 gazebo/physics/RayShape.cc | 9 +++++----
 gazebo/physics/RayShape.hh | 3 +++
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/gazebo/physics/RayShape.cc b/gazebo/physics/RayShape.cc
index a83ab67b58..d9b922ed9f 100644
--- 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
index bc3ef0e5f1..09a185ba60 100644
--- 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;