/**
* Compares two strings and returns the similarity index
* 100 == most equal
**/
int fncmp( const char* str1, const char* str2 ){
strval_t str1val, str2val;
int maxval, max1, max2;
long result;
float step;
str1val=calloc( CMP_ARRAYLEN, sizeof( char ) );
str2val=calloc( CMP_ARRAYLEN, sizeof( char ) );
max1=computestrval( str1, str1val );
max2=computestrval( str2, str2val );
// the max possible matches are defined by the min number of bits set!
maxval=(max1 < max2) ? max1 : max2;
if( maxval < 4 ) return -1;
step=100.0/maxval;
result=vecmult( str1val, str2val );
free( str1val );
free( str2val );
return step*result;
}
void SRS::SetAimGoal(const float goal[3],
const float ax[3],
float flex_angle)
{
float s[3];
cpvector(ee, goal);
cpvector(axis, ax);
get_translation(T, p_r1);
get_translation(S, s);
get_aim_circle_equation(goal,
axis, p_r1, s, proj_axis, flex_angle, c, u, v, radius);
rotation_principal_axis_to_matrix('y', flex_angle, Ry);
vecmult(ee_r1, (float*)s, Ry);
vecadd(ee_r1, ee_r1, (float*)p_r1);
}
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);
}
