-
Notifications
You must be signed in to change notification settings - Fork 1
/
VesselState.cs
258 lines (228 loc) · 14.4 KB
/
VesselState.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
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
using System;
using System.Collections.Generic;
using System.Text;
using UnityEngine;
using MuMech;
namespace MuMech
{
public class VesselState
{
public double time; //planetarium time
public double deltaT; //TimeWarp.fixedDeltaTime
public Vector3d CoM;
public Vector3d MoI;
public Vector3d up;
public Vector3d north;
public Vector3d east;
public Vector3d forward; //the direction the vessel is pointing
public Quaternion rotationSurface;
public Quaternion rotationVesselSurface;
public Vector3d velocityMainBodySurface;
public Vector3d velocityVesselSurface;
public Vector3d velocityVesselSurfaceUnit;
public Vector3d velocityVesselOrbit;
public Vector3d velocityVesselOrbitUnit;
public Vector3d angularVelocity;
public Vector3d angularMomentum;
public Vector3d upNormalToVelSurface; //unit vector in the plane of up and velocityVesselSurface and perpendicular to velocityVesselSurface
public Vector3d upNormalToVelOrbit; //unit vector in the plane of up and velocityVesselOrbit and perpendicular to velocityVesselOrbit
public Vector3d leftSurface; //unit vector perpendicular to up and velocityVesselSurface
public Vector3d leftOrbit; //unit vector perpendicular to up and velocityVesselOrbit
public Vector3d gravityForce;
public double localg; //magnitude of gravityForce
public MovingAverage speedOrbital = new MovingAverage();
public MovingAverage speedSurface = new MovingAverage();
public MovingAverage speedVertical = new MovingAverage();
public MovingAverage speedHorizontal = new MovingAverage();
public MovingAverage vesselHeading = new MovingAverage();
public MovingAverage vesselPitch = new MovingAverage();
public MovingAverage vesselRoll = new MovingAverage();
public MovingAverage altitudeASL = new MovingAverage();
public MovingAverage altitudeTrue = new MovingAverage();
public double altitudeBottom = 0;
public MovingAverage orbitApA = new MovingAverage();
public MovingAverage orbitPeA = new MovingAverage();
public MovingAverage orbitPeriod = new MovingAverage();
public MovingAverage orbitTimeToAp = new MovingAverage();
public MovingAverage orbitTimeToPe = new MovingAverage();
public MovingAverage orbitLAN = new MovingAverage();
public MovingAverage orbitArgumentOfPeriapsis = new MovingAverage();
public MovingAverage orbitInclination = new MovingAverage();
public MovingAverage orbitEccentricity = new MovingAverage();
public MovingAverage orbitSemiMajorAxis = new MovingAverage();
public MovingAverage latitude = new MovingAverage();
public MovingAverage longitude = new MovingAverage();
public double radius; //distance from planet center
public double mass;
public double thrustAvailable;
public double thrustMinimum;
public double maxThrustAccel; //thrustAvailable / mass
public double minThrustAccel; //some engines (particularly SRBs) have a minimum thrust so this may be nonzero
public double torqueRAvailable;
public double torquePYAvailable;
public double torqueThrustPYAvailable;
public double massDrag;
public double atmosphericDensity;
public double angleToPrograde;
public void Update(Vessel vessel)
{
time = Planetarium.GetUniversalTime();
deltaT = TimeWarp.fixedDeltaTime;
CoM = vessel.findWorldCenterOfMass();
up = (CoM - vessel.mainBody.position).normalized;
north = Vector3d.Exclude(up, (vessel.mainBody.position + vessel.mainBody.transform.up * (float)vessel.mainBody.Radius) - CoM).normalized;
east = vessel.mainBody.getRFrmVel(CoM).normalized;
forward = vessel.transform.up;
rotationSurface = Quaternion.LookRotation(north, up);
rotationVesselSurface = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.transform.rotation) * rotationSurface);
velocityVesselOrbit = vessel.orbit.GetVel();
velocityVesselOrbitUnit = velocityVesselOrbit.normalized;
velocityVesselSurface = velocityVesselOrbit - vessel.mainBody.getRFrmVel(CoM);
velocityVesselSurfaceUnit = velocityVesselSurface.normalized;
velocityMainBodySurface = rotationSurface * velocityVesselSurface;
angularVelocity = Quaternion.Inverse(vessel.transform.rotation) * vessel.rigidbody.angularVelocity;
upNormalToVelSurface = Vector3d.Exclude(velocityVesselSurfaceUnit, up).normalized;
upNormalToVelOrbit = Vector3d.Exclude(velocityVesselOrbit, up).normalized;
leftSurface = -Vector3d.Cross(upNormalToVelSurface, velocityVesselSurfaceUnit);
leftOrbit = -Vector3d.Cross(upNormalToVelOrbit, velocityVesselOrbitUnit); ;
gravityForce = FlightGlobals.getGeeForceAtPosition(CoM);
localg = gravityForce.magnitude;
speedOrbital.value = velocityVesselOrbit.magnitude;
speedSurface.value = velocityVesselSurface.magnitude;
speedVertical.value = Vector3d.Dot(velocityVesselSurface, up);
speedHorizontal.value = (velocityVesselSurface - (speedVertical * up)).magnitude;
vesselHeading.value = rotationVesselSurface.eulerAngles.y;
vesselPitch.value = (rotationVesselSurface.eulerAngles.x > 180) ? (360.0 - rotationVesselSurface.eulerAngles.x) : -rotationVesselSurface.eulerAngles.x;
vesselRoll.value = (rotationVesselSurface.eulerAngles.z > 180) ? (rotationVesselSurface.eulerAngles.z - 360.0) : rotationVesselSurface.eulerAngles.z;
altitudeASL.value = vessel.mainBody.GetAltitude(CoM);
RaycastHit sfc;
if (Physics.Raycast(CoM, -up, out sfc, (float)altitudeASL + 10000.0F, 1 << 15))
{
altitudeTrue.value = sfc.distance;
}
else if (vessel.mainBody.pqsController != null)
{
// from here: http://kerbalspaceprogram.com/forum/index.php?topic=10324.msg161923#msg161923
altitudeTrue.value = vessel.mainBody.GetAltitude(CoM) - (vessel.mainBody.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(vessel.mainBody.GetLongitude(CoM), Vector3d.down) * QuaternionD.AngleAxis(vessel.mainBody.GetLatitude(CoM), Vector3d.forward) * Vector3d.right) - vessel.mainBody.pqsController.radius);
}
else
{
altitudeTrue.value = vessel.mainBody.GetAltitude(CoM);
}
double surfaceAltitudeASL = altitudeASL - altitudeTrue;
altitudeBottom = altitudeTrue;
foreach (Part p in vessel.parts)
{
if (p.collider != null)
{
Vector3d bottomPoint = p.collider.ClosestPointOnBounds(vessel.mainBody.position);
double partBottomAlt = vessel.mainBody.GetAltitude(bottomPoint) - surfaceAltitudeASL;
altitudeBottom = Math.Max(0, Math.Min(altitudeBottom, partBottomAlt));
}
}
atmosphericDensity = FlightGlobals.getAtmDensity(FlightGlobals.getStaticPressure(altitudeASL, vessel.mainBody));
orbitApA.value = vessel.orbit.ApA;
orbitPeA.value = vessel.orbit.PeA;
orbitPeriod.value = vessel.orbit.period;
orbitTimeToAp.value = vessel.orbit.timeToAp;
if (vessel.orbit.eccentricity < 1) orbitTimeToPe.value = vessel.orbit.timeToPe;
else orbitTimeToPe.value = -vessel.orbit.meanAnomaly / (2 * Math.PI / vessel.orbit.period); //orbit.timeToPe is bugged for ecc > 1 and timewarp > 2x
orbitLAN.value = vessel.orbit.LAN;
orbitArgumentOfPeriapsis.value = vessel.orbit.argumentOfPeriapsis;
orbitInclination.value = vessel.orbit.inclination;
orbitEccentricity.value = vessel.orbit.eccentricity;
orbitSemiMajorAxis.value = vessel.orbit.semiMajorAxis;
latitude.value = vessel.mainBody.GetLatitude(CoM);
longitude.value = ARUtils.clampDegrees(vessel.mainBody.GetLongitude(CoM));
if (vessel.mainBody != Planetarium.fetch.Sun)
{
Vector3d delta = vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + 1) - vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() - 1);
Vector3d plUp = Vector3d.Cross(vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime()) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime()), vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4)).normalized;
angleToPrograde = ARUtils.clampDegrees360((((vessel.orbit.inclination > 90) || (vessel.orbit.inclination < -90)) ? 1 : -1) * ((Vector3)up).AngleInPlane(plUp, delta));
}
else
{
angleToPrograde = 0;
}
radius = (CoM - vessel.mainBody.position).magnitude;
mass = thrustAvailable = thrustMinimum = massDrag = torqueRAvailable = torquePYAvailable = torqueThrustPYAvailable = 0;
MoI = vessel.findLocalMOI(CoM);
foreach (Part p in vessel.parts)
{
if (p.physicalSignificance != Part.PhysicalSignificance.NONE)
{
mass += p.mass;
massDrag += p.mass * p.maximum_drag;
}
MoI += p.Rigidbody.inertiaTensor;
if (((p.State == PartStates.ACTIVE) || ((Staging.CurrentStage > Staging.lastStage) && (p.inverseStage == Staging.lastStage))) && ((p is LiquidEngine) || (p is LiquidFuelEngine) || (p is SolidRocket) || (p is AtmosphericEngine)))
{
if (p is LiquidEngine && ARUtils.engineHasFuel(p))
{
double usableFraction = Vector3d.Dot((p.transform.rotation * ((LiquidEngine)p).thrustVector).normalized, forward);
thrustAvailable += ((LiquidEngine)p).maxThrust * usableFraction;
thrustMinimum += ((LiquidEngine)p).minThrust * usableFraction;
if (((LiquidEngine)p).thrustVectoringCapable)
{
torqueThrustPYAvailable += Math.Sin(Math.Abs(((LiquidEngine)p).gimbalRange) * Math.PI / 180) * ((LiquidEngine)p).maxThrust * (p.Rigidbody.worldCenterOfMass - CoM).magnitude;
}
}
else if (p is LiquidFuelEngine && ARUtils.engineHasFuel(p))
{
double usableFraction = Vector3d.Dot((p.transform.rotation * ((LiquidFuelEngine)p).thrustVector).normalized, forward);
thrustAvailable += ((LiquidFuelEngine)p).maxThrust * usableFraction;
thrustMinimum += ((LiquidFuelEngine)p).minThrust * usableFraction;
if (((LiquidFuelEngine)p).thrustVectoringCapable)
{
torqueThrustPYAvailable += Math.Sin(Math.Abs(((LiquidFuelEngine)p).gimbalRange) * Math.PI / 180) * ((LiquidFuelEngine)p).maxThrust * (p.Rigidbody.worldCenterOfMass - CoM).magnitude;
}
}
else if (p is SolidRocket && !p.ActivatesEvenIfDisconnected)
{
double usableFraction = Vector3d.Dot((p.transform.rotation * ((SolidRocket)p).thrustVector).normalized, forward);
thrustAvailable += ((SolidRocket)p).thrust * usableFraction;
thrustMinimum += ((SolidRocket)p).thrust * usableFraction;
}
else if (p is AtmosphericEngine && ARUtils.engineHasFuel(p))
{
double usableFraction = Vector3d.Dot((p.transform.rotation * ((AtmosphericEngine)p).thrustVector).normalized, forward);
thrustAvailable += ((AtmosphericEngine)p).maximumEnginePower * ((AtmosphericEngine)p).totalEfficiency * usableFraction;
if (((AtmosphericEngine)p).thrustVectoringCapable)
{
torqueThrustPYAvailable += Math.Sin(Math.Abs(((AtmosphericEngine)p).gimbalRange) * Math.PI / 180) * ((AtmosphericEngine)p).maximumEnginePower * ((AtmosphericEngine)p).totalEfficiency * (p.Rigidbody.worldCenterOfMass - CoM).magnitude;
}
}
}
if ((!FlightInputHandler.RCSLock) && (p is RCSModule))
{
double maxT = 0;
for (int i = 0; i < 6; i++)
{
if (((RCSModule)p).thrustVectors[i] != Vector3.zero)
{
maxT = Math.Max(maxT, ((RCSModule)p).thrusterPowers[i]);
}
}
// torqueRAvailable += maxT;
torquePYAvailable += maxT * (p.Rigidbody.worldCenterOfMass - CoM).magnitude;
}
if (p is CommandPod)
{
torqueRAvailable += Math.Abs(((CommandPod)p).rotPower);
torquePYAvailable += Math.Abs(((CommandPod)p).rotPower);
}
}
angularMomentum = new Vector3d(angularVelocity.x * MoI.x, angularVelocity.y * MoI.y, angularVelocity.z * MoI.z);
maxThrustAccel = thrustAvailable / mass;
minThrustAccel = thrustMinimum / mass;
}
public double TerminalVelocity()
{
return Math.Sqrt((2 * gravityForce.magnitude * mass) / (0.009785 * Math.Exp(-altitudeASL / 5000.0) * massDrag));
}
public double thrustAccel(double throttle)
{
return (1.0 - throttle) * minThrustAccel + throttle * maxThrustAccel;
}
}
}