Ego::Math::Relation plane_intersects_aabb_max(const Plane3f& plane, const Vector3f& mins, const Vector3f& maxs) { int j; float dist, tmp; Ego::Math::Relation retval = Ego::Math::Relation::error; // find the point-plane distance for the most-positive points of the aabb dist = 0.0f; for (j = 0; j < 3; j++) { tmp = (plane.getNormal()[j] > 0.0f) ? maxs[j] : mins[j]; dist += tmp * plane.getNormal()[j]; } dist += plane.getDistance(); if (dist > 0.0f) { retval = Ego::Math::Relation::inside; } else if (dist < 0.0f) { retval = Ego::Math::Relation::outside; } else { retval = Ego::Math::Relation::intersect; } return retval; }
bool three_plane_intersection(Vector3f& dst_pos, const Plane3f& p0, const Plane3f& p1, const Plane3f& p2) { Vector3f n0 = p0.getNormal(), n1 = p1.getNormal(), n2 = p2.getNormal(); float d0 = p0.getDistance(), d1 = p1.getDistance(), d2 = p2.getDistance(); // the determinant of the matrix float det = n0[kX] * (n1[kY] * n2[kZ] - n1[kZ] * n2[kY]) - n0[kY] * (n1[kX] * n2[kZ] - n2[kX] * n1[kZ]) + n0[kZ] * (n1[kX] * n2[kY] - n2[kX] * n1[kY]); // check for system that is too close to being degenerate if (std::abs(det) < 1e-6) return false; float tmp; // the x component tmp = d0 * (n1[kZ] * n2[kY] - n1[kY] * n2[kZ]) + d1 * (n0[kY] * n2[kZ] - n0[kZ] * n2[kY]) + d2 * (n0[kZ] * n1[kY] - n0[kY] * n1[kZ]); dst_pos[kX] = tmp / det; // the y component tmp = d0 * (n1[kX] * n2[kZ] - n1[kZ] * n2[kX]) + d1 * (n0[kZ] * n2[kX] - n0[kX] * n2[kZ]) + d2 * (n0[kX] * n1[kZ] - n0[kZ] * n1[kX]); dst_pos[kY] = tmp / det; // the z component tmp = d0 * (n1[kY] * n2[kX] - n1[kX] * n2[kY]) + d1 * (n0[kX] * n2[kY] - n0[kY] * n2[kX]) + d2 * (n0[kY] * n1[kX] - n0[kX] * n1[kY]); dst_pos[kZ] = tmp / det; return true; }