Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
2189 - 1
#ifndef VECTOR3D__H
2
#define VECTOR3D__H
3
 
4
//get modulus of a 3d vector sqrt(x^2+y^2+y^2)
5
float vector3d_modulus(float* vector){
6
        static float R;  
7
        R = vector[0]*vector[0];
8
        R += vector[1]*vector[1];
9
        R += vector[2]*vector[2];
10
        return sqrt(R);
11
}
12
 
13
//convert vector to a vector with same direction and modulus 1
14
void vector3d_normalize(float* vector){
15
        static float R;  
16
        R = vector3d_modulus(vector);
17
        vector[0] /= R;
18
        vector[1] /= R;
19
        vector[2] /= R;  
20
}
21
 
22
//calcuate vector dot-product  c = a . b
23
float vector3d_dot(float* a,float* b){
24
        return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
25
}
26
 
27
 
28
//calcuate vector cross-product  c = a x b
29
void vector3d_cross(float* a,float* b, float* c){
30
        c[0] = a[1]*b[2] - a[2]*b[1];
31
        c[1] = a[2]*b[0] - a[0]*b[2];
32
        c[2] = a[0]*b[1] - a[1]*b[0];
33
}
34
 
35
//calcuate vector scalar-product  n = s x a
36
void vector3d_scale(float s, float* a , float* b){
37
        b[0] = s*a[0];
38
        b[1] = s*a[1];
39
        b[2] = s*a[2];
40
}
41
 
42
 
43
//calcuate vector sum   c = a + b
44
void vector3d_add(float* a , float* b, float* c){
45
        c[0] = a[0] + b[0];
46
        c[1] = a[1] + b[1];
47
        c[2] = a[2] + b[2];
48
}
49
 
50
 
51
//creates equivalent skew symetric matrix plus identity
52
//for v = {x,y,z} returns
53
// m = {{1,-z,y}
54
//              {z,1,-x}
55
//              {-y,x,1}}
56
void vector3d_skew_plus_identity(float *v,float* m){
57
        m[0*3+0]=1;
58
        m[0*3+1]=-v[2];
59
        m[0*3+2]=v[1];
60
        m[1*3+0]=v[2];
61
        m[1*3+1]=1;
62
        m[1*3+2]=-v[0];
63
        m[2*3+0]=-v[1];
64
        m[2*3+1]=v[0];
65
        m[2*3+2]=1;
66
}
67
 
68
 
69
 
70
 
71
 
72
#endif