/** Get the orientation of the frame at the specified time. The frame's orientation
* is undefined whenever one or more of the following is true:
* - the state of either the primary or secondary object is undefined
* - the positions of the primary and secondary object are identical
* - the secondary is stationary with respect to the primary
* - the position and velocity vectors are exactly aligned
*/
Quaterniond
TwoBodyRotatingFrame::orientation(double t) const
{
StateVector state = m_secondary->state(t) - m_primary->state(t);
if (state.position().isZero() || state.velocity().isZero())
{
return Quaterniond::Identity();
}
// Compute the axes of the two body rotating frame,
// convert this to a matrix, then derive a quaternion
// from this matrix.
// x-axis points in direction from the primary to the secondary
Vector3d xAxis = state.position().normalized();
Vector3d v = state.velocity().normalized();
Vector3d zAxis;
if (m_velocityAlligned) {
// z-axis normal to both the x-axis and the velocity vector
zAxis = xAxis.cross(v);
if (zAxis.isZero())
{
return Quaterniond::Identity();
}
}
else {
// z-axis normal to both the x-axis and the z axis of the primary
zAxis = xAxis.cross(m_primary->orientation(t) * Vector3d::UnitX());
if (zAxis.isZero())
{
return Quaterniond::Identity();
}
}
zAxis.normalize();
Vector3d yAxis = zAxis.cross(xAxis);
Matrix3d m;
m << xAxis, yAxis, zAxis;
return Quaterniond(m);
}
/** Get the angular velocity of the two-body rotating frame. It
* is undefined whenever:
* - the state of either the primary or secondary object is undefined
* - the positions of the primary and secondary object are identical
*/
Vector3d
TwoBodyRotatingFrame::angularVelocity(double t) const
{
StateVector state = m_secondary->state(t) - m_primary->state(t);
if (state.position().isZero())
{
return Vector3d::Zero();
}
return state.position().cross(state.velocity()) / state.position().squaredNorm();
}
/** Calculate the state vector at the specified time (seconds since J2000 TDB).
*
* The input time is clamped to so that it lies within the range between
* the first and last record.
*/
StateVector
InterpolatedStateTrajectory::state(double tdbSec) const
{
if (!m_states.empty())
{
// Use state vector table
TimeState ts;
ts.tsec = tdbSec;
TimeStateList::const_iterator iter = lower_bound(m_states.begin(), m_states.end(), ts, TimeStateOrdering());
if (iter == m_states.begin())
{
return m_states.front().state;
}
else if (iter == m_states.end())
{
return m_states.back().state;
}
else
{
TimeState s0 = *(iter - 1);
TimeState s1 = *iter;
double h = s1.tsec - s0.tsec;
double t = (tdbSec - s0.tsec) / h;
StateVector s = cubicHermitInterpolate(s0.state.position(), s0.state.velocity() * h,
s1.state.position(), s1.state.velocity() * h,
t);
return StateVector(s.position(), s.velocity() / h);
}
}
else if (!m_positions.empty())
{
// Use position table
TimePosition ts;
ts.tsec = tdbSec;
TimePositionList::const_iterator iter = lower_bound(m_positions.begin(), m_positions.end(), ts, TimePositionOrdering());
if (iter == m_positions.begin())
{
Vector3d velocity = estimateVelocity(m_positions, 0);
return StateVector(m_positions.front().position, velocity);
}
else if (iter == m_positions.end())
{
Vector3d velocity = estimateVelocity(m_positions, m_positions.size() - 1);
return StateVector(m_positions.back().position, velocity);
}
else
{
TimePosition s0 = *(iter - 1);
TimePosition s1 = *iter;
Vector3d v0 = estimateVelocity(m_positions, iter - m_positions.begin() - 1);
Vector3d v1 = estimateVelocity(m_positions, iter - m_positions.begin());
double h = s1.tsec - s0.tsec;
double t = (tdbSec - s0.tsec) / h;
StateVector s = cubicHermitInterpolate(s0.position, v0 * h, s1.position, v1 * h, t);
return StateVector(s.position(), s.velocity() / h);
}
}
else
{
return StateVector(Vector3d::Zero(), Vector3d::Zero());
}
}
