void powMatrix(int **C, int **A, int k,
int ***powers,
char *computed,
int N)
{
int **T, i;
T = (int **) malloc(MAX_N * sizeof(int *));
for ( i=0; i<MAX_N; ++i )
T[i] = (int *) malloc(MAX_N * sizeof(int));
if ( (int)computed[k] ) cpyMatrix(C, A, N);
else {
if ( k==1 )
cpyMatrix(powers[k], A, N);
else if ( k%2==0 ) {
powMatrix(T, A, k/2, powers, computed, N);
mulMatrix(powers[k], T, T, N);
}
else {
powMatrix(T, A, k-1, powers, computed, N);
mulMatrix(powers[k], A, T, N);
}
computed[k] = 1;
cpyMatrix(C, powers[k], N);
}
for ( i=0; i<MAX_N; ++i )
free(T[i]);
free(T);
}
void Scene::draw() const
{
glPushMatrix();
mulMatrix(translationMatrix);
mulMatrix(rotationMatrix);
object.draw();
glPopMatrix();
}
void
gaussianEstimator_Pred ( Matrix *xEst, Matrix *CEst, Matrix *U, Matrix *Cw, Matrix (*afun)(Matrix m, float t), float *dt, Matrix *m_opt)
{
float D = elem(sizeOfMatrix(*m_opt),0,1)+1; //printf("%f\n", D);
float w_opt = 1/D; //printf("%f\n", w_opt);
float Nx = elem(sizeOfMatrix(*xEst),0,0); //printf("%f\n", Nx);
float d = Nx*(D-1) + 1; //printf("%f\n", d);
float w = 1/d; //printf("%f\n", w);
/* Eigenvectors, Eigenvalues */
int dimC = elem ( sizeOfMatrix(*CEst), 0, 0 );
Matrix Vec = zeroMatrix(dimC, dimC);
Matrix Val = zeroMatrix(dimC, dimC);
eig ( CEst, &Vec, &Val );
/* m1 = vec*sqrtf(val) */
int i;
for ( i = 0; i < dimC; ++i )
setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
Matrix m1 = mulMatrix(Vec, Val);
freeMatrix(Vec);
freeMatrix(Val);
/* rotate & scale samples: m = m1*S */
Matrix m = scaledSamplePoints(m1, *m_opt);
/* x = x*ones(1,d) */
Matrix x = fillMatrix(*xEst, d);
/* shift samples: m = m + x */
m = addMatrix(m, x); //printMatrix(m);
/* afun */
/* Prediction: mean
xPredDiracs = feval(afun,m, [], [], t);
xPred = w*sum(xPredDiracs, 2);*/
Matrix xPredDiracs = (*afun) (m, *dt); //printMatrix(xPredDiracs);
Matrix xPredDiracsSum = sumMatrix(xPredDiracs, 2); //printMatrix(xPredDiracsSum);
Matrix xPred = mulScalarMatrix(w, xPredDiracsSum); //printMatrix(xPred);
//mxDiracs = xPredDiracs-repmat(xPred, 1, d);
//CPred = w_opt*mxDiracs*mxDiracs';
Matrix mxDiracs = subMatrix(xPredDiracs, fillMatrix(xPred, d)); //printMatrix(mxDiracs);
Matrix CPred = mulScalarMatrix(w_opt, mulMatrix(mxDiracs, transposeMatrix(mxDiracs))); //printMatrix(CPred);
//RETURN
*xEst = xPred; //printMatrix(*xEst);
*CEst = CPred; //printMatrix(*CEst);
freeMatrix(m);
freeMatrix(xPredDiracs);
freeMatrix(xPredDiracsSum);
}
/**
* @brief Overrides * operator for IntMatrix to multiply a matrix by current matrix.
* @param IntMatrix we wish to multiply by the current matrix.
* @return a new IntMatrix we created
*/
const IntMatrix IntMatrix :: operator*(const IntMatrix &other) const
{
// copy our matrix, use *= to multiply it by other, then return it
IntMatrix mulMatrix(*this);
mulMatrix *= other;
return mulMatrix;
}
int main(void)
{
init();
matrix_data_t m = {0};
filesToMatrix(&m, "A.txt", "B.txt");
/* A*B=C using thread pool */
mulMatrix(&m);
/* return codes of threads */
printMatrix(&m.A);
printf("---\n");
printMatrix(&m.B);
printf("---\n");
printMatrix(&m.C);
printf("---\n");
/* mutex use show off */
/* VERBOSE fprintf(stderr, "Sum C with threads\n"); */
sumCMatrixElements(&m);
printf("Sum = %f\n", theSum);
freeMatrix(&m.A);
freeMatrix(&m.B);
freeMatrix(&m.C);
return 0;
}
// マトリクス乗算
int
CKLBLuaLibMatrix::luaMulMatrix(lua_State * L)
{
CLuaState lua(L);
int argc = lua.numArgs();
if(argc < 3) { // 対象となるmatrixが二つ以上なければnilを返す
lua.retBoolean(false);
return 1;
}
MATRIX * ans = getMatPointer(lua, 1);;
MATRIX * a = getMatPointer(lua, 2);
MATRIX * b = getMatPointer(lua, 3);
if(!ans || !a || !b) {
lua.retBoolean(false);
return 1;
}
//
// 指定された二つ以上のmatrixを、コピーを行わずに順次掛け合わせていく
//
// matrixが3つ以上ある場合
MATRIX tmp[2]; // どこともつながらないテンポラリ
int idx = 0;
// 最初の二つを乗算する
mulMatrix(&tmp[idx], a, b);
// その結果に対し、残りがあれば最後の一つ手前まで順に掛け合わせていく
for(int i = 4; i <= argc; i++) {
idx = 1 - idx;
a = getMatPointer(lua, i);
if(!a) {
lua.retBoolean(false);
return 1;
}
mulMatrix(&tmp[idx], &tmp[1 - idx], a);
}
// tmp[idx]に最後の一つをかけた結果をansに書き出す
for(int i = 0; i < 16; i++) {
ans->m[i] = tmp[idx].m[i];
}
lua.retBoolean(true);
return 1;
}
int main(int argc, char** argv)
{
/* printf("argc=%d\n", argc);
int ss;
for(ss = 0; ss < argc; ss++)
printf("argv#%d=%s\n", ss, argv[ss]);
*/
int N;
scanf("%d", &N);
matrix *M1 = malloc(sizeof(matrix)), *M2 = malloc(sizeof(matrix));
mallocMatrix(M1, N, N);
mallocMatrix(M2, N, N);
fillMatrix(M1);
#ifdef DEBUG
fprintf(stderr, "A=\n");
printMatrix(M1);
#endif
getInverseCopy(M1, M2, NULL);
#ifdef DEBUG
fprintf(stderr, "A^-1=\n");
printMatrix(M2);
#else
if(argc == 1 || (argc >= 2 && argv[1][0] == 'i'))
{
printf("%d\n", M2->R);
printMatrix(M2);
}
#endif
matrix *M3 = malloc(sizeof(matrix));
mallocMul(M1, M2, M3);
mulMatrix(M1, M2, M3);
#ifdef DEBUG
fprintf(stderr, "A*A^-1=\n");
printMatrix(M3);
#else
if(argc >= 2 && argv[1][0] == 'e')
{
printf("%d\n", M3->R);
printMatrix(M3);
}
#endif
freeMatrix(M1);
freeMatrix(M2);
return(0);
}
double* Krylov::getPoly ()
{
double** Q = new double* [size];
for (int i=0;i<size;i++)
{
Q[i] = new double[size+1];
}
double* c = new double [size];
for (int i=0;i<size;i++)
{
c[i]= c_0[i];
}
for (int i=0;i<size+1;i++)
{
for (int j=0;j<size;j++)
{
Q[j][((size+1)+size-1-i)%(size+1)] = c[j];
}
c = mulMatrix (c);
}
Gauss gauss = Gauss(Q,size);
int m = 0;
double* p = gauss.solve (m);
if (m==size)
{
return p;
}
else
{
printf ("Only %d steps on get Poly\n",m);
}
for (int i=0;i<size;i++) delete [] Q[i];
delete [] Q;
delete [] c;
return NULL;
}
void
gaussianEstimator_Est (Matrix *xEst, Matrix *CEst, Matrix *y, Matrix *Cv, Matrix (*hfun)(Matrix m), Matrix *m_opt)
{
//printMatrix(*xEst);
//printMatrix(*CEst);system("PAUSE");
Matrix tmp = sizeOfMatrix(*m_opt);
float D = elem(tmp,0,1)+1; //printf("%f\n", D);
freeMatrix(tmp);
float w_opt = 1/D; //printf("%f\n", w_opt);
tmp = sizeOfMatrix(*xEst);
float Nx = elem(tmp,0,0); // printf("%f\n", Nx);
freeMatrix(tmp);
float d = Nx*(D-1) + 1; //printf("%f\n", d);
float w = 1/d; // printf("%f\n", w);system("PAUSE");
// Eigenvectors, Eigenvalues
tmp = sizeOfMatrix(*CEst);
int dimC = elem ( tmp, 0, 0 );
freeMatrix(tmp);
Matrix Vec = zeroMatrix(dimC, dimC);
Matrix Val = zeroMatrix(dimC, dimC);
eig ( CEst, &Vec, &Val ); //printMatrix(Vec);printMatrix(Val);system("PAUSE");
// m1 = vec*sqrtf(val)
int i;
for ( i = 0; i < dimC; ++i )
setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
Matrix m1 = mulMatrix(Vec, Val); //printMatrix(m1); system("PAUSE");
freeMatrix(Vec);
freeMatrix(Val);
//* rotate & scale samples: m = m1*S
Matrix m = scaledSamplePoints(m1, *m_opt); // printMatrix(m); system("PAUSE");
Matrix mxDiracs = mulScalarMatrix(1, m);
//* x = x*ones(1,d)
Matrix x = fillMatrix(*xEst, d);
// shift samples: m = m + x
tmp = addMatrix(m, x);
appMatrix(m, 0, m->height-1, 0, m->width-1, tmp, 0, tmp->height-1, 0, tmp->width-1 ) ; //printMatrix(m);
freeMatrix(tmp);
//% Predicted Measurements
//* hfun
// yPredDiracs = feval(hfun, m, [], [], t);
// yPred = w*sum(yPredDiracs, 2);
Matrix yPredDiracs = (*hfun) (m); //printMatrix(yPredDiracs );
Matrix yPredDiracsSum = sumMatrix(yPredDiracs, 2);
Matrix yPred = mulScalarMatrix(w, yPredDiracsSum);
// myDiracs = yPredDiracs-repmat(yPred, 1, d);
tmp = fillMatrix(yPred, d);
Matrix myDiracs = subMatrix(yPredDiracs, tmp);
freeMatrix(tmp);
//* CPred = w_opt*mxDiracs*mxDiracs';
// Matrix CPred = mulScalarMatrix( w_opt, mulMatrix(mxDiracs, transposeMatrix(mxDiracs)) );
// Matrix CPred = *CEst;
// Cxy = w_opt*mxDiracs*myDiracs';
Matrix tmp1 = transposeMatrix(myDiracs);
Matrix tmp2 = mulMatrix(mxDiracs, tmp1);
Matrix Cxy = mulScalarMatrix( w_opt, tmp2);
freeMatrix(tmp1);
freeMatrix(tmp2);
// Cy = w_opt*myDiracs*myDiracs'+Cv;
tmp1 = transposeMatrix(myDiracs);
tmp2 = mulMatrix(myDiracs, tmp1);
Matrix tmp3 = mulScalarMatrix( w_opt, tmp2);
Matrix Cy = addMatrix( tmp3 , *Cv );
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
// K = Cxy / Cy;
tmp = invertCovMatrix(Cy);
Matrix K = mulMatrix( Cxy, tmp);
freeMatrix(tmp);
// I = y - yPred;
Matrix I = subMatrix( *y, yPred );
// xEst = xPred + K*I;
tmp = mulMatrix( K, I );
Matrix tmp23 = addMatrix( *xEst, tmp);
appMatrix(*xEst,0,5,0,0, tmp23,0,5,0,0);
freeMatrix(tmp);
// CEst = CPred - K*Cy*K';
tmp1 = mulMatrix(K, Cy);
tmp2 = transposeMatrix(K);
tmp3 = mulMatrix( tmp1, tmp2);
Matrix tmp24 = subMatrix(*CEst, tmp3);
appMatrix(*CEst,0,5,0,5, tmp24,0,5,0,5);
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
freeMatrix(tmp24);
freeMatrix(m1);
freeMatrix(m);
freeMatrix(mxDiracs);
freeMatrix(x);
freeMatrix(yPredDiracs);
freeMatrix(yPredDiracsSum);//
freeMatrix(yPred);//
freeMatrix(myDiracs);
freeMatrix(Cxy);
freeMatrix(Cy);
freeMatrix(K);//
freeMatrix(I);//
freeMatrix(tmp23);
}
void
gaussianEstimator_Pred_decomp ( Matrix *xEst, Matrix *CEst, Matrix *U, Matrix *Cw, float *dt, Matrix *m_opt)
{
float r;
Matrix sizeMopt;
Matrix xn = zeroMatrix(3,1);
Matrix Cn = zeroMatrix(3,3);
Matrix xl = zeroMatrix(9,1);
Matrix Cl = zeroMatrix(9,9);
Matrix Cnl = zeroMatrix(3,9);
Matrix Cnl_T;
Matrix Cn_i;
Matrix CLN;
Matrix sizeCn;
Matrix Vec;
Matrix Val;
Matrix m1;
Matrix m;
Matrix x;
Matrix A;
Matrix Hi = zeroMatrix(12,9);
Matrix Cy = zeroMatrix(12, 12);
Matrix muy = zeroMatrix(12, 1);
Matrix zeros33 = zeroMatrix(3,3);
Matrix eye33 = unitMatrix(3,3);
Matrix Mat;
Matrix H;
Matrix gi = zeroMatrix(12,1);
Matrix Rot_vec = zeroMatrix(3,1);
Matrix mui;
Matrix muiy;
Matrix Ciy;
Matrix tmp;
Matrix tmp1;
Matrix tmp2;
Matrix tmp3;
Matrix tmp4;
Matrix tmp5;
Matrix tmp6;
Matrix tmp7;
Matrix tmp8;
Matrix tmpHi;
sizeMopt = sizeOfMatrix(*m_opt); //printf("%f\n",*dt);
float D = elem(sizeMopt,0,1)+1; //printf("%f\n", D);
freeMatrix(sizeMopt);
float w_opt = 1/D; //printf("%f\n", w_opt);
float Nx = 3;
float d = Nx*(D-1) + 1; //printf("%f\n", d);
float w = 1/d; //printf("%f\n", w);
//xn = xEst(4:6); % Rotation vector
appMatrix(xn, 0, 2, 0, 0, *xEst, 3, 5, 0, 0); //printMatrix(xn);system("PAUSE");
//Cn = CEst(4:6,4:6);
appMatrix(Cn, 0, 2, 0, 2, *CEst, 3, 5, 3, 5); //printMatrix(Cn);system("PAUSE");
//xl = [xEst(1:3) ; xEst(7:12)]; % Translation, angular velocity, linear velocity
appMatrix(xl, 0, 2, 0, 0, *xEst, 0, 2, 0, 0);
appMatrix(xl, 3, 8, 0, 0, *xEst, 6, 11, 0, 0); //printMatrix(xl);system("PAUSE");
//Cl = [CEst(1:3,1:3) CEst(1:3,7:12);
// CEst(7:12,1:3) CEst(7:12,7:12)] ;
appMatrix(Cl, 0, 2, 0, 2, *CEst, 0, 2, 0, 2);
appMatrix(Cl, 0, 2, 3, 8, *CEst, 0, 2, 6, 11);
appMatrix(Cl, 3, 8, 0, 2, *CEst, 6, 11, 0, 2);
appMatrix(Cl, 3, 8, 3, 8, *CEst, 6, 11, 6, 11); //printMatrix(Cl);system("PAUSE");
//Cnl = [CEst(4:6,1:3) CEst(4:6,7:12)];
appMatrix(Cnl, 0, 2, 0, 2, *CEst, 3, 5, 0, 2);
appMatrix(Cnl, 0, 2, 3, 8, *CEst, 3, 5, 6, 11); //printMatrix(Cnl);system("PAUSE");
//CLN = Cl - Cnl'*inv(Cn)*Cnl;
Cnl_T = transposeMatrix(Cnl);
// printMatrix(Cn);system("PAUSE");
Cn_i = invertCovMatrix(Cn); //printMatrix(Cn_i);system("PAUSE");
tmp = mulMatrix( Cnl_T, Cn_i);
tmp7 = mulMatrix(tmp, Cnl);
CLN = subMatrix ( Cl, tmp7); //printMatrix(CLN);system("PAUSE");
freeMatrix(tmp);
freeMatrix(tmp7);
// Eigenvectors, Eigenvalues
sizeCn = sizeOfMatrix(Cn);
int dimC = elem ( sizeCn, 0, 0 );
freeMatrix(sizeCn);
Vec = zeroMatrix(dimC, dimC);
Val = zeroMatrix(dimC, dimC);
eig ( &Cn, &Vec, &Val ); //printMatrix(Cn);printMatrix(Vec);printMatrix(Val);system("PAUSE");
// m1 = vec*sqrtf(val)
int i;
for ( i = 0; i < dimC; ++i )
setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
m1 = mulMatrix(Vec, Val); //printMatrix(m1);system("PAUSE");
// rotate & scale samples: m = m1*S
m = scaledSamplePoints(m1, *m_opt); //printMatrix(m);system("PAUSE");
// x = x*ones(1,d)
x = fillMatrix(xn, d);
// shift samples: m = m + x
tmp = addMatrix(m, x);
appMatrix(m, 0, m->height-1, 0, m->width-1, tmp, 0, tmp->height-1, 0, tmp->width-1 ); //printMatrix(m);system("PAUSE");
freeMatrix(tmp);
//A = [[eye(3,3),t*eye(3,3)];[zeros(3,3),eye(3,3)]];
A = unitMatrix(6,6);
setElem(A, 0, 3, *dt);
setElem(A, 1, 4, *dt);
setElem(A, 2, 5, *dt); //printMatrix(A);system("PAUSE");
for (i=0; i<d; i++)
{
//gi = [zeros(3,1); m(:,i); zeros(6,1)];
setElem(gi, 3, 0, elem(m, 0, i));
setElem(gi, 4, 0, elem(m, 1, i));
setElem(gi, 5, 0, elem(m, 2, i)); //printMatrix(gi);system("PAUSE");
//Rot_vec = m(:,i);
setElem(Rot_vec, 0, 0, elem(m, 0, i));
setElem(Rot_vec, 1, 0, elem(m, 1, i));
setElem(Rot_vec, 2, 0, elem(m, 2, i)); //printMatrix(Rot_vec);system("PAUSE");
//r = norm(Rot_vec);
r = sqrtf( powf((elem(Rot_vec,0,0)),2) + powf((elem(Rot_vec,1,0)),2) + powf((elem(Rot_vec,2,0)),2) ); //printf("%f\n",r);
H = zeroMatrix(3,3);
if (fmod(r, 2*pi) == 0)
{
Mat = unitMatrix(3,3);
}
else
{
// build skew symmetric Matrix
setElem(H, 0, 1, -elem(Rot_vec,2,0));
setElem(H, 0, 2, elem(Rot_vec,1,0));
setElem(H, 1, 0, elem(Rot_vec,2,0));
setElem(H, 1, 2, -elem(Rot_vec,0,0));
setElem(H, 2, 0, -elem(Rot_vec,1,0));
setElem(H, 2, 1, elem(Rot_vec,0,0)); //printMatrix(H);system("PAUSE");
// Bortz equation
// Mat = eye(3,3) + 0.5*H + (1- r*sin(r)/( 2*(1-cos(r))))/r^2*H*H;
// already declared Mat = unitMatrix(3,3);
tmp1 = mulScalarMatrix(0.5, H);
tmp4 = addMatrix( eye33 , tmp1 );
tmp2 = mulMatrix(H, H);
tmp3 = mulScalarMatrix( (1-(r*sin(r)/(2*(1-cos(r)))))/powf(r,2), tmp2);
Mat = addMatrix( tmp4, tmp3);
//printMatrix(Mat);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
freeMatrix(tmp4);
}
//Hi = [[A(1:3,1:3) zeros(3,3) A(1:3,4:6)];
// [zeros(3,3), t*Mat, zeros(3,3)];
// [zeros(3,3), eye(3,3), zeros(3,3)];
// [A(4:6,1:3),zeros(3,3), A(4:6,4:6)]];
appMatrix( Hi, 0, 2, 0, 2, A, 0, 2, 0, 2 );
appMatrix( Hi, 0, 2, 3, 5, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 0, 2, 6, 8, A, 0, 2, 3, 5 );
appMatrix( Hi, 3, 5, 0, 2, zeros33, 0, 2, 0, 2 );
tmpHi = mulScalarMatrix(*dt, Mat);
appMatrix( Hi, 3, 5, 3, 5, tmpHi, 0, 2, 0, 2 );
freeMatrix(tmpHi);
appMatrix( Hi, 3, 5, 6, 8, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 6, 8, 0, 2, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 6, 8, 3, 5, eye33, 0, 2, 0, 2 );
appMatrix( Hi, 6, 8, 6, 8, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 9, 11, 0, 2, A, 3, 5, 0, 2 );
appMatrix( Hi, 9, 11, 3, 5, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 9, 11, 6, 8, A, 3, 5, 3, 5 ); //printMatrix(Hi);system("PAUSE");
// mui = xl + Cnl'*inv(Cn)*(m(:,i)-xn); //m(:,i) -> Rot_vec
tmp = mulMatrix(Cnl_T, Cn_i );
tmp1 = subMatrix(Rot_vec, xn);
tmp2 = mulMatrix(tmp, tmp1);
mui = addMatrix(xl, tmp2);
freeMatrix(tmp);
freeMatrix(tmp1);
freeMatrix(tmp2); //printMatrix(mui);system("PAUSE");
// muiy = gi + Hi * mui;
tmp = mulMatrix(Hi, mui);
muiy = addMatrix( gi, tmp); //printMatrix(muiy);system("PAUSE");
freeMatrix(tmp);
// Ciy = Hi *CLN *Hi';
tmp1 = mulMatrix(Hi, CLN);
tmp2 = transposeMatrix(Hi);
Ciy = mulMatrix( tmp1, tmp2); //printMatrix(Ciy);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
// Cy = Cy + (w*Ciy + w_opt*muiy*muiy');
tmp3 = mulScalarMatrix(w, Ciy);
tmp1 = transposeMatrix(muiy);
tmp2 = mulMatrix(muiy, tmp1);
tmp4 = mulScalarMatrix( w_opt, tmp2 );
tmp5 = addMatrix( tmp3, tmp4 );
tmp6 = addMatrix( Cy, tmp5);
appMatrix(Cy,0,Cy->height-1,0,Cy->width-1,tmp6, 0,tmp6->height-1,0,tmp6->width-1); //printMatrix(Cy);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
freeMatrix(tmp4);
freeMatrix(tmp5);
freeMatrix(tmp6);
// muy = muy + w*muiy;
tmp = mulScalarMatrix( w, muiy );
tmp2 = addMatrix( muy, tmp );
appMatrix(muy,0,muy->height-1,0,muy->width-1, tmp2, 0, tmp2->height-1, 0, tmp2->width-1); //printMatrix(muy);system("PAUSE");
freeMatrix(tmp);
freeMatrix(tmp2);
freeMatrix(H);
freeMatrix(Mat);
freeMatrix(mui);//
freeMatrix(muiy);//
freeMatrix(Ciy);
}
appMatrix(*xEst, 0, 11, 0, 0, muy, 0, 11, 0, 0 ); //printMatrix(muy);system("PAUSE");
//CEst = Cy - muy*muy' * w_opt/w + Cw;
tmp1 = transposeMatrix(muy);
tmp2 = mulMatrix(muy, tmp1);
tmp5 = mulScalarMatrix( w_opt/w, tmp2 );
tmp6 = subMatrix(Cy, tmp5);
tmp8 = addMatrix( tmp6, *Cw); //printMatrix(*CEst);system("PAUSE");
appMatrix(*CEst,0,11,0,11, tmp8, 0,11,0,11 ); //printMatrix(tmp8);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp5);
freeMatrix(tmp6);
freeMatrix(tmp8);
freeMatrix(muy);//
freeMatrix(zeros33);//
freeMatrix(Vec);
freeMatrix(Val);
freeMatrix(Cy);
freeMatrix(xn);
freeMatrix(Cn);
freeMatrix(xl);
freeMatrix(Cl);//
freeMatrix(Cnl);
freeMatrix(Cnl_T);
freeMatrix(Cn_i);
freeMatrix(CLN);//
freeMatrix(m1);
freeMatrix(m);//
freeMatrix(x);
freeMatrix(A);
freeMatrix(eye33);
freeMatrix(Hi);
freeMatrix(gi);
freeMatrix(Rot_vec);
} /* End gaussianPred_decomp */
int main( int argc, char *argv[] )
{
unsigned int frames = 0;
int res;
Mat4 projection_matrix;
Mat4 viewMat;
Mat4 rotMat;
Mat4 modelMat;
float aspect;
float dw, dh;
int debug_flag = 0;
bcm_host_init();
res = WinCreate( &g_sc );
if( !res )
{
printf( "Create window error!\n" );
return 0;
}
res = SurfaceCreate(&g_sc);
if( !res )
{
printf( "Create surface error!\n" );
return 0;
}
makeUnit(&viewMat);
aspect = (float)g_sc.width / (float)g_sc.height;
//makeProjectionMatrix(&g_sp.VpMatrix, 1, 65536, 53, aspect);
//setPosition(&viewMat, 0, -4, -24 );
dw = 0.5f * aspect;
dh = 0.5f;
perspectiveMatrix( -dw, +dw, -dh, +dh, 1, 160, &g_sp.VpMatrix );
setPosition(&viewMat, 0, -12, -32 );
//print_mat4( &g_sp.VpMatrix );
mulMatrix(&g_sp.VpMatrix, &g_sp.VpMatrix, &viewMat);
makeUnit(&modelMat);
mulMatrix( &projection_matrix, &modelMat, &g_sp.VpMatrix );
makeUnit(&rotMat);
setRotationY(&rotMat, 0.5); /* 30 degree/sec */
pmd_file = NULL;
vmd_file = NULL;
if( argc < 2 )
{
printf( "File Error!\n" );
printf( "Argment1 : Pmd or Pmx file.\n" );
printf( "Argment2 : Vmd file.\n" );
return 0;
}
if( argc > 1 )
pmd_file = argv[ 1 ];
if( argc > 2 )
vmd_file = argv[ 2 ];
init();
if( argc > 3 && argv[ 3 ] )
{
if( strncmp( argv[ 3 ], "-D", 2 ) == 0 )
debug_flag = 1;
}
//print_mat4( &projection_matrix );
glEnable(GL_DEPTH_TEST);
glClearColor( 0.0f, 0.0f, 0.0f, 1.0f );
Mat4 delta_mat;
makeUnit( &delta_mat );
if( argc > 4 )
{
int i, j;
char cmd_string[ 255 ] = { 0 };
for( i = 3; i < argc; i ++ )
{
int length = 0;
strcpy( cmd_string, argv[ i ] );
puts( cmd_string );
length = strlen( cmd_string );
cmd_string[ length ] = ' ';
cmd_string[ length + 1 ] = '&';
cmd_string[ length + 2 ] = '\0';
system( cmd_string );
}
}
fps = new Fps();
fps->set_fps( _fps_ );
/* 1200frame / 60fps = 20sec */
//while( frames < 1200 )
while( !p->get_vmd( 0 )->is_end() || ( !vmd_file && frames < 160 ) )
//while( 1 )
{
Mat4 pl_matrix;
glViewport(0, 0, g_sc.width, g_sc.height);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
/* X Rotation */
mulMatrix( &pl_matrix, &projection_matrix, &delta_mat );
//glUniformMatrix4fv(g_sp.uModelMatrix, 1, GL_FALSE, modelMat.m);
p->set_projection_matrix( pl_matrix.m );
draw();
/*
char c = get_keyboard();
makeUnit( &delta_mat );
switch( c )
{
case 'w': setPosition( &delta_mat, 0, 0, +4 ); break;
case 's': setPosition( &delta_mat, 0, 0, -4 ); break;
case 'a': setRotationY( &delta_mat, +0.5 ); break;
case 'd': setRotationY( &delta_mat, -0.5 ); break;
}
*/
eglSwapBuffers(g_sc.display, g_sc.surface);
frames ++;
//glutTimerFunc( fps->get_wait_time() * 1000.0f, timer, 0 );
usleep( 1600 );
if( debug_flag )
fps->draw();
}
printf( "Ending process!\n" );
end();
return 0;
}
void TransformMatrix::mulMatrixFromRight(TTransformMatrix mm) {
TTransformMatrix tmp;
copyMatrix(tmp, matrix_);
mulMatrix(matrix_, tmp, mm);
}
void TransformMatrix::mulMatrixFromLeft(TTransformMatrix mm) {
TTransformMatrix tmp;
copyMatrix(tmp, matrix_);
mulMatrix(matrix_, mm, tmp);
}
int main()
{
char cmd[1000]={},split[5][100]={};
char *token;
int cmdSpt=0,i,returnA,returnB,returnTar,returnFunc,row,col,size;
double mul;
FILE *fileA=NULL,*fileB=NULL,*fileTar=NULL;
matrix matA,matB,matTar;
//prints welcome messages
system("cls");
printf("\n\n===Matrix manipulation program===\n\n");
printf("Type <help> for command list and usage\n\n");
do
{
printf("MATRIX> "); //command prompt
rewind(stdin); //rewind everything
scanf("%[^\n]",cmd); //get command (with spaces)
cmdSpt=0; //set sub command count to 0
token=strtok(cmd," "); //partition command to subcommand with spaces
while(token!=NULL&&cmdSpt<=5)
{
strcpy(split[cmdSpt],token); //save subcommands to split[]
cmdSpt++; //increase sub command count
token=strtok(NULL," ");
}
for(i=0;i<strlen(split[0]);i++) //set command to lowercase
split[0][i]=tolower(split[0][i]);
if(strcmp(split[0],"show")==0&&cmdSpt==2) //show command
{
returnA=openFile(&fileA,split[1],0); //call openFile() to open file to show
if(returnA==1) //failed to open
printf("Error: file %s failed to open.\n\n",split[1]);
else
{
matA=readMatrix(fileA); //read matrix and save to matA
showMatrix(matA,split[1]); //show matrix matA
}
}
else if(strcmp(split[0],"create")==0&&cmdSpt==4) //create command
{
returnA=openFile(&fileA,split[3],1); //call openFile() to create a new file
{
if(returnA==1) //failed to open
printf("Error: file %s failed to open.\n\n",split[3]);
else if(sscanf(split[1],"%d",&row)!=1||sscanf(split[2],"%d",&col)!=1) //incorrect matrix size entered
printf("Error: invalid matrix size.\n\n");
else if(row>50||col>50) //size too large
printf("Error: maximum matrix size is 50x50.\n\n");
else
{
createMatrix(row,col,fileA); //create a new matrix with specified size
printf("%dx%d matrix was successfully saved to %s.\n\n",row,col,split[3]);
}
}
}
else if(strcmp(split[0],"copy")==0&&cmdSpt==3) //copy command
{
returnA=openFile(&fileA,split[1],0); //open source and destination files
returnB=openFile(&fileTar,split[2],1);
if(returnA==1) //failed to open source
printf("Error: file %s failed to open.\n\n",split[1]);
if(returnB==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[2]);
if(returnA==0&&returnB==0) //passed
{
matA=readMatrix(fileA); //read source file
writeMatrix(matA,fileTar); //write to destination file
printf("Matrix from %s is successfully copied to %s.\n\n",split[1],split[2]);
}
}
else if(strcmp(split[0],"add")==0&&cmdSpt==4) //add command
{
returnA=openFile(&fileA,split[1],0); //open source and destination files
returnB=openFile(&fileB,split[2],0);
returnTar=openFile(&fileTar,split[3],1);
if(returnA==1) //failed to open source A
printf("Error: file %s failed to open.\n\n",split[1]);
if(returnB==1) //failed to open source B
printf("Error: file %s failed to open.\n\n",split[2]);
if(returnTar==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[3]);
if(returnA==0&&returnB==0&&returnTar==0) //passed
{
matA=readMatrix(fileA); //read both sources files
matB=readMatrix(fileB);
returnFunc=addMatrix(matA,matB,fileTar); //add matrices and save to destination
if(returnFunc==0) //success
{
printf("New matrix is successfully saved to %s.\n\n",split[3]);
matTar=readMatrix(fileTar);
printf("===Base matrix===\n");
showMatrix(matA,split[1]);
printf("===Adder matrix===\n");
showMatrix(matB,split[2]);
printf("===Result matrix===\n");
showMatrix(matTar,split[3]);
}
else //dimensions not matched
printf("Error: matrix dimensions unmatched.\n\n");
}
}
else if((strcmp(split[0],"subtract")==0||strcmp(split[0],"sub")==0)&&cmdSpt==4) //subtract command
{
returnA=openFile(&fileA,split[1],0); //open source and destination files
returnB=openFile(&fileB,split[2],0);
returnTar=openFile(&fileTar,split[3],1);
if(returnA==1) //failed to open source A
printf("Error: file %s failed to open.\n\n",split[1]);
if(returnB==1) //failed to open source B
printf("Error: file %s failed to open.\n\n",split[2]);
if(returnTar==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[3]);
if(returnA==0&&returnB==0&&returnTar==0) //passed
{
matA=readMatrix(fileA); //read both sources files
matB=readMatrix(fileB);
returnFunc=subMatrix(matA,matB,fileTar); //subtract matrices and save to destination
if(returnFunc==0) //success
{
printf("New matrix is successfully saved to %s.\n\n",split[3]);
matTar=readMatrix(fileTar);
printf("===Base matrix===\n");
showMatrix(matA,split[1]);
printf("===Subtractor matrix===\n");
showMatrix(matB,split[2]);
printf("===Result matrix===\n");
showMatrix(matTar,split[3]);
}
else //dimensions not matched
printf("Error: matrix dimensions unmatched.\n\n");
}
}
else if((strcmp(split[0],"multiply")==0||strcmp(split[0],"mul")==0)&&cmdSpt==4) //multiply command
{
returnA=openFile(&fileA,split[1],0); //open source and destination files
returnB=openFile(&fileB,split[2],0);
returnTar=openFile(&fileTar,split[3],1);
if(returnA==1) //failed to open source A
printf("Error: file %s failed to open.\n\n",split[1]);
if(returnB==1) //failed to open source B
printf("Error: file %s failed to open.\n\n",split[2]);
if(returnTar==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[3]);
if(returnA==0&&returnB==0&&returnTar==0) //passed
{
matA=readMatrix(fileA); //read both sources files
matB=readMatrix(fileB);
returnFunc=mulMatrix(matA,matB,fileTar); //multiply matrices and save to destination
if(returnFunc==0) //success
{
printf("New matrix is successfully saved to %s.\n\n",split[3]);
matTar=readMatrix(fileTar);
printf("===Base matrix===\n");
showMatrix(matA,split[1]);
printf("===Multiplier matrix===\n");
showMatrix(matB,split[2]);
printf("===Result matrix===\n");
showMatrix(matTar,split[3]);
}
else //dimensions not matched
printf("Error: matrix dimensions unmatched.\n\n");
}
}
else if((strcmp(split[0],"mulscalar")==0||strcmp(split[0],"muls")==0)&&cmdSpt==4) //multiply scalar command
{
returnA=openFile(&fileA,split[2],0); //open source and destination files
returnTar=openFile(&fileTar,split[3],1);
returnFunc=sscanf(split[1],"%lf",&mul); //convert multiplier to double
if(returnA==1) //failed to open source A
printf("Error: file %s failed to open.\n\n",split[2]);
if(returnTar==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[3]);
if(returnFunc!=1) //cannot convert multiplier to double (invalid)
printf("Error: invalid multiplier.\n\n");
if(returnA==0&&returnB==0&&returnFunc==1) //passed
{
matA=readMatrix(fileA); //read source file
mulScaMatrix(matA,mul,fileTar); //multiply with scalar and save to destination
printf("New matrix is successfully saved to %s.\n\n",split[3]);
matTar=readMatrix(fileTar);
printf("===Base matrix===\n");
showMatrix(matA,split[2]);
printf("Multiply with %g\n\n",mul);
printf("===Result matrix===\n");
showMatrix(matTar,split[3]);
}
}
else if((strcmp(split[0],"transpose")==0||strcmp(split[0],"trans")==0)&&cmdSpt==3) //transpose command
{
returnA=openFile(&fileA,split[1],0); //open source and destination files
returnTar=openFile(&fileTar,split[2],1);
if(returnA==1) //failed to open source A
printf("Error: file %s failed to open.\n\n",split[1]);
if(returnTar==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[2]);
if(returnA==0&&returnTar==0) //passed
{
matA=readMatrix(fileA); //read source file
transMatrix(matA,fileTar); //transpose matrix and save to destination
printf("New matrix is successfully saved to %s.\n\n",split[2]);
}
}
else if((strcmp(split[0],"identity")==0||strcmp(split[0],"iden")==0)&&cmdSpt==3) //identity matrix command
{
returnTar=openFile(&fileTar,split[2],1); //open destination file
returnFunc=sscanf(split[1],"%d",&size); //convert size to integer
if(returnTar==1) //failed to open destination
printf("Error: file %s failed to open.\n\n",split[2]);
else if(returnFunc!=1) //failed to convert size to integer (invalid)
printf("Error: invalid matrix size.\n\n");
else if(size>50) //size too large
printf("Error: maximum matrix size is 50x50.\n\n");
else
{
idenMatrix(size,fileTar); //create identity matrix with specified size and save to destination
printf("Identity matrix of size %d is successfully saved to %s.\n\n",size,split[2]);
}
}
else if(strcmp(split[0],"help")==0&&cmdSpt==1) //help command
{
displayHelp(); //display help
}
else if((strcmp(split[0],"clear")==0||strcmp(split[0],"cls")==0)&&cmdSpt==1) //clear screen command
{
system("cls"); //clear screen
}
else if((strcmp(split[0],"exit")==0||strcmp(split[0],"end")==0)&&cmdSpt==1) //exit command
{
printf("Exit\n"); //display exit message
}
else //invalid command
{
printf("Invalid command or incorrect command syntax, type <help> for command list and usage.\n\n");
}
if(fileA!=NULL) //close all file pointers in case they weren't closed by functions
fclose(fileA);
if(fileB!=NULL)
fclose(fileB);
if(fileTar!=NULL)
fclose(fileTar);
} while(strcmp(cmd,"exit")!=0&&strcmp(cmd,"end")!=0); //loop until exit
return 0;
}
