// Solve angles!
bool solve(float x, float y, float z, float& a0, float& a1, float& a2)
{
// Solve top-down view
float r, th0;
cart2polar(y, x, r, th0);
// Account for the wrist length!
r -= L3;
// In arm plane, convert to polar
float ang_P, R;
cart2polar(r, z, R, ang_P);
// Solve arm inner angles as required
float B, C;
if(!cosangle(L2, L1, R, B)) return false;
if(!cosangle(R, L1, L2, C)) return false;
// Solve for servo angles from horizontal
a0 = th0;
a1 = ang_P + B;
a2 = C + a1 - PI;
return true;
}
int main()
{
double rad, arg;
double x = 20.0, y = 45.0;
/* Show initial cartesian co-ords. */
printf("x = %.3g, y = %.3g\n", x, y);
/* Convert to polar. */
cart2polar(&rad, &arg, x, y);
/* Double the radius. */
rad *= 2;
/* Convert back to cartesian. */
polar2cart(&x, &y, rad, arg);
/* Show new cartesian co-ords. */
printf("x = %.3g, y = %.3g\n", x, y);
/* They should be 40, 90. */
return EXIT_SUCCESS;
}
int hl_prospective( float *vec, float px, float py,
float vx_own, float vy_own,
float vx_obst, float vy_obst,
float dt, float max, int length, float radius)
{
int i, j;
float v, ang, vox, voy, temp;
cart2polar(vx_own, vy_own, &v, &temp);
int flag = 0;
for (i = 0; i < length; i++)
{
j = 1;
ang = -M_PI + (2*M_PI/length)*i;
wrapToPi(&ang);
polar2cart(v, ang, &vox, &voy);
while(j*dt < max)
{
vec[i] = j*dt;
flag = hl_collisiontest( px, py, vox, voy, vx_obst, vy_obst, j*dt, radius);
if (flag)
break;
else
j++;
}
}
return flag;
}
/*
Updated the coordinates of the triangle for the collision cone of an obstacle.
*/
void collisioncone_update( float *cc,
float relx, float rely,
float relvx, float relvy,
float radius)
{
float range, bearing;
cart2polar(relx, rely, &range, &bearing);
// The other two edges are symmetrical about a diagonal extending along the localization bearing.
cc[0] = 0.0;
cc[1] = 0.0;
// This is the edge extending upwards to the right (+y)
cc[2] = 5.0; // x_body
cc[3] = 5.0*tan(atan(radius/(1.0*range))); // y_body
// This is the edge exteding upwards to the left (-y)
cc[4] = cc[2]; // x_body
cc[5] = -cc[3]; // y_body
shape_rotateatorigin(cc, 6, bearing);
shape_shift(cc, 6, relvx, relvy);
}
