| 86 | } |
| 87 | |
| 88 | bool StraightenFilter::processOne(PointRef& point) |
| 89 | { |
| 90 | |
| 91 | double segmentX, segmentY, segmentZ, segmentM, segmentAzimuth, |
| 92 | segmentOffset; |
| 93 | if (m_args->m_unstraighten) |
| 94 | { |
| 95 | m_args->m_polyline.interpolate(point, segmentX, segmentY, segmentZ, |
| 96 | segmentM, segmentAzimuth, segmentOffset); |
| 97 | |
| 98 | const Eigen::Vector3d straight( |
| 99 | 0.0, point.getFieldAs<double>(Dimension::Id::Y), |
| 100 | point.getFieldAs<double>(Dimension::Id::Z)); |
| 101 | |
| 102 | Eigen::Affine3d t; |
| 103 | straighten::Utils::getTransformation(segmentX, segmentY, segmentZ, |
| 104 | segmentM, 0.0, |
| 105 | M_PI_2 - segmentAzimuth, t); |
| 106 | |
| 107 | const Eigen::Vector3d world = t * straight; |
| 108 | |
| 109 | point.setField(Dimension::Id::X, world.x()); |
| 110 | point.setField(Dimension::Id::Y, world.y()); |
| 111 | point.setField(Dimension::Id::Z, world.z()); |
| 112 | return true; |
| 113 | } |
| 114 | else |
| 115 | { |
| 116 | |
| 117 | if (m_args->m_polyline.closestSegment( |
| 118 | point, segmentX, segmentY, segmentZ, segmentM, segmentAzimuth, |
| 119 | segmentOffset) >= 0.0) |
| 120 | { |
| 121 | const Eigen::Vector3d world( |
| 122 | point.getFieldAs<double>(Dimension::Id::X), |
| 123 | point.getFieldAs<double>(Dimension::Id::Y), |
| 124 | point.getFieldAs<double>(Dimension::Id::Z)); |
| 125 | |
| 126 | Eigen::Affine3d t; |
| 127 | straighten::Utils::getTransformation(segmentX, segmentY, segmentZ, |
| 128 | segmentM, 0.0, |
| 129 | M_PI_2 - segmentAzimuth, t); |
| 130 | |
| 131 | const Eigen::Vector3d straight = t.inverse() * world; |
| 132 | |
| 133 | point.setField(Dimension::Id::X, |
| 134 | straight.x() + segmentOffset + m_args->m_offset); |
| 135 | point.setField(Dimension::Id::Y, straight.y()); |
| 136 | point.setField(Dimension::Id::Z, straight.z()); |
| 137 | return true; |
| 138 | } |
| 139 | } |
| 140 | return false; |
| 141 | } |
| 142 | |
| 143 | void StraightenFilter::filter(PointView& view) |
| 144 | { |
nothing calls this directly
no test coverage detected