From e92fba0a32fc51b7f04bde689df28456c4d39e6c Mon Sep 17 00:00:00 2001 From: emanuel Date: Tue, 18 Oct 2022 11:19:58 +0100 Subject: [PATCH] VCM: line intersection --- src/vcm.c | 35 +++++++++++++++-------------------- 1 file changed, 15 insertions(+), 20 deletions(-) diff --git a/src/vcm.c b/src/vcm.c index 8a9ce5f..4660b70 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -20,30 +20,25 @@ static int are_vehicles_intersecting( // 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; ++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 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) { + if (tA[a].timestamp > tB[b].timestamp + 1000 || + tA[a].timestamp < tB[b].timestamp - 1000) { + continue; + } + 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 (itss_do_segments_intersect(A1, A2, B1, B2)) { -// printf("tA[a].lat=%d tA[a].lon=%d tB[b].lat=%d tB[b].lon=%d tA[a].ts=%ld tB[b].ts=%ld\n", -// tA[a].latitude, tA[a].longitude, tB[b].latitude, tB[b].longitude, tA[a].timestamp, tB[b].timestamp); fflush(stdout); - if (itss_is_inside_circle(tA[a].latitude, tA[a].longitude, tB[b].latitude, tB[b].longitude, 8, DCM_HAVERSINE)) { - if (tA[a].timestamp < tB[b].timestamp + 2000 && - tA[a].timestamp > tB[b].timestamp - 2000) { + if (itss_do_segments_intersect(A1, A2, B1, B2)) { *index = a; return 1; - } } } }