From 43503f705678d53fe925a74c41c05e92fecbc9be Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Sun, 13 Jan 2019 16:30:21 +0100 Subject: [PATCH] Add trees overlap check --- base/main.cc | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/base/main.cc b/base/main.cc index 0a426ef..1b176f8 100644 --- a/base/main.cc +++ b/base/main.cc @@ -177,6 +177,7 @@ int main() } #elif defined USE_PTHREAD bool gf = false; + float mc = 9999; pthread_t rt; // root thread pthread_t gt; // goal thread pthread_t ct; // connect thread @@ -194,8 +195,29 @@ int main() p.p_goal_.tstart(); pthread_create(&rt, NULL, &next_run, (void *) &ra); pthread_create(>, NULL, &next_run, (void *) &ga); + volatile int nofrn = 0; + volatile int nofgn = 0; + RRTNode *rn; + RRTNode *gn; while (!gf && p.elapsed() < TMAX) { - sleep(0.2); + // overlap trees + nofrn = p.p_root_.nodes().size() - 1; // TODO workaround + nofgn = p.p_goal_.nodes().size() - 1; + for (int i = 0; i < nofrn; i++) { + rn = p.p_root_.nodes()[i]; + if (rn->parent() == nullptr) + continue; + for (int j = 0; j < nofgn; j++) { + gn = p.p_goal_.nodes()[j]; + if (gn->parent() == nullptr) + continue; + if (rn->ccost() + gn->ccost() < mc && + IS_NEAR(rn, gn)) { + gf = true; + } + } + } + // end of overlap trees p.tend(); } pthread_join(rt, NULL); -- 2.39.2