void unsolve(float a0, float a1, float a2, float& x, float& y, float& z)
{
// Calculate u,v coords for arm
float u01, v01, u12, v12;
polar2cart(L1, a1, u01, v01);
polar2cart(L2, a2, u12, v12);
// Add vectors
float u, v;
u = u01 + u12 + L3;
v = v01 + v12;
// Calculate in 3D space - note x/y reversal!
polar2cart(u, a0, y, x);
z = v;
}
void collisioncone_findnewcmd( float cc[2][6],
float *v_des, float *psi_des,
float psisearch, int nfilters )
{
int i;
int count = 1;
// int ng = 1;
bool flag = true;
float psi_add;
deg2rad(psisearch, &psi_add);
float psi0 = *psi_des;
float vx, vy;
while (*v_des <= 2.0) {
polar2cart(*v_des, *psi_des, &vx, &vy);
for (i = 0; i < nfilters; i++) { // Check if we succeed
flag = collisioncone_checkdanger(cc[i], vx, vy);
if (flag)
break;
}
if(!flag) // No issues found
return;
*psi_des = psi0 + (count * psi_add);
wrapTo2Pi(psi_des);
// ng = ng * -1;
count++;
if (count >= (2*M_PI)/psi_add) {
*v_des = *v_des + *v_des;
count = 1;
}
}
// Failed to find a solution, so just stop this time
*v_des = 0.0;
};
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;
}
