/// <summary>
/// Extract the component parts of the state x_v . Fills matrices r, q, v, omega with values.
/// </summary>
/// <param name="xv"></param>
/// <param name="r"></param>
/// <param name="q"></param>
/// <param name="v"></param>
/// <param name="omega"></param>
public void extract_r_q_v_omega(Vector xv, Vector3D r, Quaternion q, Vector3D v, Vector3D omega)
{
r.SetVNL3(xv.Extract(3, 0));
Vector qRXYZ = xv.Extract(4, 3);
q.SetRXYZ(qRXYZ);
v.SetVNL3(xv.Extract(3, 7));
omega.SetVNL3(xv.Extract(3, 10));
}
