Creates matrix from carla transform.
(location, rotation)
| 201 | return lines_3d |
| 202 | |
| 203 | def get_matrix(location, rotation): |
| 204 | """ |
| 205 | Creates matrix from carla transform. |
| 206 | """ |
| 207 | pitch, roll, yaw = rotation |
| 208 | x, y, z = location |
| 209 | c_y = np.cos(np.radians(yaw)) |
| 210 | s_y = np.sin(np.radians(yaw)) |
| 211 | c_r = np.cos(np.radians(roll)) |
| 212 | s_r = np.sin(np.radians(roll)) |
| 213 | c_p = np.cos(np.radians(pitch)) |
| 214 | s_p = np.sin(np.radians(pitch)) |
| 215 | matrix = np.matrix(np.identity(4)) |
| 216 | matrix[0, 3] = x |
| 217 | matrix[1, 3] = y |
| 218 | matrix[2, 3] = z |
| 219 | matrix[0, 0] = c_p * c_y |
| 220 | matrix[0, 1] = c_y * s_p * s_r - s_y * c_r |
| 221 | matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r |
| 222 | matrix[1, 0] = s_y * c_p |
| 223 | matrix[1, 1] = s_y * s_p * s_r + c_y * c_r |
| 224 | matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r |
| 225 | matrix[2, 0] = s_p |
| 226 | matrix[2, 1] = -c_p * s_r |
| 227 | matrix[2, 2] = c_p * c_r |
| 228 | return matrix |