/*******************************************************************

KinematicsMatrix.h		developed by naka_t	2009.02.24


	運動学計算に使う行列計算
　＊プログラム作成					naka_t 2009.02.24

*******************************************************************/
#pragma once
#include "OpenCV.h"
#include <math.h>

#define M_PI acos(-1.0)

//
//	DHパラメータから同次変換行列を生成
//
CvMat *cvmCreateRotMat( double alpha , double theta );

//
//	DHパラメータから回転行列を生成
//
CvMat *cvmCreateRotMat( double rx , double ry , double rz );

//
//	DHパラメータから回転行列を生成
//
CvMat *cvmCreateTransMat( double a , double alpha , double d , double theta  );

//
//	x,y,z,rz,ry,rxから同次変換行列を生成
//
CvMat *cvmCreateTransMat( double a , double P[6]  );

//
//	DHパラメータから同次変換行列を計算
//
void cvmSetTransMatrix( CvMat *trans , double a , double alpha , double d , double theta );

//
//	DHパラメータから回転行列を計算
//
void cvmSetRotationMatrix( CvMat *rot , double alpha , double theta );

//
//	ロール・ピッチ・ヨー角から回転行列を計算
//
void cvmSetRotationMatrix( CvMat *rot , double rx , double ry , double rz);

//
//	回転行列 or 同次変換行列からロール(rz)・ピッチ(ry)・ヨー(rx)角を計算
//
void cvmCalcRPY( CvMat *t , double *rx , double *ry , double *rz ); 

//
//	回転行列同士の比較（誤差がerror以下ならを許容する）
//
bool cvmCompareRotMat( CvMat *m1 , CvMat *m2 , double error=0.05 ); 

//
//	num個の行列を全て掛けた結果を返す
//
CvMat* cvmMulAll( int num , ... );

//
//	行列を表示
//
void cvmPrint( CvMat *mat );

