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;
}
