Creates matrix from carla transform.
(self, transform)
| 607 | return obst |
| 608 | |
| 609 | def get_matrix(self, transform): |
| 610 | """ |
| 611 | Creates matrix from carla transform. |
| 612 | """ |
| 613 | |
| 614 | rotation = transform.rotation |
| 615 | location = transform.location |
| 616 | c_y = np.cos(np.radians(rotation.yaw)) |
| 617 | s_y = np.sin(np.radians(rotation.yaw)) |
| 618 | c_r = np.cos(np.radians(rotation.roll)) |
| 619 | s_r = np.sin(np.radians(rotation.roll)) |
| 620 | c_p = np.cos(np.radians(rotation.pitch)) |
| 621 | s_p = np.sin(np.radians(rotation.pitch)) |
| 622 | matrix = np.matrix(np.identity(4)) |
| 623 | matrix[0, 3] = location.x |
| 624 | matrix[1, 3] = location.y |
| 625 | matrix[2, 3] = location.z |
| 626 | matrix[0, 0] = c_p * c_y |
| 627 | matrix[0, 1] = c_y * s_p * s_r - s_y * c_r |
| 628 | matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r |
| 629 | matrix[1, 0] = s_y * c_p |
| 630 | matrix[1, 1] = s_y * s_p * s_r + c_y * c_r |
| 631 | matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r |
| 632 | matrix[2, 0] = s_p |
| 633 | matrix[2, 1] = -c_p * s_r |
| 634 | matrix[2, 2] = c_p * c_r |
| 635 | return matrix |
| 636 |