Implement random point chosing algorithm

3 files changed, 31 insertions, 5 deletions

diff --git a/geom.hpp b/geom.hpp
index 06b9e65..e69fb81 100644
--- a/geom.hpp
+++ b/geom.hpp
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <stdlib.h>
 #include <math.h>
 #include <algorithm>
 #include <assert.h>
@@ -7,6 +8,11 @@
 #define EPSILON 1e-6
 #define abs(x) ((x)<0?-(x):(x))
 
+inline double frand(double a, double b) {
+	double r = ((double)rand()) / ((double)RAND_MAX);
+	return r * (b - a) + a;
+}
+
 struct vec {
 	double x, y;
 
diff --git a/main.cpp b/main.cpp
index 440a12f..e587aa5 100644
--- a/main.cpp
+++ b/main.cpp
@@ -3,6 +3,8 @@
 #include "ui.hpp"
 
 int main() {
+	srand(time(0));
+
 	hilare_a_param p;
 	p.l = 50;
 	p.r_c_car = 25;
diff --git a/problem.cpp b/problem.cpp
index 11f4d8e..67287fa 100644
--- a/problem.cpp
+++ b/problem.cpp
@@ -48,8 +48,8 @@ bool hilare_a_mvt::intersects(const obstacle &o) const {
 }
 
 bool hilare_a_mvt::intersects(const problem &p) const {
-	for (auto i = p.obstacles.begin(); i != p.obstacles.end(); i++) {
-		if (intersects(*i)) return true;
+	for (auto& i: p.obstacles) {
+		if (intersects(i)) return true;
 	}
 	return false;
 }
@@ -212,10 +212,28 @@ solution solver_internal::try_find_solution() {
 
 void solver_internal::step(const problem &p) {
 	// take new random point
-	// try to connect to all existing points
+	double min_x = p.obstacles[0].c.c.x, min_y = p.obstacles[0].c.c.y;
+	double max_x = min_x, max_y = min_y;
+	for (auto& o: p.obstacles) {
+		if (o.c.c.x < min_x) min_x = o.c.c.x;
+		if (o.c.c.y < min_y) min_y = o.c.c.y;
+		if (o.c.c.x > max_x) max_x = o.c.c.x;
+		if (o.c.c.y > max_y) max_y = o.c.c.y;
+	}
+	hilare_a rp = p.begin_pos;
+	rp.x = frand(min_x, max_x);
+	rp.y = frand(min_y, max_y);
+	rp.theta = frand(-M_PI, M_PI);
+	rp.phi = frand(-M_PI, M_PI);
 
-	// TODO
-	sf::sleep(sf::milliseconds(10));	// no CPU hog
+	// try to connect to all existing points
+	for (unsigned i = 0; i < pts.size(); i++) {
+		solution s = solution::direct_sol(pts[i], rp);
+		if (s.movement.size() > 0 && !s.intersects(p)) {
+			paths[i][pts.size()] = s;
+		}
+	}
+	pts.push_back(rp);
 }
 
 /* vim: set ts=4 sw=4 tw=0 noet :*/