Subversion Repositories FlightCtrl

#ifndef VECTOR3D__H

#define VECTOR3D__H

//get modulus of a 3d vector sqrt(x^2+y^2+y^2)

float vector3d_modulus(float* vector){

        static float R;  

        R = vector[0]*vector[0];

        R += vector[1]*vector[1];

        R += vector[2]*vector[2];

        return sqrt(R);

}

//convert vector to a vector with same direction and modulus 1

void vector3d_normalize(float* vector){

        static float R;  

        R = vector3d_modulus(vector);

        vector[0] /= R;

        vector[1] /= R;

        vector[2] /= R;  

}

//calcuate vector dot-product  c = a . b

float vector3d_dot(float* a,float* b){

        return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];

}

//calcuate vector cross-product  c = a x b

void vector3d_cross(float* a,float* b, float* c){

        c[0] = a[1]*b[2] - a[2]*b[1];

        c[1] = a[2]*b[0] - a[0]*b[2];

        c[2] = a[0]*b[1] - a[1]*b[0];

}

//calcuate vector scalar-product  n = s x a

void vector3d_scale(float s, float* a , float* b){

        b[0] = s*a[0];

        b[1] = s*a[1];

        b[2] = s*a[2];

}

//calcuate vector sum   c = a + b

void vector3d_add(float* a , float* b, float* c){

        c[0] = a[0] + b[0];

        c[1] = a[1] + b[1];

        c[2] = a[2] + b[2];

}

//creates equivalent skew symetric matrix plus identity

//for v = {x,y,z} returns

// m = {{1,-z,y}

//              {z,1,-x}

//              {-y,x,1}}

void vector3d_skew_plus_identity(float *v,float* m){

        m[0*3+0]=1;

        m[0*3+1]=-v[2];

        m[0*3+2]=v[1];

        m[1*3+0]=v[2];

        m[1*3+1]=1;

        m[1*3+2]=-v[0];

        m[2*3+0]=-v[1];

        m[2*3+1]=v[0];

        m[2*3+2]=1;

}

#endif