void multiple_all_matrix(GLfloat M[][4]){ for(int i = 0; i < current_obj; ++i){ GLfloat I[4][4] = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}; multiMatrix(I, geoMatrix[i], I); multiMatrix(I, viewMatrix, I); multiMatrix(I, projMatrix, I); copyMatrix(aMVP[i], I); transMatrix(aMVP[i]); } }
int similarMatrix(float_j_t work1[][3], float_j_t work2[][3], float_j_t result[][3], int row, int col) { float_j_t res[3][3]={0,}; float_j_t res_t[3][3]={0,}; transMatrix(work1,res_t,3,3); //printMatrix(work1,3,3); //printMatrix(res_t,3,3); multiMatrix(work1,work2,res,3,3); //printMatrix(work2,3,3); multiMatrix(res,res_t,result,3,3); //printMatrix(result,3,3); return 0; }
int jumpFloor(int number) { int matrix[2][2] = { {1, 1}, {0, 0}}; int factorMatrix[2][2] = {{0, 1}, {1, 1}}; while(number) { if (number & 0x01) { multiMatrix(matrix, factorMatrix); } multiMatrix(factorMatrix, factorMatrix); number >>= 1; } return matrix[0][0]; }
void scaleAll(){ GLfloat M[4][4] = {0}; M[0][0] = scale[now]; M[1][1] = scale[now]; M[2][2] = scale[now]; M[3][3] = 1; multiMatrix(geoMatrix[now], M, geoMatrix[now]); multiple_all_matrix(M); }
void transport(GLfloat x, GLfloat y, GLfloat z, bool no_update){ GLfloat M[4][4] = {0}; for(int i = 0; i < 4; ++i) M[i][i] = 1; M[0][3] = x; M[1][3] = y; M[2][3] = z; GLfloat I[4][4] = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}; multiMatrix(geoMatrix[now], M, geoMatrix[now]); multiple_all_matrix(M); }
void scaling(GLfloat x, GLfloat y, GLfloat z){ transport(-x_center[now], -y_center[now], -z_center[now], NO_UPDATE); GLfloat M[4][4] = {0}; M[0][0] = x; M[1][1] = y; M[2][2] = z; M[3][3] = 1; GLfloat I[4][4] = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}; multiMatrix(geoMatrix[now], M, geoMatrix[now]); multiple_all_matrix(M); transport(x_center[now], y_center[now], z_center[now], NO_UPDATE); }
void Matrix::rotate(float angle, float x, float y, float z) { float sinAngle, cosAngle; float mag = sqrtf(x * x + y * y + z * z); sinAngle = sinf(angle * PI / 180.0f); cosAngle = cosf(angle * PI / 180.0f); if (mag > 0.0f) { float xx, yy, zz, xy, yz, zx, xs, ys, zs; float oneMinusCos; Matrix rotMat; x /= mag; y /= mag; z /= mag; xx = x * x; yy = y * y; zz = z * z; xy = x * y; yz = y * z; zx = z * x; xs = x * sinAngle; ys = y * sinAngle; zs = z * sinAngle; oneMinusCos = 1.0f - cosAngle; rotMat.m[0][0] = (oneMinusCos * xx) + cosAngle; rotMat.m[0][1] = (oneMinusCos * xy) - zs; rotMat.m[0][2] = (oneMinusCos * zx) + ys; rotMat.m[0][3] = 0.0F; rotMat.m[1][0] = (oneMinusCos * xy) + zs; rotMat.m[1][1] = (oneMinusCos * yy) + cosAngle; rotMat.m[1][2] = (oneMinusCos * yz) - xs; rotMat.m[1][3] = 0.0F; rotMat.m[2][0] = (oneMinusCos * zx) - ys; rotMat.m[2][1] = (oneMinusCos * yz) + xs; rotMat.m[2][2] = (oneMinusCos * zz) + cosAngle; rotMat.m[2][3] = 0.0F; rotMat.m[3][0] = 0.0F; rotMat.m[3][1] = 0.0F; rotMat.m[3][2] = 0.0F; rotMat.m[3][3] = 1.0F; multiMatrix(*this, rotMat, *this); } }
long long calculateFib(int n) { if (n < 0) { return -1; } if (n == 0) { return 0; } if (n < 3) { return 1; } if (n == 3) { return 2; } n = n - 3; vector<long long> base({2, 1, 1, 0}); vector<long long> matrix({1, 1, 1, 0}); vector<long long> result({1, 0, 1, 0}); while (n > 0) { if (n % 2 == 1) { multiMatrix(result, result, matrix); } multiMatrix(matrix, matrix, matrix); n /= 2; } multiMatrix(result, result, base); return result[n]; }
void rotate(GLfloat x, GLfloat y, GLfloat z){ transport(-x_center[now], -y_center[now], -z_center[now], NO_UPDATE); GLfloat M[3][4][4] = {0}; for(int i = 0; i < 3; ++i) for(int j = 0; j < 4; ++j) M[i][j][j] = 1; const double PI = std::atan(1.0); const double ANGLE_X = PI * x / 180.0; const double ANGLE_Y = PI * y / 180.0; const double ANGLE_Z = PI * z / 180.0; const GLfloat COS[] = { std::cos(ANGLE_X), std::cos(ANGLE_Y), std::cos(ANGLE_Z) }; const GLfloat SIN[] = { std::sin(ANGLE_X), std::sin(ANGLE_Y), std::sin(ANGLE_Z) }; for(int i = 0; i < 3; ++i){ if (i == 0){ M[i][1][1] = M[i][2][2] = COS[i]; M[i][1][2] = -SIN[i]; M[i][2][1] = SIN[i]; } else if (i == 1){ M[i][0][0] = M[i][2][2] = COS[i]; M[i][0][2] = -SIN[i]; M[i][2][0] = SIN[i]; } else { M[i][0][0] = M[i][1][1] = COS[i]; M[i][0][1] = -SIN[i]; M[i][1][0] = SIN[i]; } GLfloat I[4][4] = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}; multiMatrix(geoMatrix[now], M[i], geoMatrix[now]); multiple_all_matrix(M[i]); copyMatrix(aMVP[now], I); } transport(x_center[now], y_center[now], z_center[now], NO_UPDATE); }
int main() { int i; double *matrixa, *matrixb, *matrixc; matrixa = (double*)malloc(MS*MS*sizeof(double*)); matrixb = (double*)malloc(MS*MS*sizeof(double*)); matrixc = (double*)malloc(MS*MS*sizeof(double*)); for (i = 0; i < MS*MS; i++) { matrixa[i] = (i)*1.0; matrixb[i] = 2.0; } multiMatrix(matrixa, matrixb, matrixc); /*for (i = 0; i < MS*MS; i++) { printf("%f\t", matrixc[i]); } printf("\n");*/ return 0; }
void multiple_range(int i){ GLfloat I[4][4] = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}; multiMatrix(I, geoMatrix[i], I); multiMatrix(I, viewMatrix, I); multiMatrix(I, projMatrix, I); }
int kalman_function(sensor_struct_t* sen,float_j_t dt){ /* * * system model * omega = [ w_x w_y w_z ]^T * x = [pi theta psi]^T * used x_dot, C_inv, omega, x_k_1, x_k, dt, * * */ #define Def_x 0 #define Def_y 1 #define Def_z 2 #define Def_pi 0 #define Def_theta 1 #define Def_psi 2 //float_j_t dt; //float_j_t B[3][3]={0,}; float_j_t res[3][3]={0,}; float_j_t res2[3][3]={0,}; float_j_t omega[3][3]={0,}; omega[0][0] = sen->gyroX*DEG_TO_RAD; omega[1][0] = sen->gyroY*DEG_TO_RAD; omega[2][0] = sen->gyroZ*DEG_TO_RAD; static float_j_t x_k_1[3][3]={0,}; float_j_t x_k_p[3][3]={0,}; float_j_t pi,theta,psi; pi = x_k_1[Def_pi][0]; theta = x_k_1[Def_theta][0]; psi = x_k_1[Def_psi][0]; if(cosf(theta)==0.0){ printf("cosf error\n "); theta += 0.00001; } float_j_t C_inv[3][3]={0,}; C_inv[0][0] = 1*dt; C_inv[0][1] = sinf(pi)*tanf(theta)*dt; C_inv[0][2] = cosf(pi)*tanf(theta)*dt; C_inv[1][0] = 0; C_inv[1][1] = cosf(pi)*dt; C_inv[1][2] = -sinf(pi)*dt; C_inv[2][0] = 0; C_inv[2][1] = (sinf(pi)/cosf(theta))*dt; C_inv[2][2] = (cosf(pi)/cosf(theta))*dt; //x_k = x_k_1 + C_inv * omega * dt multiMatrix(C_inv,omega,res,3,3); sumMatrix(res,x_k_1,x_k_p,3,3); /* A_k*/ float_j_t A[3][3]={0,}; A[0][0] = 1 + (sinf(theta)*(omega[1][0]*cosf(pi)-omega[2][0]*sinf(pi)) / cosf(theta))*dt; A[0][1] = ((omega[1][0]*sinf(pi)+omega[2][0]*cosf(pi)) / (cosf(theta)*cosf(theta)))*dt; A[0][2] = 0; A[1][0] = (-omega[1][0]*sinf(pi)-omega[2][0]*cosf(pi))*dt; A[1][1] = 1; A[1][2] = 0; A[2][0] = ((omega[1][0]*cosf(pi)-omega[2][0]*sinf(pi)) / cosf(theta))*dt; A[2][1] = (sinf(theta)*(omega[1][0]*sinf(pi)+omega[2][0]*cosf(pi)) / (cosf(theta)*cosf(theta)))*dt; A[2][2] = 1; float_j_t P_k_p[3][3]={0,}; static float_j_t P_k_1[3][3]={1,0,0,0,1,0,0,0,1}; float_j_t Q[3][3]={1,0,0,0,1,0,0,0,1}; //P_k = A* P_k_1 * A^T + B * Q * B^T similarMatrix(A,P_k_1,res,3,3); similarMatrix(C_inv,Q,res2,3,3); sumMatrix(res,res2,P_k_p,3,3); //Correct term float_j_t H[3][3]={1,0,0,0,1,0,0,0,0}; float_j_t norm_a=9.8; norm_a=sqrtf(powf(sen->accX,2)+powf(sen->accY,2)+powf(sen->accZ,2)); // printf("norm_a = %f\n",norm_a); float_j_t R[3][3]={0,}; if((norm_a<10.8 )&&(norm_a>8.8)){ R[0][0]=1; R[1][1]=1; }else{ R[0][0]=1000; R[1][1]=1000; } float_j_t res3[3][3] = {0,}; similarMatrix(H,P_k_p,res,3,3); //printf("similarMatrix res\n"); //printMatrix(res,3,3); sumMatrix(res,R,res2,3,3); _2by2_inverseMatrix(res2,res,3,3); //inverseMatrix(res2,res,3,3); //result inverse matrix is res //so kalman gain K = Pk* H^T* res float_j_t K[3][3]={0,}; transMatrix(H,res3,3,3); multiMatrix(P_k_p,res3,res2,3,3); res2[0][2]=0; res2[1][2]=0; res2[2][0]=0; res2[2][1]=0; res2[2][2]=0; // printf("res2 gain\n"); // printMatrix(res2,3,3); // printf("res gain\n"); // printMatrix(res,3,3); multiMatrix(res2,res,K,3,3); // printf("kalman gain\n"); // printMatrix(K,3,3); /* state variable, covaliance mat update*/ float_j_t x_k[3][3]={0,}; //calculate z_k roll, ptich, with gravitational euler angle calc. float_j_t z_k[3][3] ={0,}; #if 1 z_k[0][0] = atan2f(sen->accY,sen->accZ); z_k[1][0] = atan2f(-sen->accX, sqrtf( powf(sen->accY,2)+powf(sen->accZ,2) )); #else if(sen->accZ>=0){ z_k[0][0] = atan2f(sen->accY,sen->accZ); z_k[1][0] = atan2f(-sen->accX, sqrtf( powf(sen->accY,2)+powf(sen->accZ,2) )); }else if(sen->accZ<0){ omega[1][0] = -omega[1][0]; z_k[1][0] = -atan2f(sen->accX,sen->accZ); z_k[0][0] = atan2f(-sen->accY, sqrtf( powf(sen->accX,2)+powf(sen->accZ,2) )); } #endif //GravityToEuler(sen->accX, sen->accY, sen->accZ, z_k); //printf("r %f p %f ",z_k[0][0]*180/3.14,z_k[1][0]*180/3.14); multiMatrix(H,x_k_p,res,3,3); subMatrix(z_k,res,res2,3,3); multiMatrix(K,res2,res,3,3); sumMatrix(x_k_p,res,x_k,3,3); float_j_t P_k[3][3]={0,}; multiMatrix(K,H,res,3,3); multiMatrix(res,P_k_p,res2,3,3); subMatrix(P_k_p,res2,P_k,3,3); /*insert prev value*/ insertMatrix(x_k, x_k_1,3,3); insertMatrix(P_k, P_k_1,3,3); /*Debug print*/ //printf("roll = %f , pitch = %f \n",x_k[0][0]*180.0/3.14,x_k[1][0]*180.0/3.14); sen->roll=x_k[0][0]*RAD_TO_DEG; sen->pitch=x_k[1][0]*RAD_TO_DEG; return 0; }