C++ StateVector::position方法代码示例

/** 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();
}

/** 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);
}

/** 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());
    }
}