#include #include #include #include void crossp(float out[],float v[],float w[]) { out[0] = v[1] * w[2] - v[2] * w[1]; out[1] = v[2] * w[0] - v[0] * w[2]; out[2] = v[0] * w[1] - v[1] * w[0]; return; } float power(float base,int power) { float p; for(p=1;power>0;--power) { p=p*base; } return p; } float prikp(float a[],float b[]) { float x; x = a[1] * b[1] + a[2] * b[2] + a[0] * b[0]; return x; } void crosspi(int v[],int a[],int b[]) { v[0] = a[1] * b[2] - a[2] * b[1]; v[1] = a[2] * b[0] - a[0] * b[2]; v[2] = a[0] * b[1] - a[1] * b[0]; return; } float length(float a[]) { /* Calculates length of vector a */ float vectorlength; vectorlength = sqrt(power(a[0],2) + power(a[1],2) + power(a[2],2)); return vectorlength; } float lengthi(int a[]) { /* Calculates length of vector a */ float vectorlength; vectorlength = sqrt(power(a[0],2) + power(a[1],2) + power(a[2],2)); return vectorlength; } float vektorvinkel(float a[],float b[]) { /* CALCULATION OF ANGLE BETWEEN 2 VECTORS; a AND b */ float x,pi,angle,c,d; pi =3.141592; angle = 0; x = prikp(a,b); d = length(a)*length(b); if (d == 0) { angle = 0; } else { c = x / d; angle = acos(c)*180/pi; } return angle; }