From 9c76e84a63d87a2806581265eeee02fe9f1bb523 Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Wed, 1 May 2013 17:18:46 +0200 Subject: digital/io-hub/src/common-cc/path.cc: interleave the second layer of navpoints with the first one --- digital/io-hub/src/common-cc/path.cc | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index 32aed5f0..bb947212 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -97,6 +97,7 @@ void Path::reset() void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int nlayers, const uint16_t clearance) { uint32_t rot_a, rot_b, nr; + uint32_t rot_c, rot_d; uint32_t x, y, nx; int npt, layer; @@ -111,16 +112,20 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int obstacles[obstacles_nb].r = r; obstacles_nb++; - /* Complex number of modulus 1 and rotation angle */ + /* Complex number A = cos(angle) + i sin(angle) */ nr = PATH_ANGLE_824(nodes); rot_a = fixed_cos_f824(nr); rot_b = fixed_sin_f824(nr); + /* Complex number B = cos(angle/2) + i sin(angle/2) */ + nr = PATH_ANGLE_824(nodes*2); + rot_c = fixed_cos_f824(nr); + rot_d = fixed_sin_f824(nr); + /* Extend the points radius to allow the robot to go */ /* from one to another without collision with the */ /* obstacle circle. New radius is r / cos(angle/2) */ - nr = PATH_ANGLE_824(nodes*2); - x = fixed_div_f824(r, fixed_cos_f824(nr)) + 2 /* margin for the unprecise fixed point computation */; + x = fixed_div_f824(r, rot_c) + 2 /* margin for the unprecise fixed point computation */; y = 0; /* Add a number of sets of navigation points with different weights */ @@ -152,8 +157,15 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes, const int x = nx; } + /* Prepare the next layer */ /* Enlarge the circle */ - x += BOT_SIZE_RADIUS; + x += BOT_SIZE_RADIUS; y = 0; + + /* Rotate the next point by (angle/2) */ + /* This interleaves the new navpoints with the previous ones */ + nx = fixed_mul_f824(x, rot_c) - fixed_mul_f824(y, rot_d); + y = fixed_mul_f824(y, rot_c) + fixed_mul_f824(x, rot_d); + x = nx; } #ifdef HOST -- cgit v1.2.3