Update nn3 with co2

diff --git a/base/nn.cc b/base/nn.cc
index 29a8206998a7db0c8b9c83179ba6c73fd72f7805..e6155af95c8786ecdd98b2e2d0b750d159691590 100644 (file)
--- a/base/nn.cc
+++ b/base/nn.cc
@@ -17,6 +17,7 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 
 #include <omp.h>
 #include <vector>
+#include "cost.h"
 #include "nn.h"
 #include "rrtbase.h"
 
@@ -84,14 +85,16 @@ RRTNode *nn3(
         unsigned int i = 0; // vector step
         unsigned int j = 0; // array step
         int iyj = 0;
+        float oh = node->h();
         while (nn.mc > j * IYSTEP) {
                 iyj = (int) (iy + j);
                 if (iyj >= IYSIZE)
                         iyj = IYSIZE - 1;
                 #pragma omp parallel for reduction(minn: nn)
                 for (i = 0; i < nodes[iyj].size(); i++) {
-                        if ((*cost)(nodes[iyj][i], node) < nn.mc) {
-                                nn.mc = (*cost)(nodes[iyj][i], node);
+                        node->h(nodes[iyj][i]->h());
+                        if (co2(nodes[iyj][i], node) < nn.mc) {
+                                nn.mc = co2(nodes[iyj][i], node);
                                 nn.nn = nodes[iyj][i];
                         }
                 }
@@ -101,14 +104,16 @@ RRTNode *nn3(
                                 iyj = 0;
                         #pragma omp parallel for reduction(minn: nn)
                         for (i = 0; i < nodes[iyj].size(); i++) {
-                                if ((*cost)(nodes[iyj][i], node) < nn.mc) {
-                                        nn.mc = (*cost)(nodes[iyj][i], node);
+                                node->h(nodes[iyj][i]->h());
+                                if (co2(nodes[iyj][i], node) < nn.mc) {
+                                        nn.mc = co2(nodes[iyj][i], node);
                                         nn.nn = nodes[iyj][i];
                                 }
                         }
                 }
                 j++;
         }
+        node->h(oh);
         return nn.nn;
 }