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); }