| 21 | namespace math { |
| 22 | |
| 23 | Vec3d CoordAdd(const Vec3d &a, const Vec3d &b) { |
| 24 | Vec3d c; |
| 25 | c(0) = b(0) + a(0) * std::cos(b(2)) - a(1) * std::sin(b(2)); |
| 26 | c(1) = b(1) + a(0) * std::sin(b(2)) + a(1) * std::cos(b(2)); |
| 27 | c(2) = b(2) + a(2); |
| 28 | c(2) = std::atan2(sin(c(2)), cos(c(2))); |
| 29 | return c; |
| 30 | }; |
| 31 | |
| 32 | void EigenDecomposition(const Mat3d &matrix_a, |
| 33 | Mat3d &matrix_v, |
no outgoing calls
no test coverage detected