/*
 * =====================================================================================
 *
 *       Filename:  body.cpp
 *
 *    Description:  Method definitions for body class.
 *
 *        Version:  1.0
 *        Created:  03/17/2017 08:07:35 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Martin Miller (MHM), miller7@illinois.edu
 *   Organization:  Aerospace Robotics and Controls Lab (ARC)
 *
 * =====================================================================================
 */

#include <Eigen/Dense>
#include "body.h"

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: H
 * Description:  Writes the Jacobian of the measurement model into H.
 *--------------------------------------------------------------------------------------
 */
void
Body::H ( Matrix<double,1,9> &H )
{
    H = Matrix<double,1,9>::Zero();
    H[0,2] = 1;
    return ;
}		/* -----  end of method Body::H  ----- */


/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: h
 * Description:  Writes the predicted measurement vector into h.
 *--------------------------------------------------------------------------------------
 */
void
Body::h ( const Matrix<double,9,1> &X, Matrix<double,1,1> &h )
{
    h[0] = X[2];
    return ;
}		/* -----  end of method Body::h_hat  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: F
 * Description:  Computes the Jacobian of the motion model of the body.
 *--------------------------------------------------------------------------------------
 */
void
Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q )
{
    Matrix<double,9,9> F = Matrix<double,9,9>::Zero();
    Matrix<double,3,3> Rbw(q.toRotationMatrix());
    Matrix<double,3,3> W;
    skewSymmetric(ang,W);
    F.block(0,3,3,3) = Rbw;
    F.block(3,3,3,3) = -W;
    F.block(3,6,3,3) = -Matrix<double,3,3>::Identity();
    return ;
}		/* -----  end of method Body::F  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: motionModel
 * Description:  Propagates the motion model of the body into vector X.
 *--------------------------------------------------------------------------------------
 */
void
Body::motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt)
{
    Vector3d bias(X.segment(6,3));
    Vector3d gravity_world(0.,0.,9.80655);

    Matrix<double,6,3> A;
    Matrix<double,6,1> b;
    A = Matrix<double,6,3>::Zero();
    b = Matrix<double,6,1>::Zero();

    Matrix<double,3,3> Rbw(q.toRotationMatrix());
    A.block(0,0,3,3) = Rbw;
    Matrix<double,3,3> W;
    skewSymmetric(ang,W);
    A.block(3,0,3,3) = -W;
    b.segment(3,3) = acc-bias+Rbw.transpose()*gravity_world;

    X.segment(0,6) += (A*X.segment(3,3)+b)*dt;
    return ;
}		/* -----  end of method Body::motionModel  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Body
 *      Method:  Body :: skewSymmetric
 * Description:  Create the skew symmetric matrix y from the vector x.
 *--------------------------------------------------------------------------------------
 */
void
Body::skewSymmetric ( const Vector3d &x, Matrix<double,3,3> &y )
{
    y << 0.,-x[2], x[1], x[2],0.,-x[0],-x[1],x[0],0.;
    return ;
}		/* -----  end of method Body::skewSymmetric  ----- */