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]);
	}
}
Beispiel #2
0
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;
}
Beispiel #3
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);
}
Beispiel #7
0
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);
	}
}
Beispiel #8
0
  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);
}
Beispiel #10
0
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);
}
Beispiel #12
0
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;
}