From 549af5630b42b3f7b8adef74bb0e7d851a544cd6 Mon Sep 17 00:00:00 2001 From: emanuel Date: Tue, 18 Oct 2022 10:14:08 +0100 Subject: [PATCH] VCM: diffed trajectories --- src/vcm.c | 32 +++++++++++++++++++------------- 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/src/vcm.c b/src/vcm.c index dd1d2c3..8a9ce5f 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -330,14 +330,17 @@ static int vcm_check_intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour mvc->vehicleWidth = facilities.vehicle.width; // Planned trajectory - mvc->plannedTrajectory.list.count = --trajectoryA_len; + mvc->plannedTrajectory.list.count = trajectoryA_len; mvc->plannedTrajectory.list.size = trajectoryA_len * sizeof(void*); mvc->plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*)); - for (int i = 0; i < trajectoryA_len; ++i) { + mvc->plannedTrajectory.list.array[0] = calloc(1, sizeof(STPoint_t)); + mvc->plannedTrajectory.list.array[0]->latitude = trajectoryA[0].latitude - lat; + mvc->plannedTrajectory.list.array[0]->longitude = trajectoryA[0].longitude - lon; + for (int i = 0; i < trajectoryA_len - 1; ++i) { mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); - mvc->plannedTrajectory.list.array[i]->latitude = trajectoryA[i+1].latitude; - mvc->plannedTrajectory.list.array[i]->longitude = trajectoryA[i+1].longitude; - asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[i]->timestamp, trajectoryA[i+1].timestamp); + mvc->plannedTrajectory.list.array[i]->latitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude; + mvc->plannedTrajectory.list.array[i]->longitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude; + asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[i]->timestamp, trajectoryA[i].timestamp); } // Desired trajectory @@ -354,11 +357,14 @@ static int vcm_check_intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour pt->trajectory.list.count = trajectoryA_len; pt->trajectory.list.size = sizeof(void*) * trajectoryA_len; pt->trajectory.list.array = malloc(sizeof(void*) * trajectoryA_len); - for (int i = 0; i < trajectoryA_len; ++i) { + pt->trajectory.list.array[0] = calloc(1, sizeof(STPoint_t)); + pt->trajectory.list.array[0]->latitude = trajectoryA[0].latitude - lat; + pt->trajectory.list.array[0]->longitude = trajectoryA[0].longitude - lon; + for (int i = 0; i < trajectoryA_len - 1; ++i) { pt->trajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); - pt->trajectory.list.array[i]->latitude = trajectoryA[i+1].latitude; - pt->trajectory.list.array[i]->longitude = trajectoryA[i+1].longitude; - asn_ulong2INTEGER(&pt->trajectory.list.array[i]->timestamp, trajectoryA[i+1].timestamp); + pt->trajectory.list.array[i]->latitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude; + pt->trajectory.list.array[i]->longitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude; + asn_ulong2INTEGER(&pt->trajectory.list.array[i]->timestamp, trajectoryA[i].timestamp); } pt->offer = 5; @@ -515,10 +521,10 @@ int vcm_check(VCM_t* vcm) { trajectoryB[0].longitude = vcm->vcm.currentPosition.longitude; asn_INTEGER2ulong(&vcm->vcm.currentPosition.timestamp, (unsigned long long*) &trajectoryB[0].timestamp); ++trajectoryB_len; - for (int i = 0; i < mvc->plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) { - trajectoryB[i+1].latitude = mvc->plannedTrajectory.list.array[i]->latitude; - trajectoryB[i+1].longitude = mvc->plannedTrajectory.list.array[i]->longitude; - asn_INTEGER2ulong(&mvc->plannedTrajectory.list.array[i]->timestamp, (unsigned long long*) &trajectoryB[i+1].timestamp); + for (int i = 1; i < mvc->plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) { + trajectoryB[i].latitude = mvc->plannedTrajectory.list.array[i]->latitude + trajectoryB[i-1].latitude; + trajectoryB[i].longitude = mvc->plannedTrajectory.list.array[i]->longitude + trajectoryB[i-1].longitude; + asn_INTEGER2ulong(&mvc->plannedTrajectory.list.array[i]->timestamp, (unsigned long long*) &trajectoryB[i].timestamp); ++trajectoryB_len; }