Compute a rotation matrix from an axis and an angle. Returns 3x3 Matrix. Is the same as transformations.rotation_matrix(theta, axis). cfo, 2015/08/13
(axis, theta)
| 214 | |
| 215 | |
| 216 | def axis_angle(axis, theta): |
| 217 | """Compute a rotation matrix from an axis and an angle. |
| 218 | Returns 3x3 Matrix. |
| 219 | Is the same as transformations.rotation_matrix(theta, axis). |
| 220 | cfo, 2015/08/13 |
| 221 | |
| 222 | """ |
| 223 | if theta*theta > _EPS: |
| 224 | wx = axis[0] |
| 225 | wy = axis[1] |
| 226 | wz = axis[2] |
| 227 | costheta = numpy.cos(theta) |
| 228 | sintheta = numpy.sin(theta) |
| 229 | c_1 = 1.0 - costheta |
| 230 | wx_sintheta = wx * sintheta |
| 231 | wy_sintheta = wy * sintheta |
| 232 | wz_sintheta = wz * sintheta |
| 233 | C00 = c_1 * wx * wx |
| 234 | C01 = c_1 * wx * wy |
| 235 | C02 = c_1 * wx * wz |
| 236 | C11 = c_1 * wy * wy |
| 237 | C12 = c_1 * wy * wz |
| 238 | C22 = c_1 * wz * wz |
| 239 | R = numpy.zeros((3, 3), dtype=numpy.float64) |
| 240 | R[0, 0] = costheta + C00 |
| 241 | R[1, 0] = wz_sintheta + C01 |
| 242 | R[2, 0] = -wy_sintheta + C02 |
| 243 | R[0, 1] = -wz_sintheta + C01 |
| 244 | R[1, 1] = costheta + C11 |
| 245 | R[2, 1] = wx_sintheta + C12 |
| 246 | R[0, 2] = wy_sintheta + C02 |
| 247 | R[1, 2] = -wx_sintheta + C12 |
| 248 | R[2, 2] = costheta + C22 |
| 249 | return R |
| 250 | else: |
| 251 | return first_order_rotation(axis*theta) |
| 252 | |
| 253 | |
| 254 | def expmap_so3(rotvec): |
no test coverage detected