diff --git a/src/orbital/utilities.py b/src/orbital/utilities.py index d6a9469..a9a8b48 100644 --- a/src/orbital/utilities.py +++ b/src/orbital/utilities.py @@ -271,8 +271,9 @@ def elements_from_state_vector(r, v, mu): # Inclination is the angle between the angular # momentum vector and its z component. i = acos(h.z / norm(h)) + i_small = abs(i) < SMALL_NUMBER or abs(i - pi) < SMALL_NUMBER - if abs(i) < SMALL_NUMBER: + if i_small: # For non-inclined orbits, raan is undefined; # set to zero by convention raan = 0 @@ -284,40 +285,49 @@ def elements_from_state_vector(r, v, mu): # Argument of periapsis is the angle between # eccentricity vector and its x component. arg_pe = acos(ev.x / norm(ev)) + if ev.y < 0: + arg_pe = -arg_pe + # If plane is upside down, arg_pe needs to be inverted. + if abs(i) > pi * 0.5: + arg_pe = -arg_pe + arg_pe = mod(arg_pe, 2 * pi) else: # Right ascension of ascending node is the angle # between the node vector and its x component. raan = acos(n.x / norm(n)) if n.y < 0: - raan = 2 * pi - raan + raan = mod(-raan, 2 * pi) - # Argument of periapsis is angle between - # node and eccentricity vectors. - arg_pe = acos(dot(n, ev) / (norm(n) * norm(ev))) + if abs(e) < SMALL_NUMBER: + # For circular orbits, place periapsis + # at ascending node by convention + arg_pe = 0 + else: + # Argument of periapsis is angle between + # node and eccentricity vectors. + arg_pe = vector_angle(n, ev) + if ev.z < 0: + arg_pe = mod(-arg_pe, 2 * pi) if abs(e) < SMALL_NUMBER: - if abs(i) < SMALL_NUMBER: + if i_small: # True anomaly is angle between position # vector and its x component. f = acos(r.x / norm(r)) if v.x > 0: - f = 2 * pi - f + f = mod(-f, 2 * pi) else: # True anomaly is angle between node # vector and position vector. - f = acos(dot(n, r) / (norm(n) * norm(r))) + f = vector_angle(n, r) if dot(n, v) > 0: - f = 2 * pi - f + f = mod(-f, 2 * pi) else: - if ev.z < 0: - arg_pe = 2 * pi - arg_pe - # True anomaly is angle between eccentricity # vector and position vector. - f = acos(dot(ev, r) / (norm(ev) * norm(r))) - + f = vector_angle(ev, r) if dot(r, v) < 0: - f = 2 * pi - f + f = mod(-f, 2 * pi) return OrbitalElements(a=a, e=e, i=i, raan=raan, arg_pe=arg_pe, f=f) @@ -373,6 +383,17 @@ def divmod(x, y): return (floor(x / y), mod(x, y)) +def vector_angle(v1, v2): + """Return the unsigned minimum angle between two XyzVectors [rad]. + + The result will always be between 0 and pi inclusive. + """ + # The parameter to acos is clamped to the range [-1.0, 1.0]. If this is not + # done, rounding errors can make the value slightly below -1.0 or above 1.0, + # resulting in nan. These should simply result in pi and 0.0, respectively. + return acos(np.clip(dot(v1, v2) / (norm(v1) * norm(v2)), -1.0, 1.0)) + + # Objects for package diff --git a/tests/test_orbital.py b/tests/test_orbital.py index 083d6ab..182e805 100644 --- a/tests/test_orbital.py +++ b/tests/test_orbital.py @@ -672,26 +672,19 @@ def test_from_state_vector_circular_retrograde(self): R = Position(RADIUS, 0, 0) V = Velocity(0, -sqrt(earth.mu / RADIUS), 0) - # XXX This erroneously generates warnings and raises an assertion, due - # to internal values being NaN. - with warnings.catch_warnings(): - warnings.simplefilter("ignore", category=RuntimeWarning) - self.assertRaises( - AssertionError, KeplerianElements.from_state_vector, R, V, body=earth - ) - # The expected values, after this bug is fixed: - # numpy.testing.assert_almost_equal(orbit.r, R) - # numpy.testing.assert_almost_equal(orbit.v, V) - # self.assertAlmostEqual(orbit.a, RADIUS) - # self.assertAlmostEqual(orbit.e, 0.0) - # self.assertAlmostEqual(orbit.i, radians(180)) - # self.assertAlmostEqual(orbit.raan, 0.0) - # self.assertAlmostEqual(orbit.arg_pe, 0.0) - # self.assertAlmostEqual(orbit.M0, 0.0) - - # self.assertAlmostEqual(orbit.ref_epoch, J2000) - # self.assertEqual(orbit.body, earth) - # self.assertAlmostEqual(orbit.t, 0.0) + orbit = KeplerianElements.from_state_vector(R, V, body=earth) + numpy.testing.assert_almost_equal(orbit.r, R) + numpy.testing.assert_almost_equal(orbit.v, V) + self.assertAlmostEqual(orbit.a, RADIUS) + self.assertAlmostEqual(orbit.e, 0.0) + self.assertAlmostEqual(orbit.i, radians(180)) + self.assertAlmostEqual(orbit.raan, 0.0) + self.assertAlmostEqual(orbit.arg_pe, 0.0) + self.assertAlmostEqual(orbit.M0, 0.0) + + self.assertAlmostEqual(orbit.ref_epoch, J2000) + self.assertEqual(orbit.body, earth) + self.assertAlmostEqual(orbit.t, 0.0) def test_from_state_vector_inclined(self): # Inclined circular orbit, 1/4 of the way around. @@ -700,20 +693,14 @@ def test_from_state_vector_inclined(self): R = Position(-RADIUS * 0.5 * sqrt(2), 0, RADIUS * 0.5 * sqrt(2)) V = Velocity(0, -sqrt(earth.mu / RADIUS), 0) - with warnings.catch_warnings(): - # XXX This has a warning for dividing by zero. - warnings.simplefilter("ignore", category=RuntimeWarning) - orbit = KeplerianElements.from_state_vector(R, V, body=earth) - # XXX: r, v and arg_pe are nan due to a bug in from_state_vector. - # This happens for a perfect circle, but doesn't happen in - # test_from_state_vector_circular for some reason. - # numpy.testing.assert_almost_equal(orbit.r, R) - # numpy.testing.assert_almost_equal(orbit.v, V) + orbit = KeplerianElements.from_state_vector(R, V, body=earth) + numpy.testing.assert_almost_equal(orbit.r, R) + numpy.testing.assert_almost_equal(orbit.v, V) self.assertAlmostEqual(orbit.a, RADIUS) self.assertAlmostEqual(orbit.e, 0.0) self.assertAlmostEqual(orbit.i, radians(45)) self.assertAlmostEqual(orbit.raan, radians(90)) - # self.assertAlmostEqual(orbit.arg_pe, 0.0) + self.assertAlmostEqual(orbit.arg_pe, 0.0) self.assertAlmostEqual(orbit.M0, radians(90)) self.assertAlmostEqual(orbit.ref_epoch, J2000) @@ -761,21 +748,43 @@ def test_from_state_vector_elliptical_arg_pe_gt_180(self): V = Velocity(16703.9010129, 0, 0) orbit = KeplerianElements.from_state_vector(R, V, body=earth) - # XXX These do not match (they are 180° out, due to arg_pe). - # numpy.testing.assert_almost_equal(orbit.r, R) - # numpy.testing.assert_almost_equal(orbit.v, V) + numpy.testing.assert_almost_equal(orbit.r, R) + numpy.testing.assert_almost_equal(orbit.v, V) self.assertAlmostEqual(orbit.a, 10000000.0, places=3) self.assertAlmostEqual(orbit.e, 0.75) self.assertAlmostEqual(orbit.i, 0.0) self.assertAlmostEqual(orbit.raan, 0.0) - # XXX This is incorrectly calculated as 90°. - # self.assertAlmostEqual(orbit.arg_pe, radians(270.0)) + self.assertAlmostEqual(orbit.arg_pe, radians(270.0)) self.assertAlmostEqual(orbit.M0, 0.0) self.assertAlmostEqual(orbit.ref_epoch, J2000) self.assertEqual(orbit.body, earth) self.assertAlmostEqual(orbit.t, 0.0) + def test_from_state_vector_elliptical_almost_flat(self): + # Elliptical orbit, at periapsis, arg_pe = 90°. + # Almost zero inclination, but Z is very slightly negative. + # Regression test for https://github.com/RazerM/orbital/issues/44 + # The issue is that the inclination is so small, it is considered to be + # a non-inclined case, but the eccentricity vector's Z coordinate is + # very slightly negative, which would flip arg_pe erroneously. + R = Position(0, 2500000, -0.01) + V = Velocity(-16703.901013, 0, 0) + + orbit = KeplerianElements.from_state_vector(R, V, body=earth) + numpy.testing.assert_almost_equal(orbit.r, R, decimal=2) + numpy.testing.assert_almost_equal(orbit.v, V, decimal=2) + self.assertAlmostEqual(orbit.a, 10000000.0, places=2) + self.assertAlmostEqual(orbit.e, 0.75) + self.assertAlmostEqual(orbit.i, 0.0) + # The absolute value of raan and arg_pe doesn't matter, because i is + # small. All that matters is that they are 90° apart. + # The current implementation always sets raan = 0 when i is small. + self.assertAlmostEqual(orbit.raan, 0.0) + self.assertAlmostEqual(orbit.arg_pe, radians(90.0)) + self.assertAlmostEqual(orbit.M0, 0.0) + self.assertAlmostEqual(orbit.t, 0.0) + def test_from_state_vector_f_at_periapsis(self): # Elliptical orbit, inclined, at periapsis. # Regression test for https://github.com/RazerM/orbital/issues/40. @@ -784,24 +793,19 @@ def test_from_state_vector_f_at_periapsis(self): R = Position(0, -1767766.952966369, -1767766.952966369) V = Velocity(16703.901013, 0, 0) - # XXX Currently crashes due to the above bug. - with warnings.catch_warnings(): - warnings.simplefilter("ignore", category=RuntimeWarning) - self.assertRaises( - AssertionError, KeplerianElements.from_state_vector, R, V, body=earth - ) - # numpy.testing.assert_almost_equal(orbit.r, R) - # numpy.testing.assert_almost_equal(orbit.v, V) - # self.assertAlmostEqual(orbit.a, 10000000.0, places=2) - # self.assertAlmostEqual(orbit.e, 0.75) - # self.assertAlmostEqual(orbit.i, radians(45)) - # self.assertAlmostEqual(orbit.raan, 0.0) - # self.assertAlmostEqual(orbit.arg_pe, radians(270.0)) - # self.assertAlmostEqual(orbit.M0, 0.0) - - # self.assertAlmostEqual(orbit.ref_epoch, J2000) - # self.assertEqual(orbit.body, earth) - # self.assertAlmostEqual(orbit.t, 0.0) + orbit = KeplerianElements.from_state_vector(R, V, body=earth) + numpy.testing.assert_almost_equal(orbit.r, R) + numpy.testing.assert_almost_equal(orbit.v, V) + self.assertAlmostEqual(orbit.a, 10000000.0, places=2) + self.assertAlmostEqual(orbit.e, 0.75) + self.assertAlmostEqual(orbit.i, radians(45)) + self.assertAlmostEqual(orbit.raan, 0.0) + self.assertAlmostEqual(orbit.arg_pe, radians(270.0)) + self.assertAlmostEqual(orbit.M0, 0.0) + + self.assertAlmostEqual(orbit.ref_epoch, J2000) + self.assertEqual(orbit.body, earth) + self.assertAlmostEqual(orbit.t, 0.0) def test_from_state_vector_iss(self): # ISS (Zarya) from 2008-09-20 12:25:40 @@ -832,23 +836,45 @@ def test_from_state_vector_roundtrips(self): # Circular flat, f > 180°. (Position(0, -10000000, 0), Velocity(6313.4811435530555, 0, 0)), # Circular flat, retrograde. - # (Position(10000000, 0, 0), Velocity(0, -6313.4811435530555, 0)), + (Position(10000000, 0, 0), Velocity(0, -6313.4811435530555, 0)), # Circular inclined, prograde. - # (Position(10000000, 0, 0), Velocity(0, 4464.305329499764, 4464.305329499764)), + ( + Position(10000000, 0, 0), + Velocity(0, 4464.305329499764, 4464.305329499764), + ), # Circular inclined, prograde (raan 90°, M0 90°). - # (Position(-7071067.811865476, 0, 7071067.811865476), Velocity(0, -6313.4811435530555, 0)), + ( + Position(-7071067.811865476, 0, 7071067.811865476), + Velocity(0, -6313.4811435530555, 0), + ), # Circular inclined, raan > 180°. - # (Position(0, -10000000, 0), Velocity(4464.305329499764, 0, 4464.305329499764)), + ( + Position(0, -10000000, 0), + Velocity(4464.305329499764, 0, 4464.305329499764), + ), # Circular inclined, f > 180°. - # (Position(0, -7071067.811865476, -7071067.811865476), Velocity(6313.4811435530555, 0, 0)), + ( + Position(0, -7071067.811865476, -7071067.811865476), + Velocity(6313.4811435530555, 0, 0), + ), # Circular polar. - # (Position(10000000, 0, 0), Velocity(0, 0, 6313.4811435530555)), + (Position(10000000, 0, 0), Velocity(0, 0, 6313.4811435530555)), # Elliptical flat (e=0.75), prograde. (Position(2500000, 0, 0), Velocity(0, 16703.901013, 0)), + # Elliptical flat (e=0.75), prograde, arg_pe = 90°, at periapsis. + (Position(0, 2500000, 0), Velocity(-16703.901013, 0, 0)), + # Elliptical almost-flat (e=0.75, i ~= 0 but Z slightly positive). + (Position(0, 2500000, 0.000000001), Velocity(-16703.901013, 0, 0)), + # Elliptical almost-flat (e=0.75, i ~= 0 but Z slightly negative). + (Position(0, 2500000, -0.000000001), Velocity(-16703.901013, 0, 0)), # Elliptical flat (e=0.75), arg_pe > 180°. - # (Position(0, -2500000, 0), Velocity(16703.901013, 0, 0)), + (Position(0, -2500000, 0), Velocity(16703.901013, 0, 0)), # Elliptical flat (e=0.75), retrograde. - # (Position(2500000, 0, 0), Velocity(0, -16703.901013, 0)), + (Position(2500000, 0, 0), Velocity(0, -16703.901013, 0)), + # Elliptical flat (e=0.75), retrograde, arg_pe = 90°, at periapsis. + (Position(0, -2500000, 0), Velocity(-16703.901013, 0, 0)), + # Elliptical flat, retrograde, arg_pe = 90°, at apoapsis. + (Position(0, -2500000, 0), Velocity(-10000.0, 0, 0)), # Elliptical flat (e=0.75), f > 180°. ( Position(-13255776.4031414, -5408888.899183, 0), @@ -860,7 +886,10 @@ def test_from_state_vector_roundtrips(self): Velocity(0, 11811.441678561141, 11811.441678561141), ), # Elliptical inclined, arg_pe > 180°. - # (Position(0, -1767766.952966369, -1767766.952966369), Velocity(16703.901013, 0, 0)), + ( + Position(0, -1767766.952966369, -1767766.952966369), + Velocity(16703.901013, 0, 0), + ), ] for i, (r, v) in enumerate(CASES): @@ -1077,19 +1106,12 @@ def test_set_v_arg_pe_gt_180(self): # arg_pe = 270°. V = Velocity(-10000, 0, 0) - def set_v(value): - orbit.v = value - - # XXX The 'r and v changed' detection logic is triggered in this case, - # causing a RuntimeError to be raised. If this was not raised, the - # following asserts would be wildly off. - self.assertRaises(RuntimeError, set_v, V) - # numpy.testing.assert_almost_equal(orbit.r, R) - # numpy.testing.assert_almost_equal(orbit.v, V) + orbit.v = V + numpy.testing.assert_almost_equal(orbit.r, R) + numpy.testing.assert_almost_equal(orbit.v, V) # arg_pe should have rotated around 180°, and M0 to match (so r is in # the same spot as it was before). - # XXX This is incorrectly calculated as 90°. - # self.assertAlmostEqual(orbit.arg_pe, radians(270.0)) + self.assertAlmostEqual(orbit.arg_pe, radians(270.0)) self.assertAlmostEqual(orbit.M0, radians(180.0)) def test_set_n(self):