/// <summary>
/// Create a state vector x_v from its component parts. Puts the matrices r, q, v, omega into their right places.
/// </summary>
/// <param name="r"></param>
/// <param name="q"></param>
/// <param name="v"></param>
/// <param name="omega"></param>
/// <param name="xv"></param>
public void compose_xv(ref Vector3D r, ref Quaternion q, ref Vector3D v, ref Vector3D omega, ref Vector xv)
{
xv.Update(r.GetVNL3(), 0);
xv.Update(q.GetRXYZ(), 3);
xv.Update(v.GetVNL3(), 7);
xv.Update(omega.GetVNL3(), 10);
}