float get_circle_equation(const float ee[3],
const float axis[3],
const float pos_axis[3],
float upper_len,
float lower_len,
float c[3],
float u[3],
float v[3],
float n[3])
{
float wn = norm((float *)ee);
float radius;
cpvector(n, ee);
unitize(n);
// Use law of cosines to get angle between first spherical joint
// and revolute joint
float alpha;
if (!law_of_cosines(wn, upper_len, lower_len, alpha))
return 0;
// center of circle (origin is location of first S joint)
vecscalarmult(c, n, _cos(alpha) * upper_len);
radius = _sin(alpha) * upper_len;
float temp[3];
//
// A little kludgy. If the goal is behind the joint instead
// of in front of it, we reverse the angle measurement by
// inverting the normal vector
//
if (DOT(n,pos_axis) < 0.0)
vecscalarmult(n,n,-1.0);
vecscalarmult(temp, n, DOT(axis,n));
vecsub(u, (float *)axis, temp);
unitize(u);
crossproduct(v, n, u);
#if 0
printf("Circle equation\n");
printf("c = [%lf,%lf,%lf]\n", c[0], c[1], c[2]);
printf("u = [%lf,%lf,%lf]\n", u[0], u[1], u[2]);
printf("v = [%lf,%lf,%lf]\n", v[0], v[1], v[2]);
printf("n = [%lf,%lf,%lf]\n", n[0], n[1], n[2]);
printf("r = %lf\n", radius);
#endif
return radius;
}
//
// Evaluate a point on the circle given the swivel angle
//
void SRS::evaluate_circle(float angle, float p[3])
{
#if 1
evalcircle(c, u, v, radius, angle, p);
#else
// p = o + r*cos(f)*u + r*sin(f)*v
float temp[3];
cpvector(p, c);
vecscalarmult(temp, u, radius*cos(angle));
vecadd(p, p, temp);
vecscalarmult(temp, v, radius*sin(angle));
vecadd(p, p, temp);
#endif
}
int scale_goal(const float l1[3],
const float l2[3],
float g[3])
{
float g_len = _sqrt(DOT(g,g));
float L1 = _sqrt(DOT(l1,l1));
float L2 = _sqrt(DOT(l2,l2));
float max_len = (L1 + L2) * 0.9999f;
// float min_len = fabs(L1 - L2);
if (g_len > max_len)
{
vecscalarmult(g,g,max_len/(g_len/**1.01f*/));
return 1;
}
/*
if (g_len < min_len)
{
vecscalarmult(g,g,(1.01 * min_len)/g_len);
return 1;
}
*/
return 0;
}
inline void evalcircle(const float c[3],
const float u[3],
const float v[3],
float radius,
float angle,
float p[3])
{
// p = o + r*cos(f)*u + r*sin(f)*v
float temp[3];
cpvector(p, c);
vecscalarmult(temp, (float*)u, radius*_cos(angle));
vecadd(p, p, temp);
vecscalarmult(temp, (float*)v, radius*_sin(angle));
vecadd(p, p, temp);
}
//
// p = Projection of u onto v
//
void project(float p[3], const float u[3], const float v[3])
{
float vnorm[3];
cpvector(vnorm, v);
unitize(vnorm);
vecscalarmult(p, vnorm, DOT(u,vnorm));
}
//
// Form local coordinate system {x,y} from points p,q relative to the
// implicit origin 0. pscale is the reciprocal length of the p vector
// which as it turns out is already known. If the invert flag is true
// construct the transpose of the rotation matrix instead
//
inline void make_frame(const float p[3],
float p_scale,
const float q[3],
Matrix R,
int invert = 0)
{
float x[3], y[3], t[3];
// x vector is unit vector from origin to p
vecscalarmult(x, (float *)p, p_scale);
// y vector is unit perpendicular projection of q onto x
vecscalarmult(t, x, DOT(q,x));
vecsub(y, (float *) q, t);
unitize(y);
// z vector is x cross y
if (invert)
{
R[0][0] = x[0]; R[1][0] = x[1]; R[2][0] = x[2];
R[0][1] = y[0]; R[1][1] = y[1]; R[2][1] = y[2];
R[0][2] = x[1]*y[2] - x[2]*y[1];
R[1][2] = x[2]*y[0] - x[0]*y[2];
R[2][2] = x[0]*y[1] - x[1]*y[0];
}
else
{
R[0][0] = x[0]; R[0][1] = x[1]; R[0][2] = x[2];
R[1][0] = y[0]; R[1][1] = y[1]; R[1][2] = y[2];
R[2][0] = x[1]*y[2] - x[2]*y[1];
R[2][1] = x[2]*y[0] - x[0]*y[2];
R[2][2] = x[0]*y[1] - x[1]*y[0];
}
R[3][0] = R[3][1] = R[3][2] =
R[0][3] = R[1][3] = R[2][3] = 0;
R[3][3] = 1.0;
}
void MTLmarks::VecScalarRun(std::string benchmark) {
if(benchmark == "vecscalarmult"){
mtl_result = vecscalarmult(size, steps);
}
else if(benchmark == "scale"){
mtl_result = scale(size, steps);
}
else if(benchmark == "custom"){
mtl_result = custom(size, steps);
}
else{
std::cerr << "MTLmarks benchmark does not exist." << std::endl;
exit(1);
}
}
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);
}
