| 14 | } |
| 15 | |
| 16 | MatrixFr rotate_loop_3D(const MatrixFr& loop, const VectorF& dir) { |
| 17 | const Vector3F X(1, 0, 0); |
| 18 | const Vector3F Z(0, 0, 1); |
| 19 | |
| 20 | Eigen::Quaternion<Float> Q; |
| 21 | Float dir_len = dir.norm(); |
| 22 | if (dir[2] < (-1.0 + 1e-6) * dir_len) { |
| 23 | // Handle special case: dir ~= -Z |
| 24 | // Eigen::Quaternion introduce large numeric error for such cases. |
| 25 | Q = Eigen::Quaternion<Float>( |
| 26 | Eigen::AngleAxis<Float>(M_PI, Vector3F::UnitX())); |
| 27 | Q = Eigen::Quaternion<Float>::FromTwoVectors(-Z, dir) * Q; |
| 28 | } else { |
| 29 | Q.setFromTwoVectors(Z, dir); |
| 30 | if (fabs(dir[0]) > 1e-3 * dir_len && fabs(dir[1]) > 1e-3 * dir_len) { |
| 31 | // Pre-rotate the profile so all wires has consistent profile |
| 32 | // orientation. |
| 33 | Q = Q * Eigen::Quaternion<Float>::FromTwoVectors( |
| 34 | X, Vector3F(dir[0], dir[1], 0.0)); |
| 35 | } |
| 36 | } |
| 37 | |
| 38 | return (Q.toRotationMatrix() * loop.transpose()).transpose(); |
| 39 | } |
| 40 | |
| 41 | void apply_correction(const Vector3F& dir, MatrixFr& loop, |
| 42 | const Vector3F& rel_correction, const Vector3F& abs_correction, |