File content as of revision 2:8d9a79e18fb9:

#ifndef _utilities_hpp
#define _utilities_hpp


#include "Core.h"
#ifndef M_PI
#define M_PI 3.14159265358979
#endif

using namespace std;  //calling the standard directory
using namespace Eigen;


 

namespace utilities{ 

	inline Matrix3f rotx(float alpha){
		Matrix3f Rx;
		Rx << 	1,0,0,
			0,cos(alpha),-sin(alpha),
			0,sin(alpha), cos(alpha);
		return Rx;
	}

	inline Matrix3f roty(float beta){
		Matrix3f Ry;
		Ry << 	cos(beta),0,sin(beta),
			0,1,0,
			-sin(beta),0, cos(beta);
		return Ry;
	}

	inline Matrix3f rotz(float gamma){
		Matrix3f Rz;
		Rz << 	cos(gamma),-sin(gamma),0,
			sin(gamma),cos(gamma),0,
			0,0, 1;
		return Rz;
	}


	inline Matrix4f rotx_T(float alpha){
		Matrix4f Tx = Matrix4f::Identity();
		Tx.block(0,0,3,3) = rotx(alpha);
		return Tx;
	}

	inline Matrix4f roty_T(float beta){
		Matrix4f Ty = Matrix4f::Identity();
		Ty.block(0,0,3,3) = roty(beta);
		return Ty;
	}

	inline Matrix4f rotz_T(float gamma){
		Matrix4f Tz = Matrix4f::Identity();
		Tz.block(0,0,3,3) = rotz(gamma);
		return Tz;
	}

	inline Matrix3f skew(Vector3f v)
	{
		Matrix3f S;
		S << 0,	-v[2],	 v[1],		//Skew-symmetric matrix
			v[2],	    0,	-v[0],
			-v[1],	 v[0], 	   0;
		return S;
	}


	inline Matrix3f L_matrix(Matrix3f R_d, Matrix3f R_e)
	{
		Matrix3f L = -0.5 * (skew(R_d.col(0))*skew(R_e.col(0)) + skew(R_d.col(1))*skew(R_e.col(1)) + skew(R_d.col(2))*skew(R_e.col(2)));
		return L;
	}



	inline Vector3f rotationMatrixError(Matrix4f Td, Matrix4f Te)
	{
		
		Matrix3f R_e = Te.block(0,0,3,3);		//Matrix.slice<RowStart, ColStart, NumRows, NumCols>();	
		Matrix3f R_d = Td.block(0,0,3,3);
		
		Vector3f eo = 0.5 * (skew(R_e.col(0))*R_d.col(0) + skew(R_e.col(1))*R_d.col(1) + skew(R_e.col(2))*R_d.col(2)) ;
		return eo;
	}


	inline Vector3f r2quat(Matrix3f R_iniz, float &eta)
	{
		Vector3f epsilon;
		int iu, iv, iw;

		if ( (R_iniz(0,0) >= R_iniz(1,1)) && (R_iniz(0,0) >= R_iniz(2,2)) )
		{
			iu = 0; iv = 1; iw = 2;
		}
		else if ( (R_iniz(1,1) >= R_iniz(0,0)) && (R_iniz(1,1) >= R_iniz(2,2)) )
		{
			iu = 1; iv = 2; iw = 0;
		}
		else
		{
			iu = 2; iv = 0; iw = 1;
		}

		float r = sqrt(1 + R_iniz(iu,iu) - R_iniz(iv,iv) - R_iniz(iw,iw));
		Vector3f q;
		q <<  0,0,0;
		if (r>0)
		{
		float rr = 2*r;
		eta = (R_iniz(iw,iv)-R_iniz(iv,iw)/rr);
		epsilon[iu] = r/2;
		epsilon[iv] = (R_iniz(iu,iv)+R_iniz(iv,iu))/rr;
		epsilon[iw] = (R_iniz(iw,iu)+R_iniz(iu,iw))/rr;
		}
		else
		{
		eta = 1;
		epsilon << 0,0,0;
		}
		return epsilon;
	}


	inline Vector4f rot2quat(Matrix3f R){

		float m00, m01, m02, m10, m11, m12, m20, m21, m22;

		m00 = R(0,0);
		m01 = R(0,1);
		m02 = R(0,2);
		m10 = R(1,0);
		m11 = R(1,1);
		m12 = R(1,2);
		m20 = R(2,0);
		m21 = R(2,1);
		m22 = R(2,2);

		float tr = m00 + m11 + m22;
		float qw, qx, qy, qz, S;
		Vector4f quat;

		if (tr > 0) { 
		  S = sqrt(tr+1.0) * 2; // S=4*qw 
		  qw = 0.25 * S;
		  qx = (m21 - m12) / S;
		  qy = (m02 - m20) / S; 
		  qz = (m10 - m01) / S; 
		} else if ((m00 > m11)&(m00 > m22)) { 
		  S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx 
		  qw = (m21 - m12) / S;
		  qx = 0.25 * S;
		  qy = (m01 + m10) / S; 
		  qz = (m02 + m20) / S; 
		} else if (m11 > m22) { 
		  S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
		  qw = (m02 - m20) / S;
		  qx = (m01 + m10) / S; 
		  qy = 0.25 * S;
		  qz = (m12 + m21) / S; 
		} else { 
		  S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
		  qw = (m10 - m01) / S;
		  qx = (m02 + m20) / S;
		  qy = (m12 + m21) / S;
		  qz = 0.25 * S;
		}

		quat << qw, qx, qy, qz;
		return quat;

	}
	
	
		 //Matrix ortonormalization
	inline Matrix3f matrixOrthonormalization(Matrix3f R){

		SelfAdjointEigenSolver<Matrix3f> es(R.transpose()*R);
		Vector3f D = es.eigenvalues();
		Matrix3f V = es.eigenvectors();
		R = R*((1/sqrt(D(0)))*V.col(0)*V.col(0).transpose() + (1/sqrt(D(1)))*V.col(1)*V.col(1).transpose() + (1/sqrt(D(2)))*V.col(2)*V.col(2).transpose());

		return R;
	}




	//******************************************************************************
	inline Vector3f quaternionError(Matrix4f Tbe, Matrix4f Tbe_d)
	{
		float eta, eta_d;
		Matrix3f R = Tbe.block(0,0,3,3);		//Matrix.slice<RowStart, ColStart, NumRows, NumCols>();	
		Vector3f epsilon = r2quat(R, eta);
		Matrix3f R_d = Tbe_d.block(0,0,3,3);
		Vector3f epsilon_d = r2quat(R_d, eta_d);
		Matrix3f S = skew(epsilon_d);
		Vector3f eo = eta*epsilon_d-eta_d*epsilon-S*epsilon;
		return eo;
	}


	inline Vector3f versorError(Vector3f P_d, Matrix3f Tbe_e, float &theta)
	{
		Vector3f P_e = Tbe_e.block(0,3,3,1);
		Vector3f u_e = Tbe_e.block(0,0,3,1);

		Vector3f u_d = P_d - P_e;
		u_d = u_d/ u_d.norm();
		
		Vector3f r = skew(u_e)*u_d;
		float nr = r.norm();
			
	  if (nr >0){
			r = r/r.norm();
			

			//be carefult to acos( > 1 )
			float u_e_d = u_e.transpose()*u_d;		
			if( fabs(u_e_d) <= 1.0 ) {
				theta = acos(u_e.transpose()*u_d);
			}
			else {
				theta = 0.0;
			}
			
			Vector3f error = r*sin(theta);
			return error;
		}else{
			theta = 0.0;
			return Vector3f::Zero();
		}
	}


	/*Matrix3f utilities::rotation ( float theta, Vector3f r ) {
	     
	    Matrix3f R = Zeros;
	 
	    R[0][0] = r[0]*r[0]*(1-cos(theta)) + cos(theta);
	    R[1][1] = r[1]*r[1]*(1-cos(theta)) + cos(theta);
	    R[2][2] = r[2]*r[2]*(1-cos(theta)) + cos(theta);
	 
	    R[0][1] = r[0]*r[1]*(1-cos(theta)) - r[2]*sin(theta);
	    R[1][0] = r[0]*r[1]*(1-cos(theta)) + r[2]*sin(theta);
	 
	    R[0][2] = r[0]*r[2]*(1-cos(theta)) + r[1]*sin(theta);
	    R[2][0] = r[0]*r[2]*(1-cos(theta)) - r[1]*sin(theta);
	     
	    R[1][2] = r[1]*r[2]*(1-cos(theta)) - r[0]*sin(theta);
	    R[2][1] = r[1]*r[2]*(1-cos(theta)) + r[0]*sin(theta);
	     
	 
	    return R;
	}*/


	inline Matrix3f XYZ2R(Vector3f angles) {
	  	
	  	Matrix3f R = Matrix3f::Zero(); 
	  	Matrix3f R1 = Matrix3f::Zero(); 
	  	Matrix3f R2 = Matrix3f::Zero(); 
	  	Matrix3f R3 = Matrix3f::Zero();

		float cos_phi = cos(angles[0]);
		float sin_phi = sin(angles[0]);
		float cos_theta = cos(angles[1]);
		float sin_theta = sin(angles[1]);
		float cos_psi = cos(angles[2]);
		float sin_psi = sin(angles[2]);

		R1  << 1, 0      , 0, 
			        0, cos_phi, -sin_phi, 
			        0, sin_phi, cos_phi;

		R2  << cos_theta , 0, sin_theta, 
			        0        , 1, 0       , 
			        -sin_theta, 0, cos_theta;

		R3  << cos_psi, -sin_psi, 0, 
			        sin_psi, cos_psi , 0,
			        0      , 0       , 1;

		R = R1*R2*R3;

		return R;
	}

	// This method computes the XYZ Euler angles from the Rotational matrix R.
	inline Vector3f R2XYZ(Matrix3f R) {
		double phi=0.0, theta=0.0, psi=0.0;
		Vector3f XYZ = Vector3f::Zero();
		
		theta = asin(R(0,2));
		
		if(fabsf(cos(theta))>pow(10.0,-10.0))
		{
			phi=atan2(-R(1,2)/cos(theta), R(2,2)/cos(theta));
			psi=atan2(-R(0,1)/cos(theta), R(0,0)/cos(theta));
		}
		else
		{
			if(fabsf(theta-M_PI/2.0)<pow(10.0,-5.0))
			{
				psi = 0.0;
				phi = atan2(R(1,0), R(2,0));
				theta = M_PI/2.0;
			}
			else
			{
				psi = 0.0;
				phi = atan2(-R(1,0), R(2,0));
				theta = -M_PI/2.0;
			}
		}
		
		XYZ << phi,theta,psi;
		return XYZ;
	}

	inline Matrix3f angleAxis2Rot(Vector3f ri, float theta){
	Matrix3f R;
	R << ri[0]*ri[0] * (1 - cos(theta)) + cos(theta)           , ri[0] * ri[1] * (1 - cos(theta)) - ri[2] * sin(theta) , ri[0] * ri[2] * (1 - cos(theta)) + ri[1] * sin(theta),
	         ri[0] * ri[1] * (1 - cos(theta)) + ri[2] * sin(theta) , ri[1]*ri[1] * (1 - cos(theta)) + cos(theta)           , ri[1] * ri[2] * (1 - cos(theta)) - ri[0] * sin(theta),
	         ri[0] * ri[2] * (1 - cos(theta)) - ri[1] * sin(theta) , ri[1] * ri[2] * (1 - cos(theta)) + ri[0] * sin(theta) , ri[2]*ri[2] * (1 - cos(theta)) + cos(theta);

	return R;

	}



	inline Vector3f butt_filter(Vector3f x, Vector3f x1, Vector3f x2, float omega_n, float zita, float ctrl_T){
		//applico un filtro di Butterworth del secondo ordine (sfrutto Eulero all'indietro)
		return x1*(2.0 + 2.0*omega_n*zita*ctrl_T)/(omega_n*omega_n*ctrl_T*ctrl_T + 2.0*omega_n*zita*ctrl_T + 1.0) - x2/(omega_n*omega_n*ctrl_T*ctrl_T + 2.0*omega_n*zita*ctrl_T + 1.0) + x*(omega_n*omega_n*ctrl_T*ctrl_T)/(omega_n*omega_n*ctrl_T*ctrl_T + 2.0*omega_n*zita*ctrl_T + 1.0);
	}



	/// converts a rate in Hz to an integer period in ms.
	inline uint16_t rateToPeriod(const float & rate) {
		if (rate > 0)
			return static_cast<uint16_t> (1000.0 / rate);
		else
			return 0;
	}




	    //Quaternion to rotration Matrix
	inline Matrix3f QuatToMat(Vector4f Quat){
		Matrix3f Rot;
		float s = Quat[0];
		float x = Quat[1];
		float y = Quat[2];
		float z = Quat[3];
		Rot << 1-2*(y*y+z*z),2*(x*y-s*z),2*(x*z+s*y),
		2*(x*y+s*z),1-2*(x*x+z*z),2*(y*z-s*x),
		2*(x*z-s*y),2*(y*z+s*x),1-2*(x*x+y*y);
		return Rot;
	}
	
	
	inline float rad2deg(float rad){
		float deg;
		deg = 180.0*rad/M_PI;
		return deg;
	}
	
	inline float deg2rad(float deg){
		float rad;
		rad = M_PI*deg/180.0;
		return rad;
	}
}

#endif