From 148eb02636cfb33f27ac1e5b7073e96e06f35f70 Mon Sep 17 00:00:00 2001
From: emanuel <vieira.e@ua.pt>
Date: Thu, 10 Feb 2022 17:06:56 +0000
Subject: [PATCH] Very simple vehicle intersection

---
 src/dcm.c | 32 +++++++++++++++++++-------------
 1 file changed, 19 insertions(+), 13 deletions(-)

diff --git a/src/dcm.c b/src/dcm.c
index 050a3f6..3192e49 100644
--- a/src/dcm.c
+++ b/src/dcm.c
@@ -10,25 +10,31 @@
 
 #define SLEEP_TIME_MS 100
 
-static int are_paths_intersecting(it2s_tender_st_s* tA, int tA_len, it2s_tender_st_s* tB, int tB_len) {
+static int are_vehicles_intersecting(
+        it2s_tender_st_s* tA, int tA_len, uint16_t vA_length, uint16_t vA_width, 
+        it2s_tender_st_s* tB, int tB_len, uint16_t vB_length, uint16_t vB_width
+        ) {
 
     // TODO check first time intersection, then the spacial trajectory
     double A1[2], A2[2], B1[2], B2[2];
     uint64_t tsA, tsB;
-    for (int a = 0; a < tA_len-1; ++a) {
-        A1[0] = tA[a].latitude;
-        A1[1] = tA[a].longitude;
-        A2[0] = tA[a+1].latitude;
-        A2[1] = tA[a+1].longitude;
+    for (int a = 0; a < tA_len; ++a) {
+//    for (int a = 0; a < tA_len-1; ++a) {
+//        A1[0] = tA[a].latitude;
+//        A1[1] = tA[a].longitude;
+//        A2[0] = tA[a+1].latitude;
+//        A2[1] = tA[a+1].longitude;
 
 
-        for (int b = 0; b < tB_len-1; ++b) {
-            B1[0] = tB[b].latitude;
-            B1[1] = tB[b].longitude;
-            B2[0] = tB[b+1].latitude;
-            B2[1] = tB[b+1].longitude;
+        for (int b = 0; b < tB_len; ++b) {
+//        for (int b = 0; b < tB_len-1; ++b) {
+//            B1[0] = tB[b].latitude;
+//            B1[1] = tB[b].longitude;
+//            B2[0] = tB[b+1].latitude;
+//            B2[1] = tB[b+1].longitude;
 
-            if (it2s_tender_do_segments_intersect(A1, A2, B1, B2)) {
+            //if (it2s_tender_do_segments_intersect(A1, A2, B1, B2)) {
+            if (it2s_tender_is_inside_rectangle(tA[a].latitude, tA[a].longitude, tB[b].latitude, tB[b].longitude, 8, 8, 0, DCM_VINCENTY)) {
                 if (tA[a].timestamp < tB[b].timestamp + 2000 &&
                     tA[a].timestamp > tB[b].timestamp - 2000) {
                     syslog_info("[facilities] [dc] intersecting @ (%d, %d) in %ld ms", tA[a].latitude, tA[a].longitude, tA[a].timestamp-tA[0].timestamp);
@@ -80,7 +86,7 @@ int dcm_check(void* fc, DCM_t* dcm) {
     }
 
     if (trajectoryA_len > 1 && trajectoryB_len > 1) {
-        are_paths_intersecting(trajectoryA, trajectoryA_len, trajectoryB, trajectoryB_len);
+        are_vehicles_intersecting(trajectoryA, trajectoryA_len, facilities->vehicle.length, facilities->vehicle.width, trajectoryB, trajectoryB_len, dcm->dcm.vehicleDimensions.length, dcm->dcm.vehicleDimensions.width);
     }
 
 cleanup: