|
|
|
|
@ -196,15 +196,21 @@ testManeuverStep(const ManeuverNode& maneuver,
|
|
|
|
|
if (!maneuver.active) |
|
|
|
|
return false; |
|
|
|
|
|
|
|
|
|
double r = maneuver.true_anomaly; |
|
|
|
|
previous_theta = orbitClampAngle(previous_theta); |
|
|
|
|
next_theta = orbitClampAngle(next_theta); |
|
|
|
|
// NOTE: clamp angles between 0 and 2 pi to simplify inequalities
|
|
|
|
|
double r = orbitClampAngle(maneuver.true_anomaly); |
|
|
|
|
double clamped_previous_theta = orbitClampAngle(previous_theta); |
|
|
|
|
double clamped_next_theta = orbitClampAngle(next_theta); |
|
|
|
|
|
|
|
|
|
// FIXME: 2 unhandled cases:
|
|
|
|
|
// 1) sat passes through 0 degrees
|
|
|
|
|
// FIXME: unhandled cases:
|
|
|
|
|
// 2) direction is ccw
|
|
|
|
|
//
|
|
|
|
|
if (previous_theta <= r && next_theta >= r) |
|
|
|
|
|
|
|
|
|
// NOTE: sat passess through 0
|
|
|
|
|
if (clamped_next_theta - clamped_previous_theta < 0) { |
|
|
|
|
if (r >= clamped_previous_theta || r <= clamped_next_theta) |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
else if (r >= clamped_previous_theta && r <= clamped_next_theta) |
|
|
|
|
return true; |
|
|
|
|
|
|
|
|
|
return false; |
|
|
|
|
|