| 185 | } |
| 186 | |
| 187 | inline bool CheckLineSegmentsIntersection2D(const Eigen::Ref<const Eigen::Vector2d> &line1_start, |
| 188 | const Eigen::Ref<const Eigen::Vector2d> &line1_end, |
| 189 | const Eigen::Ref<const Eigen::Vector2d> &line2_start, |
| 190 | const Eigen::Ref<const Eigen::Vector2d> &line2_end, |
| 191 | Eigen::Vector2d *intersection = NULL) { |
| 192 | double s_numer, t_numer, denom, t; |
| 193 | Eigen::Vector2d line1 = line1_end - line1_start; |
| 194 | Eigen::Vector2d line2 = line2_end - line2_start; |
| 195 | |
| 196 | denom = line1.coeffRef(0) * line2.coeffRef(1) - line2.coeffRef(0) * line1.coeffRef(1); |
| 197 | if (denom == 0) { |
| 198 | return false; |
| 199 | } |
| 200 | bool denomPositive = denom > 0; |
| 201 | |
| 202 | Eigen::Vector2d aux = line1_start - line2_start; |
| 203 | |
| 204 | s_numer = line1.coeffRef(0) * aux.coeffRef(1) - line1.coeff(1) * aux.coeffRef(0); |
| 205 | if ((s_numer < 0) == denomPositive) { |
| 206 | return false; |
| 207 | } |
| 208 | |
| 209 | t_numer = line2.coeffRef(0) * aux.coeffRef(1) - line2.coeffRef(1) * aux.coeffRef(0); |
| 210 | if ((t_numer < 0) == denomPositive) { |
| 211 | return false; |
| 212 | } |
| 213 | |
| 214 | if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive)) { |
| 215 | return false; |
| 216 | } |
| 217 | |
| 218 | t = t_numer / denom; |
| 219 | if (intersection) { |
| 220 | *intersection = line1_start + t * line1; |
| 221 | } |
| 222 | |
| 223 | return true; |
| 224 | } |
| 225 | |
| 226 | inline bool CheckLineSegmentsIntersection2D(const geometry::LineSegment2D &line0, |
| 227 | const geometry::LineSegment2D &line1, |
no test coverage detected