From 1033ebd2276fcf57b461b20bb69eea348c98313d Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Tue, 28 Jul 2020 13:48:05 +0200 Subject: [PATCH] Use uniform circle sampling --- src/rrts.cc | 32 ++++++++++++++++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/src/rrts.cc b/src/rrts.cc index 149ff00..f5babec 100644 --- a/src/rrts.cc +++ b/src/rrts.cc @@ -172,12 +172,40 @@ void RRTS::sample() double y = 0; double h = 0; switch (this->sample_dist_type()) { - case 1: + case 1: // uniform x = this->udx_(this->gen_); y = this->udy_(this->gen_); h = this->udh_(this->gen_); break; - default: + case 2: // uniform circle + { + // see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409 + double R = sqrt( + pow( + this->nodes().front().x() + - this->goals().front().x(), + 2 + ) + + pow( + this->nodes().front().y() + - this->goals().front().y(), + 2 + ) + ); + double a = atan2( + this->goals().front().y() - this->nodes().front().y(), + this->goals().front().x() - this->nodes().front().x() + ); + double cx = this->goals().front().x() + R/2 * cos(a); + double cy = this->goals().front().y() + R/2 * sin(a); + double r = R * sqrt(this->udx_(this->gen_)); + double theta = this->udy_(this->gen_) * 2 * M_PI; + x = cx + r * cos(theta); + y = cy + r * sin(theta); + h = this->udh_(this->gen_); + } + break; + default: // normal x = this->ndx_(this->gen_); y = this->ndy_(this->gen_); h = this->ndh_(this->gen_); -- 2.39.2