1 // orbit.cpp
2 //
3 // Copyright (C) 2001, Chris Laurel <claurel@shatters.net>
4 //
5 // This program is free software; you can redistribute it and/or
6 // modify it under the terms of the GNU General Public License
7 // as published by the Free Software Foundation; either version 2
8 // of the License, or (at your option) any later version.
9
10 #include <functional>
11 #include <algorithm>
12 #include <cmath>
13 #include <cassert>
14 #include <celmath/mathlib.h>
15 #include <celmath/solve.h>
16 #include "astro.h"
17 #include "orbit.h"
18 #include "body.h"
19
20 using namespace std;
21
22
23 // Orbital velocity is computed by differentiation for orbits that don't
24 // override velocityAtTime().
25 static const double ORBITAL_VELOCITY_DIFF_DELTA = 1.0 / 1440.0;
26
27
EllipticalOrbit(double _pericenterDistance,double _eccentricity,double _inclination,double _ascendingNode,double _argOfPeriapsis,double _meanAnomalyAtEpoch,double _period,double _epoch)28 EllipticalOrbit::EllipticalOrbit(double _pericenterDistance,
29 double _eccentricity,
30 double _inclination,
31 double _ascendingNode,
32 double _argOfPeriapsis,
33 double _meanAnomalyAtEpoch,
34 double _period,
35 double _epoch) :
36 pericenterDistance(_pericenterDistance),
37 eccentricity(_eccentricity),
38 inclination(_inclination),
39 ascendingNode(_ascendingNode),
40 argOfPeriapsis(_argOfPeriapsis),
41 meanAnomalyAtEpoch(_meanAnomalyAtEpoch),
42 period(_period),
43 epoch(_epoch)
44 {
45 orbitPlaneRotation = (Mat3d::zrotation(_ascendingNode) *
46 Mat3d::xrotation(_inclination) *
47 Mat3d::zrotation(_argOfPeriapsis));
48 }
49
50
51 // Standard iteration for solving Kepler's Equation
52 struct SolveKeplerFunc1 : public unary_function<double, double>
53 {
54 double ecc;
55 double M;
56
SolveKeplerFunc1SolveKeplerFunc157 SolveKeplerFunc1(double _ecc, double _M) : ecc(_ecc), M(_M) {};
58
operator ()SolveKeplerFunc159 double operator()(double x) const
60 {
61 return M + ecc * sin(x);
62 }
63 };
64
65
66 // Faster converging iteration for Kepler's Equation; more efficient
67 // than above for orbits with eccentricities greater than 0.3. This
68 // is from Jean Meeus's _Astronomical Algorithms_ (2nd ed), p. 199
69 struct SolveKeplerFunc2 : public unary_function<double, double>
70 {
71 double ecc;
72 double M;
73
SolveKeplerFunc2SolveKeplerFunc274 SolveKeplerFunc2(double _ecc, double _M) : ecc(_ecc), M(_M) {};
75
operator ()SolveKeplerFunc276 double operator()(double x) const
77 {
78 return x + (M + ecc * sin(x) - x) / (1 - ecc * cos(x));
79 }
80 };
81
82
83 struct SolveKeplerLaguerreConway : public unary_function<double, double>
84 {
85 double ecc;
86 double M;
87
SolveKeplerLaguerreConwaySolveKeplerLaguerreConway88 SolveKeplerLaguerreConway(double _ecc, double _M) : ecc(_ecc), M(_M) {};
89
operator ()SolveKeplerLaguerreConway90 double operator()(double x) const
91 {
92 double s = ecc * sin(x);
93 double c = ecc * cos(x);
94 double f = x - s - M;
95 double f1 = 1 - c;
96 double f2 = s;
97 x += -5 * f / (f1 + sign(f1) * sqrt(abs(16 * f1 * f1 - 20 * f * f2)));
98
99 return x;
100 }
101 };
102
103 struct SolveKeplerLaguerreConwayHyp : public unary_function<double, double>
104 {
105 double ecc;
106 double M;
107
SolveKeplerLaguerreConwayHypSolveKeplerLaguerreConwayHyp108 SolveKeplerLaguerreConwayHyp(double _ecc, double _M) : ecc(_ecc), M(_M) {};
109
operator ()SolveKeplerLaguerreConwayHyp110 double operator()(double x) const
111 {
112 double s = ecc * sinh(x);
113 double c = ecc * cosh(x);
114 double f = s - x - M;
115 double f1 = c - 1;
116 double f2 = s;
117 x += -5 * f / (f1 + sign(f1) * sqrt(abs(16 * f1 * f1 - 20 * f * f2)));
118
119 return x;
120 }
121 };
122
123 typedef pair<double, double> Solution;
124
125
velocityAtTime(double tdb) const126 Vec3d Orbit::velocityAtTime(double tdb) const
127 {
128 Point3d p0 = positionAtTime(tdb);
129 Point3d p1 = positionAtTime(tdb + ORBITAL_VELOCITY_DIFF_DELTA);
130 return (p1 - p0) * (1.0 / ORBITAL_VELOCITY_DIFF_DELTA);
131 }
132
133
eccentricAnomaly(double M) const134 double EllipticalOrbit::eccentricAnomaly(double M) const
135 {
136 if (eccentricity == 0.0)
137 {
138 // Circular orbit
139 return M;
140 }
141 else if (eccentricity < 0.2)
142 {
143 // Low eccentricity, so use the standard iteration technique
144 Solution sol = solve_iteration_fixed(SolveKeplerFunc1(eccentricity, M), M, 5);
145 return sol.first;
146 }
147 else if (eccentricity < 0.9)
148 {
149 // Higher eccentricity elliptical orbit; use a more complex but
150 // much faster converging iteration.
151 Solution sol = solve_iteration_fixed(SolveKeplerFunc2(eccentricity, M), M, 6);
152 // Debugging
153 // printf("ecc: %f, error: %f mas\n",
154 // eccentricity, radToDeg(sol.second) * 3600000);
155 return sol.first;
156 }
157 else if (eccentricity < 1.0)
158 {
159 // Extremely stable Laguerre-Conway method for solving Kepler's
160 // equation. Only use this for high-eccentricity orbits, as it
161 // requires more calcuation.
162 double E = M + 0.85 * eccentricity * sign(sin(M));
163 Solution sol = solve_iteration_fixed(SolveKeplerLaguerreConway(eccentricity, M), E, 8);
164 return sol.first;
165 }
166 else if (eccentricity == 1.0)
167 {
168 // Nearly parabolic orbit; very common for comets
169 // TODO: handle this
170 return M;
171 }
172 else
173 {
174 // Laguerre-Conway method for hyperbolic (ecc > 1) orbits.
175 double E = log(2 * M / eccentricity + 1.85);
176 Solution sol = solve_iteration_fixed(SolveKeplerLaguerreConwayHyp(eccentricity, M), E, 30);
177 return sol.first;
178 }
179 }
180
181
182 // Compute the position at the specified eccentric
183 // anomaly E.
positionAtE(double E) const184 Point3d EllipticalOrbit::positionAtE(double E) const
185 {
186 double x, y;
187
188 if (eccentricity < 1.0)
189 {
190 double a = pericenterDistance / (1.0 - eccentricity);
191 x = a * (cos(E) - eccentricity);
192 y = a * sqrt(1 - square(eccentricity)) * sin(E);
193 }
194 else if (eccentricity > 1.0)
195 {
196 double a = pericenterDistance / (1.0 - eccentricity);
197 x = -a * (eccentricity - cosh(E));
198 y = -a * sqrt(square(eccentricity) - 1) * sinh(E);
199 }
200 else
201 {
202 // TODO: Handle parabolic orbits
203 x = 0.0;
204 y = 0.0;
205 }
206
207 Point3d p = orbitPlaneRotation * Point3d(x, y, 0);
208
209 // Convert to Celestia's internal coordinate system
210 return Point3d(p.x, p.z, -p.y);
211 }
212
213
214 // Compute the velocity at the specified eccentric
215 // anomaly E.
velocityAtE(double E) const216 Vec3d EllipticalOrbit::velocityAtE(double E) const
217 {
218 double x, y;
219
220 if (eccentricity < 1.0)
221 {
222 double a = pericenterDistance / (1.0 - eccentricity);
223 double sinE = sin(E);
224 double cosE = cos(E);
225
226 x = -a * sinE;
227 y = a * sqrt(1 - square(eccentricity)) * cosE;
228
229 double meanMotion = 2.0 * PI / period;
230 double edot = meanMotion / (1 - eccentricity * cosE);
231 x *= edot;
232 y *= edot;
233 }
234 else if (eccentricity > 1.0)
235 {
236 double a = pericenterDistance / (1.0 - eccentricity);
237 x = -a * (eccentricity - cosh(E));
238 y = -a * sqrt(square(eccentricity) - 1) * sinh(E);
239 }
240 else
241 {
242 // TODO: Handle parabolic orbits
243 x = 0.0;
244 y = 0.0;
245 }
246
247 Vec3d v = orbitPlaneRotation * Vec3d(x, y, 0);
248
249 // Convert to Celestia's coordinate system
250 return Vec3d(v.x, v.z, -v.y);
251 }
252
253
254 // Return the offset from the center
positionAtTime(double t) const255 Point3d EllipticalOrbit::positionAtTime(double t) const
256 {
257 t = t - epoch;
258 double meanMotion = 2.0 * PI / period;
259 double meanAnomaly = meanAnomalyAtEpoch + t * meanMotion;
260 double E = eccentricAnomaly(meanAnomaly);
261
262 return positionAtE(E);
263 }
264
265
velocityAtTime(double t) const266 Vec3d EllipticalOrbit::velocityAtTime(double t) const
267 {
268 t = t - epoch;
269 double meanMotion = 2.0 * PI / period;
270 double meanAnomaly = meanAnomalyAtEpoch + t * meanMotion;
271 double E = eccentricAnomaly(meanAnomaly);
272
273 return velocityAtE(E);
274 }
275
276
getPeriod() const277 double EllipticalOrbit::getPeriod() const
278 {
279 return period;
280 }
281
282
getBoundingRadius() const283 double EllipticalOrbit::getBoundingRadius() const
284 {
285 // TODO: watch out for unbounded parabolic and hyperbolic orbits
286 return pericenterDistance * ((1.0 + eccentricity) / (1.0 - eccentricity));
287 }
288
289
sample(double,double t,int nSamples,OrbitSampleProc & proc) const290 void EllipticalOrbit::sample(double, double t, int nSamples,
291 OrbitSampleProc& proc) const
292 {
293 if (eccentricity >= 1.0)
294 {
295 double dE = 1 * PI / (double) nSamples;
296 for (int i = 0; i < nSamples; i++)
297 proc.sample(t, positionAtE(dE * i));
298 }
299 else
300 {
301 // Adaptive sampling of the orbit; more samples in regions of high curvature.
302 // nSamples is the number of samples that will be used for a perfectly circular
303 // orbit. Elliptical orbits will have regions of higher curvature thar require
304 // additional sample points.
305 double E = 0.0;
306 double dE = 2 * PI / (double) nSamples;
307 double w = (1 - square(eccentricity));
308 double M0 = E - eccentricity * sin(E);
309
310 while (E < 2 * PI)
311 {
312 // Compute the time tag for this sample
313 double M = E - eccentricity * sin(E); // Mean anomaly from ecc anomaly
314 double tsamp = t + (M - M0) * period / (2 * PI); // Time from mean anomaly
315
316 proc.sample(tsamp, positionAtE(E));
317
318 // Compute the curvature
319 double k = w * pow(square(sin(E)) + w * w * square(cos(E)), -1.5);
320
321 // Step amount based on curvature--constrain it so that we don't end up
322 // taking too many samples anywhere. Clamping the curvature to 20 effectively
323 // limits the numbers of samples to 3*nSamples
324 E += dE / max(min(k, 20.0), 1.0);
325 }
326 }
327 }
328
329
CachingOrbit()330 CachingOrbit::CachingOrbit() :
331 lastTime(-1.0e30),
332 positionCacheValid(false),
333 velocityCacheValid(false)
334 {
335 }
336
337
~CachingOrbit()338 CachingOrbit::~CachingOrbit()
339 {
340 }
341
342
positionAtTime(double jd) const343 Point3d CachingOrbit::positionAtTime(double jd) const
344 {
345 if (jd != lastTime)
346 {
347 lastTime = jd;
348 lastPosition = computePosition(jd);
349 positionCacheValid = true;
350 velocityCacheValid = false;
351 }
352 else if (!positionCacheValid)
353 {
354 lastPosition = computePosition(jd);
355 positionCacheValid = true;
356 }
357
358 return lastPosition;
359 }
360
361
velocityAtTime(double jd) const362 Vec3d CachingOrbit::velocityAtTime(double jd) const
363 {
364 if (jd != lastTime)
365 {
366 lastVelocity = computeVelocity(jd);
367 lastTime = jd; // must be set *after* call to computeVelocity
368 positionCacheValid = false;
369 velocityCacheValid = true;
370 }
371 else if (!velocityCacheValid)
372 {
373 lastVelocity = computeVelocity(jd);
374 velocityCacheValid = true;
375 }
376
377 return lastVelocity;
378 }
379
380
381 /*! Calculate the velocity at the specified time (units are
382 * kilometers / Julian day.) The default implementation just
383 * differentiates the position.
384 */
computeVelocity(double jd) const385 Vec3d CachingOrbit::computeVelocity(double jd) const
386 {
387 // Compute the velocity by differentiating.
388 Point3d p0 = positionAtTime(jd);
389
390 // Call computePosition() instead of positionAtTime() so that we
391 // don't affect the cached value.
392 // TODO: check the valid ranges of the orbit to make sure that
393 // jd+dt is still in range.
394 Point3d p1 = computePosition(jd + ORBITAL_VELOCITY_DIFF_DELTA);
395
396 return (p1 - p0) * (1.0 / ORBITAL_VELOCITY_DIFF_DELTA);
397 }
398
399
sample(double start,double t,int nSamples,OrbitSampleProc & proc) const400 void CachingOrbit::sample(double start, double t, int nSamples,
401 OrbitSampleProc& proc) const
402 {
403 double dt = t / (double) nSamples;
404 for (int i = 0; i < nSamples; i++)
405 proc.sample(start + dt * i, positionAtTime(start + dt * i));
406 }
407
408
StateVectorToOrbit(const Point3d & position,const Vec3d & v,double mass,double t)409 static EllipticalOrbit* StateVectorToOrbit(const Point3d& position,
410 const Vec3d& v,
411 double mass,
412 double t)
413 {
414 Vec3d R = position - Point3d(0.0, 0.0, 0.0);
415 Vec3d L = R ^ v;
416 double magR = R.length();
417 double magL = L.length();
418 double magV = v.length();
419 L *= (1.0 / magL);
420
421 Vec3d W = L ^ (R / magR);
422
423 double G = astro::G * 1e-9; // convert from meters to kilometers
424 double GM = G * mass;
425
426 // Compute the semimajor axis
427 double a = 1.0 / (2.0 / magR - square(magV) / GM);
428
429 // Compute the eccentricity
430 double p = square(magL) / GM;
431 double q = R * v;
432 double ex = 1.0 - magR / a;
433 double ey = q / sqrt(a * GM);
434 double e = sqrt(ex * ex + ey * ey);
435
436 // Compute the mean anomaly
437 double E = atan2(ey, ex);
438 double M = E - e * sin(E);
439
440 // Compute the inclination
441 double cosi = L * Vec3d(0, 1.0, 0);
442 double i = 0.0;
443 if (cosi < 1.0)
444 i = acos(cosi);
445
446 // Compute the longitude of ascending node
447 double Om = atan2(L.x, L.z);
448
449 // Compute the argument of pericenter
450 Vec3d U = R / magR;
451 double s_nu = (v * U) * sqrt(p / GM);
452 double c_nu = (v * W) * sqrt(p / GM) - 1;
453 s_nu /= e;
454 c_nu /= e;
455 Vec3d P = U * c_nu - W * s_nu;
456 Vec3d Q = U * s_nu + W * c_nu;
457 double om = atan2(P.y, Q.y);
458
459 // Compute the period
460 double T = 2 * PI * sqrt(cube(a) / GM);
461 T = T / 86400.0; // Convert from seconds to days
462
463 return new EllipticalOrbit(a * (1 - e), e, i, Om, om, M, T, t);
464 }
465
466
MixedOrbit(Orbit * orbit,double t0,double t1,double mass)467 MixedOrbit::MixedOrbit(Orbit* orbit, double t0, double t1, double mass) :
468 primary(orbit),
469 afterApprox(NULL),
470 beforeApprox(NULL),
471 begin(t0),
472 end(t1),
473 boundingRadius(0.0)
474 {
475 assert(t1 > t0);
476 assert(orbit != NULL);
477
478 double dt = 1.0 / 1440.0; // 1 minute
479 Point3d p0 = orbit->positionAtTime(t0);
480 Point3d p1 = orbit->positionAtTime(t1);
481 Vec3d v0 = (orbit->positionAtTime(t0 + dt) - p0) / (86400 * dt);
482 Vec3d v1 = (orbit->positionAtTime(t1 + dt) - p1) / (86400 * dt);
483 beforeApprox = StateVectorToOrbit(p0, v0, mass, t0);
484 afterApprox = StateVectorToOrbit(p1, v1, mass, t1);
485
486 boundingRadius = beforeApprox->getBoundingRadius();
487 if (primary->getBoundingRadius() > boundingRadius)
488 boundingRadius = primary->getBoundingRadius();
489 if (afterApprox->getBoundingRadius() > boundingRadius)
490 boundingRadius = afterApprox->getBoundingRadius();
491 }
492
~MixedOrbit()493 MixedOrbit::~MixedOrbit()
494 {
495 if (primary != NULL)
496 delete primary;
497 if (beforeApprox != NULL)
498 delete beforeApprox;
499 if (afterApprox != NULL)
500 delete afterApprox;
501 }
502
503
positionAtTime(double jd) const504 Point3d MixedOrbit::positionAtTime(double jd) const
505 {
506 if (jd < begin)
507 return beforeApprox->positionAtTime(jd);
508 else if (jd < end)
509 return primary->positionAtTime(jd);
510 else
511 return afterApprox->positionAtTime(jd);
512 }
513
514
velocityAtTime(double jd) const515 Vec3d MixedOrbit::velocityAtTime(double jd) const
516 {
517 if (jd < begin)
518 return beforeApprox->velocityAtTime(jd);
519 else if (jd < end)
520 return primary->velocityAtTime(jd);
521 else
522 return afterApprox->velocityAtTime(jd);
523 }
524
525
getPeriod() const526 double MixedOrbit::getPeriod() const
527 {
528 return primary->getPeriod();
529 }
530
531
getBoundingRadius() const532 double MixedOrbit::getBoundingRadius() const
533 {
534 return boundingRadius;
535 }
536
537
sample(double t0,double t1,int nSamples,OrbitSampleProc & proc) const538 void MixedOrbit::sample(double t0, double t1, int nSamples,
539 OrbitSampleProc& proc) const
540 {
541 Orbit* o;
542 if (t0 < begin)
543 o = beforeApprox;
544 else if (t0 < end)
545 o = primary;
546 else
547 o = afterApprox;
548 o->sample(t0, t1, nSamples, proc);
549 }
550
551
552 /*** FixedOrbit ***/
553
FixedOrbit(const Point3d & pos)554 FixedOrbit::FixedOrbit(const Point3d& pos) :
555 position(pos)
556 {
557 }
558
559
~FixedOrbit()560 FixedOrbit::~FixedOrbit()
561 {
562 }
563
564
565 Point3d
positionAtTime(double) const566 FixedOrbit::positionAtTime(double /*tjd*/) const
567 {
568 return position;
569 }
570
571
572 bool
isPeriodic() const573 FixedOrbit::isPeriodic() const
574 {
575 return false;
576 }
577
578
579 double
getPeriod() const580 FixedOrbit::getPeriod() const
581 {
582 return 1.0;
583 }
584
585
586 double
getBoundingRadius() const587 FixedOrbit::getBoundingRadius() const
588 {
589 return position.distanceFromOrigin() * 1.1;
590 }
591
592
593 void
sample(double,double,int,OrbitSampleProc &) const594 FixedOrbit::sample(double, double, int, OrbitSampleProc&) const
595 {
596 /*
597 for (int i = 0; i < nSamples; i++)
598 proc.sample(t, position);
599 */
600 }
601
602
603 /*** SynchronousOrbit ***/
604 // TODO: eliminate this class once body-fixed reference frames are implemented
SynchronousOrbit(const Body & _body,const Point3d & _position)605 SynchronousOrbit::SynchronousOrbit(const Body& _body,
606 const Point3d& _position) :
607 body(_body),
608 position(_position)
609 {
610 }
611
612
~SynchronousOrbit()613 SynchronousOrbit::~SynchronousOrbit()
614 {
615 }
616
617
positionAtTime(double jd) const618 Point3d SynchronousOrbit::positionAtTime(double jd) const
619 {
620 Quatd q = body.getEquatorialToBodyFixed(jd);
621 return position * q.toMatrix3();
622 }
623
624
getPeriod() const625 double SynchronousOrbit::getPeriod() const
626 {
627 return body.getRotationModel(0.0)->getPeriod();
628 }
629
630
getBoundingRadius() const631 double SynchronousOrbit::getBoundingRadius() const
632 {
633 return position.distanceFromOrigin();
634 }
635
636
sample(double,double,int,OrbitSampleProc &) const637 void SynchronousOrbit::sample(double, double, int, OrbitSampleProc&) const
638 {
639 // Empty method--we never want to show a synchronous orbit.
640 }
641
642
643