private static Vector3d KM_GimbalTorqueVector(PartModule p, int i, Vector3d CoM) { KM_Gimbal_3 gimbal = p as KM_Gimbal_3; Vector3d torque = Vector3d.zero; if (!gimbal.enableGimbal) { return(Vector3d.zero); } float maxRollGimbalRange = Math.Max(gimbal.pitchGimbalRange, gimbal.yawGimbalRange); Vector3d position = gimbal.gimbalTransforms[i].position - CoM; double distance = position.magnitude; double radius = Vector3.ProjectOnPlane(position, Vector3.Project(position, p.vessel.ReferenceTransform.up)).magnitude; torque.x = Math.Sin(Math.Abs(gimbal.pitchGimbalRange) * Math.PI / 180d) * distance; torque.z = Math.Sin(Math.Abs(gimbal.yawGimbalRange) * Math.PI / 180d) * distance; if (gimbal.enableRoll) { torque.y = Math.Sin(Math.Abs(maxRollGimbalRange) * Math.PI / 180d) * radius; } return(torque); }
private static Quaternion KM_GimbalInitialRot(PartModule p, Transform engineTransform, int i) { KM_Gimbal_3 gimbal = p as KM_Gimbal_3; // Save the current local rot Quaternion save = gimbal.gimbalTransforms[i].localRotation; // Apply the default rot and let unity compute the world rot gimbal.gimbalTransforms[i].localRotation = gimbal.initRots[i]; Quaternion initalRot = gimbal.gimbalTransforms[i].rotation; // Restore the current local rot gimbal.gimbalTransforms[i].localRotation = save; return(initalRot); }
private static bool KM_GimbalIsValid(PartModule p) { KM_Gimbal_3 gimbal = p as KM_Gimbal_3; return(gimbal.initRots.Any()); }