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

VCM: diffed trajectories

This commit is contained in:
emanuel 2022-10-18 10:14:08 +01:00
parent 56e00f2cc5
commit 549af5630b
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/vcm.c
View File

@ -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;
}