TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Very simple vehicle intersection

This commit is contained in:
emanuel 2022-02-10 17:06:56 +00:00
parent d51d7faeed
commit 148eb02636
1 changed files with 19 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/dcm.c
View File

@ -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: