Note that this is just here to support the above. The box we're testing is treated as centered at the origin with faces parallel to planes formed by the axes.
| 234 | // testing is treated as centered at the origin with faces parallel to |
| 235 | // planes formed by the axes. |
| 236 | bool Obb::intersectNormalized(const Segment& seg) const |
| 237 | { |
| 238 | Eigen::Vector3d p0 = seg.first; |
| 239 | Eigen::Vector3d p1 = seg.second; |
| 240 | |
| 241 | // These represent both points on the faces of this box and |
| 242 | // outward-facing normal vectors to those faces. |
| 243 | const size_t numFaces = 6; |
| 244 | std::array<Eigen::Vector3d, numFaces> faces |
| 245 | {{ |
| 246 | {m_hx, 0, 0}, |
| 247 | {-m_hx, 0, 0}, |
| 248 | {0, m_hy, 0}, |
| 249 | {0, -m_hy, 0}, |
| 250 | {0, 0, m_hz}, |
| 251 | {0, 0, -m_hz} |
| 252 | }}; |
| 253 | |
| 254 | // Faces of the base box represented as 2D areas. |
| 255 | std::array<BOX2D, 3> boxes |
| 256 | {{ |
| 257 | {-m_hy, -m_hz, m_hy, m_hz}, |
| 258 | {-m_hx, -m_hz, m_hx, m_hz}, |
| 259 | {-m_hx, -m_hy, m_hx, m_hy} |
| 260 | }}; |
| 261 | |
| 262 | // Find the 3D intersection point of the segment and each of the faces. |
| 263 | // Convert to a 2D point WRT the face and see if the point is in the 2D |
| 264 | // face. |
| 265 | for (size_t i = 0; i < numFaces; ++i) |
| 266 | { |
| 267 | Eigen::Vector3d face = faces[i]; |
| 268 | |
| 269 | Eigen::Vector3d v1 = face - p0; |
| 270 | Eigen::Vector3d v2 = p1 - p0; |
| 271 | |
| 272 | double num = v1.dot(face); |
| 273 | double den = v2.dot(face); |
| 274 | if (den == 0) |
| 275 | return false; |
| 276 | double t = num / den; |
| 277 | |
| 278 | // t is the distance on the line from p0 to p1 in parametric form |
| 279 | // where the line intersects the plane of the face. |
| 280 | Eigen::Vector3d isect = t * (p1 - p0) + p0; |
| 281 | |
| 282 | // If t < 0 or > 1, then the edge doesn't intersect the plane |
| 283 | // between p0 and p1. |
| 284 | if (t < 0 || t > 1) |
| 285 | continue; |
| 286 | |
| 287 | // We know that the edge intersects the plane of the face. Now |
| 288 | // check that it intersects in the face itself. |
| 289 | |
| 290 | // Convert our 3d point to a 2d one, ignoring the dimension |
| 291 | // in the direction of the normal. Find the coordinates of the |
| 292 | // face in 2d, ignoring the dimension in the direction of the normal. |
| 293 | double coord[2]; |
no test coverage detected