fixed raystepping

Author: Alexander Seeck

Assets/MixedRealityToolkit/Definitions/Physics/RayStep.cs

@@ -137,31 +137,15 @@ public static Vector3 GetPointByDistance(RayStep[] steps, float distance)
             Debug.Assert(steps != null);
             Debug.Assert(steps.Length > 0);
 
-            Vector3 point = Vector3.zero;
-            float remainingDistance = distance;
-            int numSteps = steps.Length;
-
-            for (int i = 0; i < numSteps; i++)
+            var (rayStep, remainingDistance) = GetStepByDistance(steps, distance);
+            if (remainingDistance > 0)
             {
-                if (remainingDistance > numSteps)
-                {
-                    remainingDistance -= numSteps;
-                }
-                else
-                {
-                    point = Vector3.Lerp(steps[i].Origin, steps[i].Terminus, remainingDistance / steps[i].Length);
-                    remainingDistance = 0;
-                    break;
-                }
+                return Vector3.Lerp(rayStep.Origin, rayStep.Terminus, remainingDistance / rayStep.Length);
             }
-
-            if (remainingDistance > 0)
+            else
             {
-                // If we reach the end and still have distance left, set the point to the terminus of the last step
-                point = steps[numSteps - 1].Terminus;
+                return rayStep.Terminus;
             }
-
-            return point;
         }
 
         /// <summary>
@@ -170,36 +154,31 @@ public static Vector3 GetPointByDistance(RayStep[] steps, float distance)
         /// <param name="steps"></param>
         /// <param name="distance"></param>
         /// <returns></returns>
-        public static RayStep GetStepByDistance(RayStep[] steps, float distance)
+        public static (RayStep rayStep, float traveledDistance) GetStepByDistance(RayStep[] steps, float distance)
         {
             Debug.Assert(steps != null);
             Debug.Assert(steps.Length > 0);
 
-            RayStep step = new RayStep();
             float remainingDistance = distance;
+
             int numSteps = steps.Length;
+            float stepLength = 0;
 
             for (int i = 0; i < numSteps; i++)
             {
-                if (remainingDistance > steps[i].Length)
+                stepLength = steps[i].Length;
+
+                if (remainingDistance > stepLength)
                 {
-                    remainingDistance -= steps[i].Length;
+                    remainingDistance -= stepLength;
                 }
                 else
                 {
-                    step = steps[i];
-                    remainingDistance = 0;
-                    break;
+                    return (steps[i], remainingDistance);
                 }
             }
 
-            if (remainingDistance > 0)
-            {
-                // If we reach the end and still have distance left, return the last step
-                step = steps[steps.Length - 1];
-            }
-
-            return step;
+            return (steps[steps.Length - 1], remainingDistance);
         }
 
         /// <summary>
@@ -213,7 +192,7 @@ public static Vector3 GetDirectionByDistance(RayStep[] steps, float distance)
             Debug.Assert(steps != null);
             Debug.Assert(steps.Length > 0);
 
-            return GetStepByDistance(steps, distance).Direction;
+            return GetStepByDistance(steps, distance).rayStep.Direction;
         }
 
         #endregion