| 154 | } |
| 155 | |
| 156 | std::vector<Point3d> removeCollinearLegacy(const Point3dVector& points, double tol) { |
| 157 | const size_t N = points.size(); |
| 158 | if (N < 3) { |
| 159 | return points; |
| 160 | } |
| 161 | |
| 162 | std::vector<Point3d> result; |
| 163 | Point3d lastPoint = points[0]; |
| 164 | result.push_back(lastPoint); |
| 165 | |
| 166 | for (unsigned i = 1; i < N; ++i) { |
| 167 | const Point3d& currentPoint = points[i]; |
| 168 | Point3d nextPoint = points[0]; |
| 169 | if (i < N - 1) { |
| 170 | nextPoint = points[i + 1]; |
| 171 | } |
| 172 | |
| 173 | Vector3d a = (currentPoint - lastPoint); |
| 174 | Vector3d b = (nextPoint - currentPoint); |
| 175 | |
| 176 | // if these fail to normalize we have zero length vectors (e.g. adjacent points) |
| 177 | if (a.normalize()) { |
| 178 | if (b.normalize()) { |
| 179 | |
| 180 | const Vector3d c = a.cross(b); |
| 181 | if (c.length() >= tol) { |
| 182 | // cross product is significant |
| 183 | result.push_back(currentPoint); |
| 184 | lastPoint = currentPoint; |
| 185 | } else { |
| 186 | // see if dot product is near -1 |
| 187 | const double d = a.dot(b); |
| 188 | if (d <= -1.0 + tol) { |
| 189 | // this is a line reversal |
| 190 | result.push_back(currentPoint); |
| 191 | lastPoint = currentPoint; |
| 192 | } |
| 193 | } |
| 194 | } |
| 195 | } |
| 196 | } |
| 197 | |
| 198 | size_t iBegin = 0; |
| 199 | size_t iEnd = result.size(); |
| 200 | |
| 201 | bool resizeBegin = true; |
| 202 | while (resizeBegin) { |
| 203 | resizeBegin = false; |
| 204 | const unsigned newN = iEnd - iBegin; |
| 205 | if (newN > 3) { |
| 206 | Vector3d a = (result[iBegin] - result[iEnd - 1]); |
| 207 | Vector3d b = (result[iBegin + 1] - result[iBegin]); |
| 208 | if (a.normalize()) { |
| 209 | if (b.normalize()) { |
| 210 | const double d = a.dot(b); |
| 211 | if (d >= 1.0 - tol) { |
| 212 | iBegin++; |
| 213 | resizeBegin = true; |