Transforms world coordinates to sensor.
(self, cords, sensor, move_cords=False)
| 746 | return sensor_transform |
| 747 | |
| 748 | def _world_to_sensor(self, cords, sensor, move_cords=False): |
| 749 | """ |
| 750 | Transforms world coordinates to sensor. |
| 751 | """ |
| 752 | |
| 753 | sensor_world_matrix = self.get_matrix(sensor) |
| 754 | world_sensor_matrix = np.linalg.inv(sensor_world_matrix) |
| 755 | sensor_cords = np.dot(world_sensor_matrix, cords) |
| 756 | |
| 757 | if move_cords: |
| 758 | _num_cords = range(sensor_cords.shape[1]) |
| 759 | modified_cords = np.array([]) |
| 760 | for i in _num_cords: |
| 761 | if sensor_cords[0,i] < 0: |
| 762 | for j in _num_cords: |
| 763 | if sensor_cords[0,j] > 0: |
| 764 | _direction = sensor_cords[:,i] - sensor_cords[:,j] |
| 765 | _distance = -sensor_cords[0,j] / _direction[0] |
| 766 | new_cord = sensor_cords[:,j] + _distance[0,0] * _direction * 0.9999 |
| 767 | modified_cords = np.hstack([modified_cords, new_cord]) if modified_cords.size else new_cord |
| 768 | else: |
| 769 | modified_cords = np.hstack([modified_cords, sensor_cords[:,i]]) if modified_cords.size else sensor_cords[:,i] |
| 770 | |
| 771 | return modified_cords |
| 772 | else: |
| 773 | return sensor_cords |
| 774 | |
| 775 | def get_matrix(self, transform): |
| 776 | """ |
no test coverage detected