| 1369 | } |
| 1370 | |
| 1371 | Inertia transformInertia(const Eigen::Isometry3d& _T, const Inertia& _I) |
| 1372 | { |
| 1373 | // operation count: multiplication = 186, addition = 117, subtract = 21 |
| 1374 | |
| 1375 | Inertia ret = Inertia::Identity(); |
| 1376 | |
| 1377 | double d0 = _I(0, 3) + _T(2, 3) * _I(3, 4) - _T(1, 3) * _I(3, 5); |
| 1378 | double d1 = _I(1, 3) - _T(2, 3) * _I(3, 3) + _T(0, 3) * _I(3, 5); |
| 1379 | double d2 = _I(2, 3) + _T(1, 3) * _I(3, 3) - _T(0, 3) * _I(3, 4); |
| 1380 | double d3 = _I(0, 4) + _T(2, 3) * _I(4, 4) - _T(1, 3) * _I(4, 5); |
| 1381 | double d4 = _I(1, 4) - _T(2, 3) * _I(3, 4) + _T(0, 3) * _I(4, 5); |
| 1382 | double d5 = _I(2, 4) + _T(1, 3) * _I(3, 4) - _T(0, 3) * _I(4, 4); |
| 1383 | double d6 = _I(0, 5) + _T(2, 3) * _I(4, 5) - _T(1, 3) * _I(5, 5); |
| 1384 | double d7 = _I(1, 5) - _T(2, 3) * _I(3, 5) + _T(0, 3) * _I(5, 5); |
| 1385 | double d8 = _I(2, 5) + _T(1, 3) * _I(3, 5) - _T(0, 3) * _I(4, 5); |
| 1386 | double e0 = _I(0, 0) + _T(2, 3) * _I(0, 4) - _T(1, 3) * _I(0, 5) |
| 1387 | + d3 * _T(2, 3) - d6 * _T(1, 3); |
| 1388 | double e3 = _I(0, 1) + _T(2, 3) * _I(1, 4) - _T(1, 3) * _I(1, 5) |
| 1389 | - d0 * _T(2, 3) + d6 * _T(0, 3); |
| 1390 | double e4 = _I(1, 1) - _T(2, 3) * _I(1, 3) + _T(0, 3) * _I(1, 5) |
| 1391 | - d1 * _T(2, 3) + d7 * _T(0, 3); |
| 1392 | double e6 = _I(0, 2) + _T(2, 3) * _I(2, 4) - _T(1, 3) * _I(2, 5) |
| 1393 | + d0 * _T(1, 3) - d3 * _T(0, 3); |
| 1394 | double e7 = _I(1, 2) - _T(2, 3) * _I(2, 3) + _T(0, 3) * _I(2, 5) |
| 1395 | + d1 * _T(1, 3) - d4 * _T(0, 3); |
| 1396 | double e8 = _I(2, 2) + _T(1, 3) * _I(2, 3) - _T(0, 3) * _I(2, 4) |
| 1397 | + d2 * _T(1, 3) - d5 * _T(0, 3); |
| 1398 | double f0 = _T(0, 0) * e0 + _T(1, 0) * e3 + _T(2, 0) * e6; |
| 1399 | double f1 = _T(0, 0) * e3 + _T(1, 0) * e4 + _T(2, 0) * e7; |
| 1400 | double f2 = _T(0, 0) * e6 + _T(1, 0) * e7 + _T(2, 0) * e8; |
| 1401 | double f3 = _T(0, 0) * d0 + _T(1, 0) * d1 + _T(2, 0) * d2; |
| 1402 | double f4 = _T(0, 0) * d3 + _T(1, 0) * d4 + _T(2, 0) * d5; |
| 1403 | double f5 = _T(0, 0) * d6 + _T(1, 0) * d7 + _T(2, 0) * d8; |
| 1404 | double f6 = _T(0, 1) * e0 + _T(1, 1) * e3 + _T(2, 1) * e6; |
| 1405 | double f7 = _T(0, 1) * e3 + _T(1, 1) * e4 + _T(2, 1) * e7; |
| 1406 | double f8 = _T(0, 1) * e6 + _T(1, 1) * e7 + _T(2, 1) * e8; |
| 1407 | double g0 = _T(0, 1) * d0 + _T(1, 1) * d1 + _T(2, 1) * d2; |
| 1408 | double g1 = _T(0, 1) * d3 + _T(1, 1) * d4 + _T(2, 1) * d5; |
| 1409 | double g2 = _T(0, 1) * d6 + _T(1, 1) * d7 + _T(2, 1) * d8; |
| 1410 | double g3 = _T(0, 2) * d0 + _T(1, 2) * d1 + _T(2, 2) * d2; |
| 1411 | double g4 = _T(0, 2) * d3 + _T(1, 2) * d4 + _T(2, 2) * d5; |
| 1412 | double g5 = _T(0, 2) * d6 + _T(1, 2) * d7 + _T(2, 2) * d8; |
| 1413 | double h0 = _T(0, 0) * _I(3, 3) + _T(1, 0) * _I(3, 4) + _T(2, 0) * _I(3, 5); |
| 1414 | double h1 = _T(0, 0) * _I(3, 4) + _T(1, 0) * _I(4, 4) + _T(2, 0) * _I(4, 5); |
| 1415 | double h2 = _T(0, 0) * _I(3, 5) + _T(1, 0) * _I(4, 5) + _T(2, 0) * _I(5, 5); |
| 1416 | double h3 = _T(0, 1) * _I(3, 3) + _T(1, 1) * _I(3, 4) + _T(2, 1) * _I(3, 5); |
| 1417 | double h4 = _T(0, 1) * _I(3, 4) + _T(1, 1) * _I(4, 4) + _T(2, 1) * _I(4, 5); |
| 1418 | double h5 = _T(0, 1) * _I(3, 5) + _T(1, 1) * _I(4, 5) + _T(2, 1) * _I(5, 5); |
| 1419 | |
| 1420 | ret(0, 0) = f0 * _T(0, 0) + f1 * _T(1, 0) + f2 * _T(2, 0); |
| 1421 | ret(0, 1) = f0 * _T(0, 1) + f1 * _T(1, 1) + f2 * _T(2, 1); |
| 1422 | ret(0, 2) = f0 * _T(0, 2) + f1 * _T(1, 2) + f2 * _T(2, 2); |
| 1423 | ret(0, 3) = f3 * _T(0, 0) + f4 * _T(1, 0) + f5 * _T(2, 0); |
| 1424 | ret(0, 4) = f3 * _T(0, 1) + f4 * _T(1, 1) + f5 * _T(2, 1); |
| 1425 | ret(0, 5) = f3 * _T(0, 2) + f4 * _T(1, 2) + f5 * _T(2, 2); |
| 1426 | ret(1, 1) = f6 * _T(0, 1) + f7 * _T(1, 1) + f8 * _T(2, 1); |
| 1427 | ret(1, 2) = f6 * _T(0, 2) + f7 * _T(1, 2) + f8 * _T(2, 2); |
| 1428 | ret(1, 3) = g0 * _T(0, 0) + g1 * _T(1, 0) + g2 * _T(2, 0); |
no outgoing calls
no test coverage detected