/*
 * =====================================================================================
 *
 *       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"

    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  ----- */

    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  ----- */