Ok, i´m not done :\ mi friend told me to try this:
float dotProduct(float x[3],float y[3]){
return x[0]*y[0]+x[1]*y[1]*x[2]+y[2];}
float length(float x[3]){
return sqrtf(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);
}
float clamp(float v, float i, float s){
if (v<i)
return i;
else if(v>s)
return s;
else return v;
}
float anglbetween(float x[3], float y[3])
{
float xu[3]; //aux array so i can create an unitary vector
float yu[3]; //aux array so i can create an unitary vector
xu[0]=x[0]/(abs(x[0]));
xu[1]=x[1]/(abs(x[1]));
xu[2]=x[2]/(abs(x[2]));
yu[0]=y[0]/(abs(y[0]));
yu[1]=y[1]/(abs(y[1]));
yu[2]=y[2]/(abs(y[2]));
float lp = length(xu) * length(yu);
if(lp<1e-6f)
lp=1e-6f;
float f= dotProduct(xu,yu)/lp;
f=clamp(f,-1,1);
return acosf(f);
}
but it does not work. I used some "printf" for fast debug, and i found a strange thing, first i use calculadistancia() and both parameters have their values right, but wen i passe the same parameters to anglbetween() they have strange values like: 1.#IND000
And i was so close :\ i think xD
Inside your anglbetween() function:
xu[0]=x[0]/(abs(x[0]));
xu[1]=x[1]/(abs(x[1]));
xu[2]=x[2]/(abs(x[2]));
yu[0]=y[0]/(abs(y[0]));
yu[1]=y[1]/(abs(y[1]));
yu[2]=y[2]/(abs(y[2]));
is incorrect for calculating unit vector. It does not make any sense if you think about it carefully. x[0]/(abs(x[0]) is going to return you either 1.0f or -1.0f. You are not going to get anything other ones or minus ones for your xu and yu which is obviously wrong.
You should be dividing each component with the magnitude or length of the original vector, like this:
xu[0] = x[0] / length(x);
xu[1] = x[1] / length(x);
xu[2] = x[2] / length(x);
yu[0] = y[0] / length(y);
yu[1] = y[1] / length(y);
yu[2] = y[2] / length(y);
After this step, since you have both unit vectors xu and yu, there is no need to calculate Ip as shown in your code. Just take the dot product of xu and yu and acosf it to get the angle between two vectors. Be careful to note whether acosf returns degree or radians, like what unbird has mentioned.