A whole bunch of possibly-wrong changes to Orbit.
[Orbitizer.git] / Orbit_old.py
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
#!/usr/bin/python3
 
from Body import Body
from Vector3D import *
from math import acos, atan2, cos, pi, sin
 
class Orbit():
        def __init__(self, PosVector = None, VelVector = None, Primary = None):
                '''
                @param PosVector:
                @type Vector3D:
                @param VelVector:
                @type Vector3D:
                @param Primary:
                @type Body:
                '''
                self.PosVector = PosVector
                self.VelVector = VelVector
                self.Primary = Primary
        
        def VisVivaEnergy(self):
                return self.VelVector.Magnitude() ** 2. / 2 - self.Primary.GravParm() / self.PosVector.Magnitude()
        
        def SemiMajorAxis(self):
                return -self.Primary.GravParm() / (2. * self.VisVivaEnergy())
        
        def SpecAngMmntm(self):
                return self.PosVector * self.VelVector
        
        def EccentricityVector(self):
                return self.VelVector * self.SpecAngMmntm() / self.Primary.GravParm() - self.PosVector.UnitVector()
        
        def Eccentricity(self):
                return self.EccentricityVector().Magnitude()
        
        def Inclination(self):
                h = self.SpecAngMmntm()
                hZ = h._k
                return acos(hZ / h.Magnitude())
        
        def AscNodeVector(self):
                return kVector * self.SpecAngMmntm()
        
        def LongitudeAscNode(self):
                n = self.AscNodeVector()
                
                try:
                        Omega = acos(n._i / n.Magnitude())
                except ZeroDivisionError:
                        return None
                if n._j >= 0:
                        return Omega
                else:
                        return 2 * pi - Omega
        
        def ArgPeriapsis(self):
                e = self.EccentricityVector()
                try:
                        n = self.AscNodeVector().UnitVector()
                except ZeroDivisionError:
                        arg = atan2(e._j / e.Magnitude(), e._i / e.Magnitude())
                        if self.SpecAngMmntm()._k < 0:
                                arg = 2. * pi - arg
                        return arg
                
                arg = acos(n.Dot(e) / (n.Magnitude() * e.Magnitude()))
                
                if e._k >= 0:
                        return arg
                else:
                        return 2 * pi - arg
        
        def TrueAnomaly(self):
                e = self.EccentricityVector()
                r = self.PosVector
                f = acos(e.Dot(r) / (e.Magnitude() * r.Magnitude()))
                if r.Dot(self.VelVector) >= 0:
                        return f
                else:
                        return 2 * pi - f
        
        def EccentricAnomaly(self):
                e = self.Eccentricity()
                f = self.TrueAnomaly()
                E = acos((e + cos(f)) / (1 + e * cos(f)))
                if f <= pi:
                        return E
                else:
                        return 2 * pi - E
        
        def MeanAnomaly(self):
                E = self.EccentricAnomaly()
                e = self.Eccentricity()
                M = E - e * sin(E)
                return M
        
        def PeriapsisVector(self):
                dummy = Orbit(self.PosVector, self.VelVector, self.Primary)
                dummy.UpdateTrueAnomaly(0)
                
                return dummy.PosVector
        def Periapsis(self):
                return self.PeriapsisVector().Magnitude()
        
        def ApoapsisVector(self):
                dummy = Orbit(self.PosVector, self.VelVector, self.Primary)
                dummy.UpdateTrueAnomaly(pi)
                
                return dummy.PosVector
        def Apoapsis(self):
                return self.ApoapsisVector().Magnitude()
        
        def UpdateFromElements(self, a, e, i, Omega, w, f):
                p = (a * (1 - e ** 2.) / (1 + e * cos(f)))
                Rx = p * (cos(Omega) * cos(w + f) - sin(Omega) * cos(i) * sin(w + f))
                Ry = p * (sin(Omega) * cos(w + f) + cos(Omega) * cos(i) * sin(w + f))
                Rz = p * sin(i) * sin(w + f)
                PosVector = Vector3D(Rx, Ry, Rz)
                
                Vx = -(self.Primary.GravParm() / p) ** .5 * (cos(Omega) * (sin(w + f) + e * sin(w)) + sin(Omega) * cos(i) * (cos(w + f) + e * cos(w)))
                Vy = -(self.Primary.GravParm() / p) ** .5 * (sin(Omega) * (sin(w + f) + e * sin(w)) - cos(Omega) * cos(i) * (cos(w + f) + e * cos(w)))
                Vz = -(self.Primary.GravParm() / p) ** .5 * (sin(i) * (cos(w + f) + e * cos(w)))
                VelVector = Vector3D(Vx, Vy, Vz)
                
                self.PosVector = PosVector
                self.VelVector = VelVector
        
        def UpdateFromShapeAndPos(self, a, e, i, Omega, w, PosVector):
                self.UpdateFromElements(a, e, i, Omega, w, 0)
                self.PosVector = PosVector
                
#               f = self.TrueAnomaly()
#               self.UpdateFromElements(a, e, i, Omega, w, f)
#               
#               if (self.PosVector - PosVector).Magnitude() > .001 * self.SemiMajorAxis():
#                       s  = "Specified PosVector is not on specified orbit!\n"
#                       s += "Specified PosVector: {0:.2}\n".format(PosVector)
#                       s += "Calculated PosVector: {0:.2}\n".format(self.PosVector)
#                       raise ValueError(s)
        
        def UpdateTrueAnomaly(self, f):
                return self.UpdateFromElements(self.SemiMajorAxis(), self.Eccentricity(), self.Inclination(), self.LongitudeAscNode(), self.ArgPeriapsis(), f)
        
        def __str__(self):
                s  = "Orbiting {0}\n".format(self.Primary.Name)
                s += "a = {0} [m]\n".format(self.SemiMajorAxis())
                s += "e = {0}\n".format(self.Eccentricity())
                s += "i = {0} [radians]\n".format(self.Inclination())
                s += "Ω = {0} [radians]\n".format(self.LongitudeAscNode())
                s += "ω = {0} [radians]\n".format(self.ArgPeriapsis())
                s += "υ = {0} [radians]\n".format(self.TrueAnomaly())
                s += "\n"
                s += "Current Position: {0} (radius: {1})\n".format(self.PosVector, self.PosVector.Magnitude())
                s += "Current Velocity: {0} (magnitude: {1})\n".format(self.VelVector, self.VelVector.Magnitude())
                
                return s
 
if __name__ == "__main__":
        Kerbin = Body("Kerbin", Mass = 5.2915793e22, Radius = 600e3)
        Orbit1 = Orbit(Primary = Kerbin)
        Orbit1.UpdateFromElements(4.35e6, 0.28, pi / 6., 0, 3. * pi / 2., pi / 2.)
        print(Orbit1)
        
        Orbit2 = Orbit(Primary = Kerbin)
        Orbit2.UpdateFromShapeAndPos(Orbit1.PosVector.Magnitude(), 0., 0, 0, 0, Orbit1.PosVector)
        print(Orbit2)
        
        deltaV = Orbit2.VelVector - Orbit1.VelVector
        print("delta-V to transfer: {0} (magnitude: {1})".format(deltaV, deltaV.Magnitude()))
        
        dVPRN = deltaV.ConvertToFrame(Orbit1.VelVector, Orbit1.PosVector, Orbit1.VelVector * Orbit1.PosVector)
        print("delta-V in Prograde, Radial, Normal: {0} (magnitude {1})".format(dVPRN, dVPRN.Magnitude()))