/**
 * @brief			Description: Algorithm module of robotics, according to the
 *					book[modern robotics : mechanics, planning, and control].
 * @file:			RobotAlgorithmModule.h
 * @author:			Brian
 * @date:			2019/03/01 12:23
 * Copyright(c) 	2019 Brian. All rights reserved.
 *
 * Contact 			https://blog.csdn.net/Galaxy_Robot
 * @note:     
 * @warning: 		
*/
#ifndef RobotALGORITHMMODULE_H_
#define RobotALGORITHMMODULE_H_
#ifdef __cplusplus
extern "C" {
#endif

	#define			PINV_MAX			200       //求矩阵伪逆允许最大维数

	#define			DEBUGMODE			1			//打印调试信息
	#define			PI					3.14159265358979323846
	//if the norm of vector is near zero(< 1.0E-6),regard as zero.
	#define			ZERO_VECTOR			1.0E-6	
	#define			ZERO_ELEMENT		1.0E-6		
	#define			ZERO_ANGLE			1.0E-6
	#define			ZERO_DISTANCE		1.0E-6
	#define			ZERO_VALUE			1.0E-6

	/**
	*@brief			Description:use GrublersFormula calculate The number of degrees of
	*				freedom of a mechanism with links and joints.
	*@param[in]		N		the number of links( including the ground ).
	*@param[in]		m		the number of degrees of freedom of a rigid body
	*						(m = 3 for planar mechanisms and m = 6 for spatial mechanisms).
	*@param[in]		J		the number of joints.
	*@param[in]		f		the number of freedoms provided by each joint..
	*@return				The number of degrees of freedom of a mechanism
	*@note:					This formula holds only if all joint constraints are independent.
	*						If they are not independent then the formula provides a lower
	*						bound on the number of degrees of freedom.
	*@waring:
	*/
	int GrublersFormula(int m, int N,  int J, int *f);

	/**
	*@brief Description: Make  matrix b equal to  matrix a.
	*@param[in]			a		a 3 x 3  matrix.
	*@param[out]		b		result,b=a.
	*@return		No return value.
	*@note:
	*@warning:
	*/
	void Matrix3Equal(double a[][3], double b[][3]);

	/**
	*@brief Description: Make  matrix b equal to  matrix a.
	*@param[in]			a		a 4 x 4  matrix.
	*@param[out]		b		result,b=a.
	*@return		No return value.
	*@note:
	*@warning:
	*/
	void Matrix4Equal(double a[][4], double b[][4]);

	/**
	*@brief Description:Calculate a 3 x 3  matrix add a 3 x 3  matrix.
	*@param[in]		a		a 3 x 3  matrix.
	*@param[in]		b		a 3 x 3  matrix.
	*@param[out]	c		result of a+b.
	*@return		No return value.
	*@note:
	*@warning:
	*/
	void Matrix3Add(double a[][3], double b[][3], double c[][3]);

	/**
	*@brief Description:Calculate a 4 x 4  matrix add a 4 x 4  matrix.
	*@param[in]		a		a 4 x 4  matrix.
	*@param[in]		b		a 4 x 4  matrix.
	*@param[out]	c		result of a+b.
	*@return		No return value.
	*@note:
	*@warning:
	*/
	void Matrix4Add(double a[][4], double b[][4], double c[][4]);


	/**
	*@brief Description:Calculate a 3 x 3  matrix Subtract a 3 x 3  matrix.
	*@param[in]		a		a 3 x 3  matrix.
	*@param[in]		b		a 3 x 3  matrix.
	*@param[out]	c		result of a-b.
	*@return		No return value.
	*@note:
	*@warning:
	*/
	void Matrix3Sub(double a[][3], double b[][3], double c[][3]);

	/**
	*@brief Description:Calculate a 4 x 4  matrix Subtract a 4 x 4  matrix.
	*@param[in]		a		a 4 x 4  matrix.
	*@param[in]		b		a 4 x 4  matrix.
	*@param[out]	c		result of a-b.
	*@return		No return value.
	*@note:
	*@warning:
	*/
	void Matrix4Sub(double a[][4], double b[][4], double c[][4]);

	/**
	*@brief Description: Calculate two 3 x 3  matrix multiplication.
	*@param[in]		a		a 3 x 3  matrix.
	*@param[in]		b		a 3 x 3  matrix.
	*@param[out]	c		result of a*b.
	*@return		No return value.
	*@note:
	*@warning:
	*/
	void Matrix3Mult(double a[][3], double b[][3], double c[][3]);

	/**
	*@brief Description: Calculate two 4 x 4  matrix multiplication.
	*@param[in]		a		a 4 x 4  matrix.
	*@param[in]		b		a 4 x 4  matrix.
	*@param[out]	c		result of a*b.
	*@return		No return value.
	*@note:
	*@warning:
	*/
	void Matrix4Mult(double a[][4], double b[][4], double c[][4]);

	/**
	*@brief Description: Calculate  3 x 3  matrix multiply a value.
	*@param[in]		a		a 3 x 3  matrix.
	*@param[in]		Value	a scalar value.
	*@param[out]	c		result of a*Value.
	*@return		No return value.
	*@note:
	*@warning:
	*/
	void Matrix3MultValue(double a[][3], double Value, double c[][3]);

	/**
	*@brief Description: Calculate  4 x 4  matrix multiply a value.
	*@param[in]		a		a 4 x 4  matrix.
	*@param[in]		Value	a scalar value.
	*@param[out]	c		result of a*Value.
	*@return		No return value.
	*@note:
	*@warning:
	*/
	void Matrix4MultValue(double a[][4], double Value, double c[][4]);


	/**
	*@brief Description:Computes the result of a 3 x 3 Matrix multiply a 3-vector.
	*@param[in]		R			a 3 x 3 Matrix.
	*@param[in]		vec1		an input of 3-vector.
	*@param[out]	vec2		the output result of 3-vector.
	*@return		No return value.
	*@note:
	*@warning:
	*/
	void Matrix3MultVec(double R[3][3], double vec1[3], double vec2[3]);

	/**
	*@brief			Computes the inverse of the rotation matrix R.
	*@param[in]		R		rotation matrix .
	*@param[out]	InvR	inverse matrix of R.
	*@note					Input R must be a 3x3 rotation matrix.
	*/
	void RotInv(double R[3][3], double InvR[3][3]);

	/**
	*@brief			Description:Returns the 3 x 3 skew-symmetric matrix corresponding to omg.
	*@param[in]		omg		a unit 3-vector.
	*@param[out]	so3Mat	the 3 x 3 skew-symmetric matrix corresponding to omg.
	*@note:
	*@waring:
	*/
	void VecToso3(double omg[3], double so3Mat[3][3]);

	/**
	*@brief			Description:Returns the 3-vector corresponding to the 3×3 skew-symmetric matrix so3mat.
	*@param[in]		so3Mat		the 3×3 skew-symmetric matrix so3mat. 
	*@param[out]	omg			the 3-vector.
	*@note:
	*@waring:
	*/
	void so3ToVec(double so3Mat[3][3], double omg[3]);

	/**
	*@brief			Description:Extracts  the unit rotation axis omghat and the corresponding rotation angle theta from
	*				exponential coordinates omghat*theta for rotation, expc3.
	*@param[in]		expc3
	*@param[out]	omghat		the unit vector of rotation axis .
	*@param[out]	theta		rotation angle.
	*@retval        0			success.
	*@retval		1			failure,normal of expc3 is zero or near zero(<1.0E-6).
	*@note:
	*@waring:
	*/
	void AxisAng3(double expc3[3], double omghat[3], double *theta);

	/**
	*@brief			Description:Computes the rotation matrix R in SO(3) corresponding to
	*				the matrix exponential of so3mat in so(3).
	*@param[in]		so3Mat		[omghat]*theta,matrix exponential of so3mat in so(3).
	*@param[out]	R			rotation matrix R in SO(3).
	*@note:
	*@waring:
	*/
	void MatrixExp3(double so3Mat[3][3], double R[3][3]);

	/**
	*@brief			Description:Computes the matrix logarithm so3mat in so(3) of the rotation matrix R in SO(3).
	*@param[in]		R		the rotation matrix
	*@param[out]	so3Mat	matrix logarithm
	*@note:
	*@waring:
	*/
	void MatrixLog3(double R[3][3], double so3Mat[3][3]);

	/**
	*@brief			Description:Builds the homogeneous transformation matrix T corresponding to a rotation
	*				matrix R in SO(3) and a position vector p in R3.
	*@param[in]		R		a rotation matrix R in SO(3).
	*@param[in]		p		a position vector p in R3.
	*@param[out]	T		the homogeneous transformation matrix T.
	*@note:
	*@waring:
	*/
	void RpToTrans(double R[3][3], double p[3], double T[4][4]);

	/**
	*@brief			Description: Extracts the rotation matrix R and position vector p
	*				from a homogeneous transformation matrix T.
	*@param[in]		T		a homogeneous transformation matrix.
	*@param[out]	R		the rotation matrix.
	*@param[out]	p		position vector.
	*@note:
	*@waring:
	*/
	void TransToRp(double T[4][4], double R[3][3], double p[3]);

	/**
	*@brief			Description:Computes the inverse of a homogeneous transformation matrix T.
	*@param[in]		T		a homogeneous transformation matrix.
	*@param[out]	InvT	the inverse of T.
	*@note:
	*@waring:
	*/
	void TransInv(double T[4][4], double InvT[4][4]);

	/**
	*@brief			Description:Computes the se(3) matrix corresponding to a 6 - vector twist V.
	*@param[in]		V		a 6 - vector twist V.
	*@param[out]	se3Mat	the se(3) matrix.
	*@note:
	*@waring:
	*/
	void VecTose3(double V[6], double se3Mat[4][4]);

	/**
	*@brief			Description:Computes the 6-vector twist corresponding to an se(3) matrix se3mat.
	*@param[in]		se3Mat			an se(3) matrix.
	*@param[out]	V				he 6-vector twist.
	*@note:
	*@waring:
	*/
	void se3ToVec(double se3Mat[4][4], double V[6]);

	/**
	*@brief			Description:Computes the 6 × 6 adjoint representation [AdT ] of the homogeneous transformation matrix T.
	*@param[in]		T		a homogeneous transformation matrix.
	*@param[out]	AdT		the 6 × 6 adjoint representation [AdT ].
	*@note:
	*@waring:
	*/
	void Adjoint(double T[4][4], double AdT[6][6]);

	/**
	*@brief			Description: Extracts the normalized screw axis S and the distance traveled along the screw
	*				theta from the 6-vector of exponential coordinates S*theta.
	*@param[in]		expc6		the 6-vector of exponential coordinates.
	*@param[out]	S			the normalized screw axis.
	*@param[out]	theta		the distance traveled along the screw.
	*@retval		0			success;
	*@retval		1			failure,because the input expc6 is a zero vector
	*@note:
	*@waring:
	*/
	void AxisAng6(double expc6[6], double S[6], double *theta);

	/**
	*@brief			Description: Computes the homogeneous transformation matrix T in SE(3) corresponding to
	*				the matrix exponential of se3mat in se(3).
	*@param[in]		se3mat			the matrix exponential of se3mat in se(3).
	*@param[out]	T				the homogeneous transformation matrix T in SE(3).
	*@note:
	*@waring:
	*/
	void MatrixExp6(double se3Mat[4][4], double T[4][4]);

	/**
	*@brief			Description: Computes the matrix logarithm se3mat in se(3) of
	*				the homogeneous transformation matrix T in SE(3)
	*@param[in]		T			the homogeneous transformation matrix.
	*@param[out]	se3Mat		the matrix logarithm of T.
	*@note:
	*@waring:
	*/
	void MatrixLog6(double T[4][4], double se3Mat[4][4]);

	/**
	*@brief			Description: Computes the end-effector frame given the zero position of the end-effector M,
	*				the list of joint screws Slist expressed in the fixed-space frame, and the list of joint values thetalist.
	*@param[in]		M			the zero position of the end-effector expressed in the fixed-space frame.
	*@param[in]		Joint		Num	the number of joints.
	*@param[in]		Slist		the list of joint screws Slist expressed in the fixed-space frame.
	*							in the format of a matrix with the screw axes as the column.
	*@param[in]		thetalist   the list of joint values.
	*@param[out]	T			the end-effector frame expressed in the fixed-space frame.
	*@return		No return value.
	*@note:			when Slist is a matrix ,make sure that columns number of Slist is equal to JointNum,
	*				rows number of Slist 6 .The function call should be written as
	*				FKinSpace(...,(double *)Slist,...).
	*@waring:
	*/
	void FKinSpace(double M[4][4], int  JointNum, double *Slist, double *thetalist, double T[4][4]);

	/**
	*@brief			Description:Computes the end-effector frame given the zero position of the end-effector M,
	*				the list of joint screws Blist expressed in the end-effector frame, and the list of joint values thetalist.
	*@param[in]		M			the zero position of the end-effector expressed in the end-effector frame.
	*@param[in]		JointNum	the number of joints.
	*@param[in]		Blist		the list of joint screws Slist expressed in the end-effector frame.
	*							in the format of a matrix with the screw axes as the column.
	*@param[in]		thetalist   the list of joint values.
	*@param[out]	T			the end-effector frame expressed in the end-effector frame.
	*@return		No return value.
	*@note:			when Blist is a matrix ,make sure that columns number of Slist is equal to JointNum,
	*				rows number of Slist 6 .The function call should be written as
	*				FKinBody(...,(double *)Blist,...).
	*@waring:
	*/
	void FKinBody(double M[4][4], int  JointNum, double *Blist, double thetalist[], double T[4][4]);

	/**
	*@brief Description: Computes the body Jacobian Jb(theta) in 6×n given a list of joint screws Bi
	expressed in the body frame and a list of joint angles.
	*@param[in]		Blist		The joint screw axes in the end - effector frame when the manipulator is
	*							at the home position, in the format of a matrix with the screw axes as the row.
	*@param[in]		thetalist	A list of joint coordinates.
	*@param[out]	Jb			Body Jacobian matrix.
	*@return        No return value.
	*@note:			 when Blist and Jb are matrixes ,make sure that columns number of Slist or Jb is equal to JointNum,
	*				 rows number of Slist or Jb is 6 .The function call should be written as
	*				 JacobianSpace(JointNum,(double *)Slist,thetalist,(double *)Jb).
	*@waring:
	*/
	void JacobianBody(int JointNum, double *Blist, double *thetalist, double *Jb);


	/**
	*@brief			Description:Computes the space Jacobian Js(theta) in R6 x n given a list of joint screws Si
	*				expressed in the fixed space frame and a list of joint angles.
	*@param[in]		Slist		The joint screw axes expressed in the fixed space frame when the manipulator is
	*							at the home position, in the format of a matrix with the screw axes as the column.
	*@param[in]		thetalist	A list of joint coordinates.
	*@param[out]	Js			Space Jacobian matrix.
	*@return		 No return value.
	*@note:			 when Slist and Js are matrixes ,make sure that columns number of Slist or Js is equal to JointNum,
	*				 rows number of Slist or Js is 6 .The function call should be written as
	*				 JacobianSpace(JointNum,(double *)Slist,thetalist,(double *)Js).
	*@waring:
	*/
	void JacobianSpace(int JointNum, double *Slist, double *thetalist, double *Js);

	/**
	*@brief			Given the matrix a,this routine computes its singular value decomposition.
	*@param[in/out]	a		input matrix a or output matrix u.
	*@param[in]		m		number of rows of matrix a.
	*@param[in]		n		number of columns of matrix a.
	*@param[in]		tol		any	singular values less than a tolerance are treated as zero.
	*@param[out]	w		output vector w.
	*@param[out]	v		output matrix v.
	*@return        No return value.
	*@note:			input a must be a  m rows and n columns matrix,w is n-vector,v is a n rows and n columns matrix. 
	*@waring:
	*/
	int svdcmp(double *a, int m, int n,double tol, double *w, double *v);

	/**
	*@brief			Copy  matrix from a to  b .
	*@param[in]		a		First matrix.
	*@param[in]		m		rows of matrix a.
	*@param[in]		n		columns of matrix a.
	*@param[in]		b		Second matrix.
	*@note:
	*@waring:
	*/
	void MatrixCopy(const double *a, int m, int n, double *b);

	/**
	*@brief			Computes matrix multiply.
	*@param[in]		a		First matrix.
	*@param[in]		m		rows of matrix a.
	*@param[in]		n		columns of matrix b.
	*@param[in]		b		Second matrix.
	*@param[in]		l		columns of matrix b.
	*@param[out]	c		result of a*b.
	*@note:
	*@waring:
	*/
	void MatrixMult(const double *a, int m, int n, const double *b, int l, double *c);

	/**
	*@brief			Computes Matrix transpose.
	*@param[in]		a			a matrix.
	*@param[in]		m			rows of matrix a.
	*@param[in]		n			columns of matrix a.
	*@param[out]	b			transpose of matrix a.
	*@note:
	*@waring:
	*/
	void MatrixT(double *a, int m, int n, double *b);

	/**
	*@brief			Computes Moore-Penrose pseudoinverse by  Singular Value Decomposition (SVD) algorithm.
	*@param[in]		a			an arbitrary order matrix.
	*@param[in]		m			rows.
	*@param[in]		n			columns.
	*@param[in]		tol			any	singular values less than a tolerance are treated as zero.
	*@param[out]	b			Moore-Penrose pseudoinverse of matrix a.
	*@return        0:success,Nonzero:failure.
	*@retval		0			success.
	*@retval		1			failure when use malloc to allocate memory.
	*@note:
	*@waring:
	*/
	int MatrixPinv(const double *a, int m, int n, double tol, double *b);


	/**
	*@brief 		Description:use an iterative Newton-Raphson root-finding method.
	*@param[in]		JointNum		Number of joints.
	*@param[in]		Blist			The joint screw axes in the Body frame when the manipulator
	*								is at the home position, in the format of a matrix with the
	*								screw axes as the columns.
	*@param[in]		M				The home configuration of the end-effector.
	*@param[in]		T				The desired end-effector configuration Tsd.
	*@param[in]		thetalist0		An initial guess of joint angles that are close to satisfying Tsd.
	*@param[in]		eomg			A small positive tolerance on the end-effector orientation error.
	*								The returned joint angles must give an end-effector orientation error less than eomg.
	*@param[in]		ev				A small positive tolerance on the end-effector linear position error. The returned
	*								joint angles must give an end-effector position error less than ev.
	*@param[in]		maxiter			The maximum number of iterations before the algorithm is terminated.
	*@param[out]	thetalist		Joint angles that achieve T within the specified tolerances.
	*@return		0:success,Nonzero:failure
	*@retval		0				success.
	*@retval		1				The maximum number of iterations reach before the algorithm is terminated.
	*@retval		2				failure to allocate memory for Jacobian matrix.
	*@note:
	*@waring:
	*/
	int IKinBodyNR(int JointNum, double *Blist, double M[4][4], double  T[4][4], double *thetalist0, double eomg, double ev, int maxiter, double *thetalist);


	/**
	*@brief 		Description:use an iterative Newton-Raphson root-finding method.
	*@param[in]		JointNum		Number of joints.
	*@param[in]		Slist			The joint screw axes in the space frame when the manipulator
	*								is at the home position, in the format of a matrix with the
	*								screw axes as the columns.
	*@param[in]		M				The home configuration of the end-effector.
	*@param[in]		T				The desired end-effector configuration Tsd.
	*@param[in]		thetalist0		An initial guess of joint angles that are close to satisfying Tsd.
	*@param[in]		eomg			A small positive tolerance on the end-effector orientation error.
	*								The returned joint angles must give an end-effector orientation error less than eomg.
	*@param[in]		ev				A small positive tolerance on the end-effector linear position error. The returned
	*								joint angles must give an end-effector position error less than ev.
	*@param[in]		maxiter			The maximum number of iterations before the algorithm is terminated.
	*@param[out]	thetalist		Joint angles that achieve T within the specified tolerances.
	*@return		0:success,Nonzero:failure
	*@retval		0
	*@note:
	*@waring:
	*/
	int IKinSpaceNR(int JointNum, double *Slist, double M[4][4], double T[4][4], double *thetalist0, double eomg, double ev, int maxiter, double *thetalist);


	/**
	* @brief 			Description: Algorithm for Computing the roll-pitch-yaw angles(rotate around fix reference X,Y,Z axis).
	* @param[in]		R				Rotation matrix.
	* @param[out]		roll			Angles for rotate around fix reference X axis.
	* @param[out]		pitch			Angles for rotate around fix reference Y axis.
	* @param[out]		yaw				Angles for rotate around fix reference Z axis.
	* @return			No return value.
	* @note:
	*@warning:
	*/
	void RotToRPY(double R[3][3], double *roll, double *pitch, double *yaw);


	/**
	* @brief 			Description: Algorithm for Computing the rotation matrix of the roll-pitch-yaw angles.
	* @param[in]		roll			Angles for rotate around fix reference X axis.
	* @param[in]		pitch			Angles for rotate around fix reference Y axis.
	* @param[in]		yaw				Angles for rotate around fix reference Z axis.
	* @param[out]		R				Rotation matrix.
	* @return			No return value.
	* @note:
	*@warning:
	*/
	void RPYToRot(double roll, double pitch, double yaw, double R[3][3]);


	/**
	* @brief 			Description: Algorithm for Computing the ZYX Euler Angles according to rotation matrix.
	* @param[in]		R				Rotation matrix.
	* @param[out]		alpha			Angles for rotation around Z axis.
	* @param[out]		beta			Angles for rotation around Y axis.
	* @param[out]		gamma			Angles for rotation around X axis.
	* @return			No return value.
	* @note:
	* @waring:
	*/
	void RotToZYXEulerAngle(double R[3][3], double *alpha, double *beta, double *gamma);


	/**
	* @brief 			Description: Algorithm for Computing the rotation matrix of ZYX Euler Angles.
	* @param[in]		alpha			Angles for rotation around Z axis.
	* @param[in]		beta			Angles for rotation around Y axis.
	* @param[in]		gamma			Angles for rotation around X axis.
	* @param[out]		R				Rotation matrix.
	* @return			No return value.
	* @note:
	* @waring:
	*/
	void ZYXEulerAngleToRot(double alpha, double beta, double gamma, double R[3][3]);


	/**
	* @brief 			Description: Computes the unit vector of Euler axis and rotation angle corresponding to rotation matrix.
	* @param[in]		R				A rotation matrix.
	* @param[out]		omghat			the unit vector of Euler axis .
	* @param[out]		theta			the rotation angle.
	* @return			No return value.
	* @retval			0
	* @note:			if  theta is zero ,the unit axis is undefined and set it as a zero vector [0;0;0].
	* @waring:
	*/
	void RotToAxisAng(double R[3][3], double omghat[3], double *theta);


	/**
	* @brief 			Description: Computes the unit quaternion corresponding to the Euler axis and rotation angle.
	* @param[in]		omg				Unit vector of Euler axis.
	* @param[in]		theta			Rotation angle.
	* @param[in]		q				The unit quaternion
	* @return			No return value.
	* @note:
	* @warning:
	*/
	void AxisAngToQuaternion(double omg[3], double theta, double q[4]);


	/**
	* @brief 			Description:Computes the unit quaternion corresponding to a rotation matrix.
	* @param[in]		q				Unit quaternion.
	* @param[out]		R				Rotation matrix.
	* @return			No return value.
	* @note:
	* @warning:
	*/
	void QuaternionToRot(double q[4], double R[3][3]);


	/**
	* @brief 			Description: Computes the unit quaternion corresponding to the rotation matrix.
	* @param[in]		R				The rotation matrix.
	* @param[out]		q				The unit quaternion.
	* @return			No return value.
	* @note:
	* @warning:
	*/
	void RotToQuaternion(double R[3][3], double q[4]);

	/**
	* @brief 			单位四元数转换为旋转矩阵。
	* @param[in]		q				单位四元数[ox,oy,oz,ow].
	* @param[out]		R				旋转矩阵.
	* @return			无返回值.
	* @note:
	* @warning:         (1)此函数不对输入参数做检查，需保证输入为单位四元数。
						(2)四元数顺序为[ox,oy,oz,w]。
	*/
	void q2rot(double q[4], double R[3][3]);

	/**
	* @brief 			旋转矩阵转单位四元数。
	* @param[in]		R				旋转矩阵
	* @param[out]		q				单位四元数.[ox,oy,oz,ow].
	* @return			无返回值.
	* @note:
	* @warning:         (1)此函数不对输入参数做检查，需保证输入为旋转矩阵。
						(2)四元数顺序为[ox,oy,oz,w]。
	*/
	void rot2q(double R[3][3], double q[4]);

	/**
	* @brief 			Description: structure of LinePath interpolation parameters.
	*/
	typedef struct
	{
		double p1[3];
		double p2[3];
		double L;
		double t[3];
		double pi[3];
		double Li;
		int InpFlag;
	}LineInpParam;


	/**
	* @brief 			Description: structure of arc interpolation parameters.
	*/
	typedef struct  
	{
		double p1[3];
		double p2[3];
		double p3[3];
		double N[3];
		double C[3];
		double theta;
		double R;
	}ArcInpParam;


	/**
	* @brief 			Description: structure of orientation interpolation parameters.
	*/
	typedef struct
	{
		double Rs[3][3];//Start orientation Rotation matrix. 
		double Re[3][3];//End orientation Rotation matrix.
		double R[3][3];	//Matrix rotation from Start orientation to End orientation.
		double omg[3];  //unit vector of Euler axis.
		double theta;	//Total interpolation angle.
		double Ri[3][3];//Current orientation rotation Matrix.
		double thetai;  //Current angle.
		int InpFlag;	//1:finish initial ,2;interpolating ,3;finish interpolation
	}OrientInpParam;


	/**
	* @brief 			Description: structure of line Path and Orientation (PO) interpolation parameters.
	*/
	typedef struct  
	{
		LineInpParam Line;
		OrientInpParam Orient;
		double Ts[4][4];
		double Te[4][4];
		double Ti[4][4];
		int InpFlag;
	}LinePOParam;


	/**
	* @brief 			Description:Computes the parameters of  interpolation between two orientations.
	* @param[in]		Rs				Start posture Rotation matrix.
	* @param[in]		Re				End posture Rotation matrix.
	* @param[in]		Param			structure of orientation interpolation parameters..
	* @return			No return value.
	* @note:
	* @warning:
	*/
	void InitialOrientInpParam(double Rs[3][3], double Re[3][3], OrientInpParam *Param);


	/**
	* @brief 			Description: Computes orientations interpolation.
	* @param[in]		Param			Interpolation parameter structure.
	* @param[out]		dtheta			angle  need to rotate from previous orientation to next orientation in next time step.
	* @return			Ri1				orientations in next time step.
	* @retval			0
	* @note:
	* @warning:
	*/
	void QuaternionOrientInp(OrientInpParam *Param, double dtheta, double Ri1[3][3]);


	/**
	* @brief 			Description:Computes the parameters of line path for interpolation.
	* @param[in]		p1				Coordinates of start point.
	* @param[in]		p2				Coordinates of end point.
	* @param[out]		p				Line path parameters structure.
	* @return			No return value.
	* @note:
	* @warning:
	*/
	void InitialLinePathParam(double p1[3], double p2[3], LineInpParam *p);


	/**
	* @brief 			Description:Computes the line path interpolation coordinates in each interpolation cycle.
	* @param[in]		p				Line path parameters structure.
	* @param[in]		dL				step length in next interpolation cycle.
	* @param[in]		pi1				coordinates in next interpolation cycle.
	* @return			No return value.
	* @note:
	* @warning:
	*/
	void LinePathInp(LineInpParam *p, double dL, double pi1[3]);

	/**
	* @brief 			Description: Computes the parameters of both line path and orientation for interpolation.
	* @param[in]		p1				Start coordinates,including x,y,z coordinates and orientation angles roll-pitch-yaw angles.
	* @param[in]		p2				End coordinates,including x,y,z coordinates and orientation angles roll-pitch-yaw angles.
	* @param[out]		LPO				Parameters of both line path and orientation for interpolation.
	* @return			No return value.
	* @note:
	* @warning:
	*/
	void InitialLinePOInpParam(double p1[6], double p2[6], LinePOParam *LPO);


	/**
	* @brief 			Description:Computes the line path interpolation coordinates and orientation in each interpolation cycle.
	* @param[out]		LPO				Line path and orientation parameters structure.
	* @param[in]		dL				Line path interpolation step length.
	* @param[out]		dtheta			angle interpolation step length for Orientation interpolation.
	* @return			No return value.
	* @note:
	* @warning:
	*/
	void LinePOInp(LinePOParam *LPO, double dL, double dtheta, double Ti[4][4]);






#ifdef __cplusplus
}
#endif

#endif//RobotALGORITHMMODULE_H_