From 0fb58f80aa4f91d851325e85138305bfcb325e57 Mon Sep 17 00:00:00 2001
From: siimav <siim.aaver@gmail.com>
Date: Thu, 15 Feb 2024 01:11:45 +0200
Subject: [PATCH] Fix a couple of NaNs and nullrefs in VesselModuleRotationRO

---
 Source/VesselModuleRotationRO.cs | 46 +++++++++++++++++++++++++-------
 1 file changed, 36 insertions(+), 10 deletions(-)

diff --git a/Source/VesselModuleRotationRO.cs b/Source/VesselModuleRotationRO.cs
index 9a5d315246..d84a151101 100644
--- a/Source/VesselModuleRotationRO.cs
+++ b/Source/VesselModuleRotationRO.cs
@@ -1,6 +1,7 @@
 using System;
 using UnityEngine;
 using System.Collections.Generic;
+using System.Runtime.CompilerServices;
 
 
 // Modified version of the VesselModule of the same name in MandatoryRCS by gotmachine
@@ -132,6 +133,11 @@ private void SetRot()
 
         public void SetPosRot(Quaternion rotation, Vector3 position)
         {
+            if (rotation.x == float.NaN)
+            {
+                Debug.LogError("[RealismOverhaul] VesselModuleRotationRO is setting rotation to NaN!");
+            }
+
             if (!vessel.loaded)
             {
                 vessel.vesselTransform.SetPositionAndRotation(position, rotation);
@@ -538,13 +544,13 @@ private Vector3 AutopilotTargetDirection()
             if (autopilotMode == 1 || autopilotMode == 2)
             {
                 if (autopilotContext == 0) // Orbit prograde
-                    target = vessel.obt_velocity;
+                    target = GetVesselObtVelocity();
                 else if (autopilotContext == 1) // Surface prograde
-                    target = vessel.srf_velocity;
+                    target = GetVesselSrfVelocity();
                 else if (autopilotContext == 2) // Target prograde
                 {
                     if (_lastTarget != null)
-                        target = -(_lastTarget.GetObtVelocity() - vessel.obt_velocity);
+                        target = -(_lastTarget.GetObtVelocity() - GetVesselObtVelocity());
                     else
                         return vessel.transform.TransformDirection(referenceUpLocal);
                 }
@@ -559,14 +565,14 @@ private Vector3 AutopilotTargetDirection()
             else if (autopilotMode == 3 || autopilotMode == 4 || autopilotMode == 5 || autopilotMode == 6)
             {
                 // Get body up vector
-                Vector3 planetUp = (vessel.rootPart.transform.position - vessel.mainBody.position).normalized;
+                Vector3 planetUp = (vessel.transform.position - vessel.mainBody.position).normalized;
 
                 // Get normal vector
                 Vector3 normal = new Vector3();
                 if (autopilotContext == 0) // Orbit
-                    normal = Vector3.Cross(vessel.obt_velocity, planetUp).normalized;
+                    normal = Vector3.Cross(GetVesselObtVelocity(), planetUp).normalized;
                 else if (autopilotContext == 1 || autopilotContext == 2) // Surface/Target (seems to be the same for normal/radial)
-                    normal = Vector3.Cross(vessel.srf_velocity, planetUp).normalized;
+                    normal = Vector3.Cross(GetVesselSrfVelocity(), planetUp).normalized;
 
                 // Return normal/antinormal or calculate radial
                 if (autopilotMode == 3) // Normal
@@ -578,9 +584,9 @@ private Vector3 AutopilotTargetDirection()
                     // Get RadialIn vector
                     Vector3 radial = new Vector3();
                     if (autopilotContext == 0) // Orbit
-                        radial = Vector3.Cross(vessel.obt_velocity, normal).normalized;
+                        radial = Vector3.Cross(GetVesselObtVelocity(), normal).normalized;
                     else if (autopilotContext == 1 || autopilotContext == 2) // Surface/Target (seems to be the same for normal/radial)
-                        radial = Vector3.Cross(vessel.srf_velocity, normal).normalized;
+                        radial = Vector3.Cross(GetVesselSrfVelocity(), normal).normalized;
 
                     // Return radial vector
                     if (autopilotMode == 5) // Radial In
@@ -635,8 +641,15 @@ private Vector3 AutopilotTargetDirection()
             return target.normalized;
         }
 
-        // Copypasted from PersistentRotation main.cs
-        private Quaternion FromToRotation(Vector3d fromv, Vector3d tov) //Stock FromToRotation() doesn't work correctly
+        /// <summary>
+        /// Returns NaN when either input vector is (0,0,0)
+        /// Copypasted from PersistentRotation main.cs
+        /// Stock FromToRotation() doesn't work correctly
+        /// </summary>
+        /// <param name="fromv"></param>
+        /// <param name="tov"></param>
+        /// <returns></returns>
+        private Quaternion FromToRotation(Vector3d fromv, Vector3d tov)
         {
             Vector3d cross = Vector3d.Cross(fromv, tov);
             double dot = Vector3d.Dot(fromv, tov);
@@ -645,6 +658,19 @@ private Quaternion FromToRotation(Vector3d fromv, Vector3d tov) //Stock FromToRo
             return new QuaternionD(cross.x * norm, cross.y * norm, cross.z * norm, wval * norm);
         }
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private Vector3d GetVesselObtVelocity()
+        {
+            return vessel.obt_velocity.IsZero() ? vessel.orbit.GetRelativeVel() : vessel.obt_velocity;
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private Vector3d GetVesselSrfVelocity()
+        {
+            return vessel.srf_velocity.IsZero() ? vessel.orbit.GetVel() - vessel.mainBody.getRFrmVelOrbit(vessel.orbit) :
+                                                  vessel.srf_velocity;
+        }
+
         public static VesselModuleRotationRO GetModule(Vessel v)
         {
             int c = v.vesselModules.Count;