// 単位行列
void unitMat4h(double m[VECTOR_LEN][VECTOR_LEN])
{
zeroMat4h(m);
for (int i = 0; i < VECTOR_LEN; i++)
{
m[i][i] = 1.0;
}
}
// 参考サイト: http://www.mech.tohoku-gakuin.ac.jp/rde/contents/course/robotics/coordtrans.html
void invMat4h(double im[][VEC_SIZE], double m[][VEC_SIZE]){
double det;
double u[VEC_SIZE], v[VEC_SIZE];
det = m[0][0] * m[1][1] * m[2][2] + m[1][0] * m[2][1] * m[0][2] + m[2][0] * m[0][1] * m[1][2]
- m[0][0] * m[2][1] * m[1][2] - m[2][0] * m[1][1] * m[0][2] - m[1][0] * m[0][1] * m[2][2];
if (det == 0){
printf("逆行列は存在しません\n");
}
else{
// 一応初期化
zeroMat4h(im);
// 逆行列計算
// 回転要素の計算
im[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
im[0][1] = m[0][2] * m[2][1] - m[0][1] * m[2][2];
im[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];
im[1][0] = m[1][2] * m[2][0] - m[1][0] * m[2][2];
im[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];
im[1][2] = m[0][2] * m[1][0] - m[0][0] * m[1][2];
im[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0];
im[2][1] = m[0][1] * m[2][0] - m[0][0] * m[2][1];
im[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];
for (int i = 0; i < VEC_SIZE; ++i){
for (int j = 0; j < VEC_SIZE; ++j){
im[i][j] = im[i][j] / det;
}
}
// 平行移動要素初期化
for (int i = 0; i < VEC_SIZE; ++i){
u[i] = 0.0;
v[i] = m[i][VEC_SIZE - 1];
}
u[VEC_SIZE - 1] = 1.0;
// 平行移動要素計算
for (int i = 0; i< VEC_SIZE - 1; ++i) {
for (int j = 0; j < VEC_SIZE - 1; ++j) {
u[i] += im[i][j] * v[j];
}
im[i][VEC_SIZE - 1] = -u[i];
}
im[VEC_SIZE - 1][VEC_SIZE - 1] = 1;
#ifdef DEBUG
// 結果の出力
printf("Inverse ");
printMat(im);
#endif
}
}
void translate4h(double m[][VEC_SIZE], double v[]){
zeroMat4h(m);
for (int i = 0; i< VEC_SIZE; ++i) {
m[i][3] = v[i];
m[i][i] = 1;
}
#ifdef DEBUG
printMat(m);
#endif
}
void mirrorO4h(double m[][VEC_SIZE]){
zeroMat4h(m);
m[0][0] = -1;
m[1][1] = -1;
m[2][2] = -1;
m[3][3] = 1;
#ifdef DEBUG
printMat(m);
#endif
}
void scale4h(double m[][VEC_SIZE], double a, double b, double c){
zeroMat4h(m);
m[0][0] = a;
m[1][1] = b;
m[2][2] = c;
m[3][3] = 1;
#ifdef DEBUG
printMat(m);
#endif
}
void rotateXSC4h(double m[][VEC_SIZE], double sv, double cv){
zeroMat4h(m);
m[1][1] = cv;
m[1][2] = -sv;
m[2][1] = sv;
m[2][2] = cv;
m[0][0] = m[3][3] = 1;
#ifdef DEBUG
printMat(m);
#endif
}
void rotateZ4h(double m[][VEC_SIZE], double a){
zeroMat4h(m);
m[0][0] = cos(a);
m[0][1] = -sin(a);
m[1][0] = sin(a);
m[1][1] = cos(a);
m[2][2] = m[3][3] = 1;
#ifdef DEBUG
printMat(m);
#endif
}
void setMatCol4h(double a[][VEC_SIZE], double v0[], double v1[], double v2[], double p[]){
zeroMat4h(a);
for (int i = 0; i < VEC_SIZE; ++i){
a[i][0] = v0[i];
a[i][1] = v1[i];
a[i][2] = v2[i];
a[i][3] = p[i];
}
#ifdef DEBUG
printMat(a);
#endif
}
void mulMM4h(double m01[][VEC_SIZE], double m0[][VEC_SIZE], double m1[][VEC_SIZE]){
zeroMat4h(m01);
for (int i = 0; i< VEC_SIZE; ++i) {
for (int j = 0; j < VEC_SIZE; ++j) {
for (int k = 0; k < VEC_SIZE; ++k) {
m01[i][j] += m0[i][k] * m1[k][j];
}
}
}
#ifdef DEBUG
printMat(m01);
#endif
}
void unitMat4h(double m[][VEC_SIZE]){
zeroMat4h(m);
for (int i = 0; i < VEC_SIZE; ++i){
m[i][i] = 1;
}
}
