-
Notifications
You must be signed in to change notification settings - Fork 0
/
satellite.hpp
210 lines (164 loc) · 6.25 KB
/
satellite.hpp
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
/** common.hpp
satellite_t struct and orbital mechanics math
Orbital algorithms adapted from:
https://github.com/RazerM/orbital/blob/master/orbital/utilities.py
*/
#include <math.h>
#include <stdio.h>
#ifndef SATELLITE_HPP
#define SATELLITE_HPP
#define degToRad(angleInDegrees) ((angleInDegrees) * M_PI / 180.0)
#define radToDeg(angleInRadians) ((angleInRadians) * 180.0 / M_PI)
#define radToDegPos(angleInRadians) fmod((((angleInRadians) * 180.0 / M_PI) + 360), 360)
#define EARTH_G 398600.4418
#define EARTH_RADIUS 6371 // Km
#define EARTH_RADIUS_SQUARED 40589641 // Km
#define MAX_ITERATIONS 100
/** */
typedef struct {
double a; // semimajor axis
double e; // eccentricity
double i; // Inclination
double rAscen; // Ω
double argPeri; // Argument of perigee ω
double trueAnomaly; // [+/- rad] : Angle from perigee to the spacecraft’s position
// this does not change with time, based only on semi-major axis (a)
double calc_mean_motion() {
double a_cubed = this->a * this->a * this->a; // rad/sec
return sqrt(EARTH_G/a_cubed);
}
/** Returns mean anomaly (from the true anomoly) */
double true_to_mean_anoml() {
double f = trueAnomaly;
// true to Ecc
double e_squared = e * e;
double E = atan2( sqrt(1 - e_squared) * sin(f), e + cos(f) );
E = fmod(E, 2 * M_PI);
// Ecc to mean
return E - e * sin(E);
}
/** Returns true anomaly (from mean anomaly) */
double mean_to_true_anoml(double M) {
// mean to Ecc
double E = mean_to_eccentric_anomaly(M);
// Ecc to true
return 2 * atan2( sqrt(1 + e) * sin(E / 2), sqrt(1 - e) * cos(E / 2) );
}
/** Returns eccentric anomaly (from mean anamoly), solving Kepler equation. */
double mean_to_eccentric_anomaly(double M, double tolerance=1e-14) {
double Mnorm, E, E0, dE, count;
double t1, t2, t3, t4, t5, t6;
double e_cubed = e * e * e;
Mnorm = fmod(M, 2 * M_PI);
E0 = M + (-1 / 2 * e_cubed + e + ( (e * e) + 3 / 2 * cos(M) * e_cubed) * cos(M)) * sin(M);
dE = tolerance + 1;
count = 0;
while (dE > tolerance) {
t1 = cos(E0);
t2 = -1 + e * t1;
t3 = sin(E0);
t4 = e * t3;
t5 = -E0 + t4 + Mnorm;
t6 = t5 / (1 / 2 * t5 * t4 / t2 + t2);
E = E0 - t5 / ((1 / 2 * t3 - 1 / 6 * t1 * t6) * e * t6 + t2);
dE = abs(E - E0);
E0 = E;
count += 1;
if (count == MAX_ITERATIONS) {
printf("Did not converge after %d iterations. (e=%f, M=%f)\n", MAX_ITERATIONS, e, M);
return -1; // TODO make this better???
}
}
return E;
}
/** Returns x,y,z as Earth Centered Inertial (ECI) coordinates
ECI: x,y,z relative to center of non-roating Earth*/
void getECI_XYZ(double &ret_x, double &ret_y, double &ret_z) {
double * f = &this->trueAnomaly;
double radius_at_f = (this->a * (1 - this->e * this->e)) / (1 + this->e * cos(*f));
// double vel = sqrt(EARTH_G * (2/radius_at_f - 1/this->a));
// http://control.asu.edu/Classes/MAE462/462Lecture07.pdf
double r_x_peri = radius_at_f * cos(*f);
double r_y_peri = radius_at_f * sin(*f);
double r_z_peri = 0;
double R[3][3]; // TODO, maybe ballance this with memory and memory access time
double sinsinomeg = sin(this->rAscen) * sin(this->argPeri); // sin(Ω) * sin(ω)
double coscosomeg = cos(this->rAscen) * cos(this->argPeri); // cos(Ω) * cos(ω)
double sincosomeg = sin(this->rAscen) * cos(this->argPeri); // sin(Ω) * cos(ω)
double cossinomeg = cos(this->rAscen) * sin(this->argPeri); // cos(Ω) * sin(ω)
double sin_i = sin(this->i);
double cos_i = cos(this->i);
R[0][0] = coscosomeg - sinsinomeg * cos_i;
R[0][1] = -cossinomeg - sincosomeg * cos_i;
R[0][2] = sin(this->rAscen) * sin_i;
R[1][0] = sincosomeg + cossinomeg * cos_i;
R[1][1] = -sinsinomeg + coscosomeg * cos_i;
R[1][2] = -cos(this->rAscen) * sin_i;
R[2][0] = sin(this->argPeri) * sin_i;
R[2][1] = cos(this->argPeri) * sin_i;
R[2][2] = cos_i;
ret_x = r_x_peri * R[0][0] + r_y_peri * R[0][1] + r_z_peri * R[0][2];
ret_y = r_x_peri * R[1][0] + r_y_peri * R[1][1] + r_z_peri * R[1][2];
ret_z = r_x_peri * R[2][0] + r_y_peri * R[2][1] + r_z_peri * R[2][2];
}
} satellite_t;
/** */
void init_satellites(satellite_t *sats, int n) {
for (int i = 0; i<n; i++) {
sats[i].trueAnomaly = degToRad(5); // aka: `f`
sats[i].a = 25600; //Km
sats[i].e = 0.0;
sats[i].i = 0;
sats[i].rAscen = 0;
sats[i].argPeri = 0;
}
}
/** May want this, leaving as blank for now. */
void random_init_satellites(satellite_t *sats, int n) {
for (int i = 0; i<n; i++) {
// sats[i].trueAnomaly = 0; // aka: `f`
//
// sats[i].a = 25600; //Km
// sats[i].e = 0.6;
// sats[i].i = 0; // < 2pi rad
// sats[i].rAscen = 0; // < 2pi rad
// sats[i].argOfPerigee = 0; // < 2pi rad
}
}
/** Returns true if the line (given by 2, 3D points) intersects a sphere with radius.
False if line never intersects or touches.
Must square the radius before calling (for better usage of immediates and macros).
*/
bool lineIntersectsSphere(double &x1, double &y1, double &z1, double &x2, double &y2, double &z2, double radius_squared) {
double dx = x2 - x1;
double dy = y2 - y1;
double dz = z2 - z1;
// quadratic components
double a = dx*dx + dy*dy + dz*dz;
double b = 2 * (dx*x1 + dy*y1 + dz*z1);
double c = (x1*x1 + y1*y1 + z1*z1) - radius_squared;
double determinant = (b*b) - 4 * a * c;
return determinant >= 0; // intersects if determinant >= 0
}
/**
Given two satilites, determines (boolean) if they have line of site of each other.
Method: check if the line created by each of their ECI 3D points intersects the Earth sphere.
TODO shrink sphere to model atmosphere bouncing
*/
bool satellitesHaveLineOfSight(satellite_t *one, satellite_t *two) {
double x1_eci, y1_eci, z1_eci;
double x2_eci, y2_eci, z2_eci;
one->getECI_XYZ(x1_eci, y1_eci, z1_eci);
two->getECI_XYZ(x2_eci, y2_eci, z2_eci);
return !lineIntersectsSphere(x1_eci, y1_eci, z1_eci, x2_eci, y2_eci, z2_eci, EARTH_RADIUS_SQUARED);
}
bool satellitesHaveLineOfSight(double &x1_eci, double &y1_eci, double &z1_eci, satellite_t *other, double &retDistance) {
double x2_eci, y2_eci, z2_eci;
other->getECI_XYZ(x2_eci, y2_eci, z2_eci);
double dx = x2_eci - x1_eci;
double dy = y2_eci - y1_eci;
double dz = z2_eci - z1_eci;
retDistance = sqrt(dx*dx + dy*dy + dz*dz);
return !lineIntersectsSphere(x1_eci, y1_eci, z1_eci, x2_eci, y2_eci, z2_eci, EARTH_RADIUS_SQUARED);
}
#endif