//
// Return the angle between two vectors u,v about the axis n
//
float angle_between_vectors(float u[3], float v[3], float n[3])
{
#if 0
float temp[3];
float up[3];
float vp[3];
cpvector(up, u);
cpvector(vp, v);
unitize(up);
unitize(vp);
crossproduct(temp, up, vp);
float mag = DOT(temp,n);
// Vectors are parallel at 0 or 180
if (mag*mag < 1e-8)
{
if (DOT(up,vp) < 0)
return M_PI;
else
return 0;
}
int sign = (mag > 0) ? 1 : -1;
float t = DOT(up,vp);
if (t > 1.0)
t = 1.0;
else if (t < -1.0)
t = -1.0;
return sign*acos(t);
#else
float up[3];
float vp[3];
float uv[3];
project_plane(up, u, n);
project_plane(vp, v, n);
crossproduct(uv, up, vp);
return atan2(DOT(n, uv), DOT(up, vp));
#endif
}
float SRS::PosToAngle(const float p[3])
{
// Find vector from center of circle to pos and project it onto circle
float cp[3], pp[3];
vecsub(cp, (float *) p, c);
project_plane(pp , cp, n);
// Find angle between u and pp. This is the swivel angle
return angle_between_vectors(u, pp, n);
}
static void get_aim_circle_equation(const float g[3],
const float a[3],
const float ta[3],
const float tb[3],
const float proj_axis[3],
float theta4,
float center[3],
float u[3],
float v[3],
float &radius)
{
float L1 = DOT(ta,ta);
float L2 = DOT(tb,tb);
Matrix Ry, Ryt;
rotation_principal_axis_to_matrix('y', theta4, Ry);
invertrmatrix(Ryt, Ry);
// Compute distance of hand to shoulder
float t1[3], t2[3];
vecmult(t1, (float *) tb, Ry);
vecmult(t2, (float *) ta, Ryt);
float L3 = _sqrt(L1 + L2 + DOT(ta,t1) + DOT(tb,t2));
// Lengths of upper and lower arms
L1 = _sqrt(L1);
L2 = _sqrt(L2);
// Compute angle between a and shoulder-to-hand vector
// This is done assuming R1 = I since the angle does
// not depend on the shoulder joints
//
// h = Ry*tb + ta
// a = Ry*a
vecadd(t2, t1, (float *) ta);
unitize(t2);
vecmult(t1, (float *) a, Ry);
float alpha = acos(DOT(t1,t2));
//
// Compute the angles of the triangle s,h,g
//
float L4 = _sqrt(DOT(g,g));
float beta = M_PI - alpha;
float delta = asin(_sin(beta)*L3/L4);
if (delta < 0)
delta = - delta;
float gamma = M_PI - delta - beta;
float c_gamma = _cos(gamma);
float n[3];
cpvector(n, g);
unitize(n);
vecscalarmult(center, n, c_gamma*L3);
radius = _sqrt(1-c_gamma*c_gamma)*L3;
project_plane(u, (float *) proj_axis, n);
unitize(u);
crossproduct(v, n, u);
}
