Math.cpp

00001 /* This file is part of the Razor AHRS Firmware */
00002 #include <math.h>
00003 
00004 // Computes the dot product of two vectors
00005 float Vector_Dot_Product(float vector1[3], float vector2[3])
00006 {
00007   float op=0;
00008   
00009   for(int c=0; c<3; c++)
00010   {
00011     op+=vector1[c]*vector2[c];
00012   }
00013   
00014   return op; 
00015 }
00016 
00017 // Computes the cross product of two vectors
00018 void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
00019 {
00020   vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
00021   vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
00022   vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
00023 }
00024 
00025 // Multiply the vector by a scalar. 
00026 void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
00027 {
00028   for(int c=0; c<3; c++)
00029   {
00030     vectorOut[c]=vectorIn[c]*scale2; 
00031   }
00032 }
00033 
00034 // Adds two vectors
00035 void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3])
00036 {
00037   for(int c=0; c<3; c++)
00038   {
00039     vectorOut[c]=vectorIn1[c]+vectorIn2[c];
00040   }
00041 }
00042 
00043 //Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). 
00044 void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
00045 {
00046   float op[3]; 
00047   for(int x=0; x<3; x++)
00048   {
00049     for(int y=0; y<3; y++)
00050     {
00051       for(int w=0; w<3; w++)
00052       {
00053        op[w]=a[x][w]*b[w][y];
00054       } 
00055       mat[x][y]=0;
00056       mat[x][y]=op[0]+op[1]+op[2];
00057       
00058       float test=mat[x][y];
00059     }
00060   }
00061 }
00062 
00063 // Init rotation matrix using euler angles
00064 void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll)
00065 {
00066   float c1 = cos(roll);
00067   float s1 = sin(roll);
00068   float c2 = cos(pitch);
00069   float s2 = sin(pitch);
00070   float c3 = cos(yaw);
00071   float s3 = sin(yaw);
00072 
00073   // Euler angles, right-handed, intrinsic, XYZ convention
00074   // (which means: rotate around body axes Z, Y', X'') 
00075   m[0][0] = c2 * c3;
00076   m[0][1] = c3 * s1 * s2 - c1 * s3;
00077   m[0][2] = s1 * s3 + c1 * c3 * s2;
00078 
00079   m[1][0] = c2 * s3;
00080   m[1][1] = c1 * c3 + s1 * s2 * s3;
00081   m[1][2] = c1 * s2 * s3 - c3 * s1;
00082 
00083   m[2][0] = -s2;
00084   m[2][1] = c2 * s1;
00085   m[2][2] = c1 * c2;
00086 }
00087 
00088 float constrain(float in, float min, float max)
00089 {
00090     in = in > max ? max : in;
00091     in = in < min ? min : in;
00092     return in;
00093 }