forked from NathanKell/MechJebKMGimbalExt
-
Notifications
You must be signed in to change notification settings - Fork 4
/
MechJebKMGimbal3Ext.cs
72 lines (60 loc) · 2.7 KB
/
MechJebKMGimbal3Ext.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using MuMech;
using UnityEngine;
using km_Gimbal;
using System.Reflection;
namespace MuMech
{
public class MechJebKMGimbal3Ext : ComputerModule
{
public MechJebKMGimbal3Ext(MechJebCore core) : base(core) { }
bool isKMLoaded = false;
private static bool KM_GimbalIsValid(PartModule p)
{
KM_Gimbal_3 gimbal = p as KM_Gimbal_3;
return gimbal.initRots.Any();
}
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;
}
public override void OnStart(PartModule.StartState state)
{
isKMLoaded = AssemblyLoader.loadedAssemblies.Any(a => a.assembly.GetName().Name == "km_Gimbal");
print("MechJebKMGimbal3Ext adding MJ2 callback");
if(isKMLoaded && !VesselState.SupportsGimbalExtension<KM_Gimbal_3>())
{
VesselState.AddGimbalExtension<KM_Gimbal_3>(new VesselState.GimbalExt() { isValid = KM_GimbalIsValid, initialRot = KM_GimbalInitialRot, torqueVector = KM_GimbalTorqueVector });
}
}
}
}