Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/moving.py @ 1213:3f2214125164
work in progress
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 03 May 2023 14:58:26 -0400 |
parents | af329f3330ba |
children | 01c24c1cdb70 |
comparison
equal
deleted
inserted
replaced
1212:af329f3330ba | 1213:3f2214125164 |
---|---|
394 return ttc2 | 394 return ttc2 |
395 else: # ttc1 < 0 and ttc2 < 0: | 395 else: # ttc1 < 0 and ttc2 < 0: |
396 return None | 396 return None |
397 else: | 397 else: |
398 return None | 398 return None |
399 | |
400 @staticmethod | |
401 def ttc_calculate(bbox_info_total_1, bbox_info_total_2): | |
402 """ | |
403 :param bbox_1: list: [bbox_1 array 4*2 , v_1_x (float), v_1_y (float)] | |
404 :param bbox_2: [bbox_2_x array 4*2, v_2_x (float), v_2_y (float)] | |
405 :return: ttc | |
406 """ | |
407 from sympy import solve | |
408 def NewFourPoints(bbox, col_time, v_x, v_y): | |
409 return [i + col_time * v_x for i in bbox[0]], [j + col_time * v_y for j in bbox[1]] | |
410 | |
411 bbox_1, v_1_x, v_1_y = bbox_info_total_1[0], bbox_info_total_1[1], bbox_info_total_1[2] | |
412 bbox_2, v_2_x, v_2_y = bbox_info_total_2[0], bbox_info_total_2[1], bbox_info_total_2[2] | |
413 x_bbox_1 = bbox_1[:, 0].tolist() | |
414 y_bbox_1 = bbox_1[:, 1].tolist() | |
415 bbox_1 = [x_bbox_1, y_bbox_1] | |
416 x_bbox_2 = bbox_2[:, 0].tolist() | |
417 y_bbox_2 = bbox_2[:, 1].tolist() | |
418 bbox_2 = [x_bbox_2, y_bbox_2] | |
419 t_total = [] | |
420 line = [[0, 1], [1, 2], [2, 3], [3, 0]] | |
421 for i in range(4): | |
422 for j in range(4): | |
423 p0_x, p0_y = x_bbox_2[i], y_bbox_2[i] | |
424 p1_x, p1_y = x_bbox_1[line[j][0]], y_bbox_1[line[j][0]] | |
425 p2_x, p2_y = x_bbox_1[line[j][1]], y_bbox_1[line[j][1]] | |
426 f = (p2_x - p1_x) * (p0_y - p1_y) - (p0_x - p1_x) * (p2_y - p1_y) | |
427 collision_t = solve(f, t, set=True, dict=True) | |
428 if collision_t and collision_t[0][t] >= 0: | |
429 collision_t = collision_t[0][t] | |
430 print(f'collision_t: {collision_t} >>>>>>>>>') | |
431 x_bbox_1_new, y_bbox_1_new = NewFourPoints(bbox_1, collision_t, v_1_x, v_1_y) | |
432 x_bbox_2_new, y_bbox_2_new = NewFourPoints(bbox_2, collision_t, v_2_x, v_2_y) | |
433 p0_x, p0_y = x_bbox_2_new[i], y_bbox_2_new[i] | |
434 p1_x, p1_y = x_bbox_1_new[line[j][0]], y_bbox_1_new[line[j][0]] | |
435 p2_x, p2_y = x_bbox_1_new[line[j][1]], y_bbox_1_new[line[j][1]] | |
436 min_x, max_x = min(p1_x, p2_x), max(p1_x, p2_x) | |
437 min_y, max_y = min(p1_y, p2_y), max(p1_y, p2_y) | |
438 if min_x == max_x: | |
439 if min_y <= p0_y <= max_y: | |
440 t_total.append(collision_t) | |
441 | |
442 elif min_y == max_y: | |
443 if min_x <= p0_x <= max_x: | |
444 t_total.append(collision_t) | |
445 | |
446 else: | |
447 if min_x <= p0_x <= max_x or min_y <= p0_y <= max_y: | |
448 t_total.append(collision_t) | |
449 x_temps = x_bbox_2 | |
450 x_bbox_2 = x_bbox_1 | |
451 x_bbox_1 = x_temps | |
452 # print(f'Change bounding box1 and box2 >>>>>>>>>>>>>>>>>>>\n\n\n') | |
453 for i in range(4): | |
454 for j in range(4): | |
455 p0_x, p0_y = x_bbox_2[i], y_bbox_2[i] | |
456 p1_x, p1_y = x_bbox_1[line[j][0]], y_bbox_1[line[j][0]] | |
457 p2_x, p2_y = x_bbox_1[line[j][1]], y_bbox_1[line[j][1]] | |
458 f = (p2_x - p1_x) * (p0_y - p1_y) - (p0_x - p1_x) * (p2_y - p1_y) | |
459 collision_t = solve(f, t, set=True, dict=True) | |
460 if collision_t and collision_t[0][t] >= 0: | |
461 collision_t = collision_t[0][t] | |
462 # print(f'collision_t: {collision_t} >>>>>>>>>') | |
463 x_bbox_1_new, y_bbox_1_new = NewFourPoints(bbox_1, collision_t) | |
464 x_bbox_2_new, y_bbox_2_new = NewFourPoints(bbox_2, collision_t) | |
465 p0_x, p0_y = x_bbox_2_new[i], y_bbox_2_new[i] | |
466 p1_x, p1_y = x_bbox_1_new[line[j][0]], y_bbox_1_new[line[j][0]] | |
467 p2_x, p2_y = x_bbox_1_new[line[j][1]], y_bbox_1_new[line[j][1]] | |
468 min_x, max_x = min(p1_x, p2_x), max(p1_x, p2_x) | |
469 min_y, max_y = min(p1_y, p2_y), max(p1_y, p2_y) | |
470 if min_x == max_x: | |
471 if min_y <= p0_y <= max_y: | |
472 t_total.append(collision_t) | |
473 | |
474 elif min_y == max_y: | |
475 if min_x <= p0_x <= max_x: | |
476 t_total.append(collision_t) | |
477 | |
478 else: | |
479 if min_x <= p0_x <= max_x or min_y <= p0_y <= max_y: | |
480 t_total.append(collision_t) | |
481 | |
482 if t_total: | |
483 collision_t_min = min(t_total) | |
484 # print(f'collision_time:') | |
485 # print(collision_t_min) | |
486 else: | |
487 collision_t_min = 1e7 | |
488 # print(f'No collision') | |
489 return collision_t_min | |
399 | 490 |
400 @staticmethod | 491 @staticmethod |
401 def midPoint(p1, p2): | 492 def midPoint(p1, p2): |
402 'Returns the middle of the segment [p1, p2]' | 493 'Returns the middle of the segment [p1, p2]' |
403 return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y) | 494 return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y) |