(vehicle, lights, pixels_per_meter=5.5, size=512, radius=5)
| 53 | |
| 54 | |
| 55 | def get_nearby_lights(vehicle, lights, pixels_per_meter=5.5, size=512, radius=5): |
| 56 | result = list() |
| 57 | |
| 58 | transform = vehicle.get_transform() |
| 59 | pos = transform.location |
| 60 | theta = np.radians(90 + transform.rotation.yaw) |
| 61 | R = np.array([ |
| 62 | [np.cos(theta), -np.sin(theta)], |
| 63 | [np.sin(theta), np.cos(theta)], |
| 64 | ]) |
| 65 | |
| 66 | for light in lights: |
| 67 | delta = light.get_transform().location - pos |
| 68 | |
| 69 | target = R.T.dot([delta.x, delta.y]) |
| 70 | target *= pixels_per_meter |
| 71 | target += size // 2 |
| 72 | |
| 73 | if min(target) < 0 or max(target) >= size: |
| 74 | continue |
| 75 | |
| 76 | trigger = light.trigger_volume |
| 77 | light.get_transform().transform(trigger.location) |
| 78 | dist = trigger.location.distance(vehicle.get_location()) |
| 79 | a = np.sqrt( |
| 80 | trigger.extent.x ** 2 + |
| 81 | trigger.extent.y ** 2 + |
| 82 | trigger.extent.z ** 2) |
| 83 | b = np.sqrt( |
| 84 | vehicle.bounding_box.extent.x ** 2 + |
| 85 | vehicle.bounding_box.extent.y ** 2 + |
| 86 | vehicle.bounding_box.extent.z ** 2) |
| 87 | |
| 88 | if dist > a + b: |
| 89 | continue |
| 90 | |
| 91 | result.append(light) |
| 92 | |
| 93 | return result |
| 94 | |
| 95 | |
| 96 | def draw_traffic_lights(image, vehicle, lights, pixels_per_meter=5.5, size=512, radius=5): |
no test coverage detected