1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76
|
bool Geometry::doLinesIntersect2D(const Line& line1, const Line& line2) {
double x1 = line1.start.x;
double y1 = line1.start.y;
double z1 = line1.start.z;
double x2 = line1.end.x;
double y2 = line1.end.y;
double z2 = line1.end.z;
double x3 = line2.start.x;
double y3 = line2.start.y;
double z3 = line2.start.z;
double x4 = line2.end.x;
double y4 = line2.end.y;
double z4 = line2.end.z;
double ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
double ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1));
if (ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1) {
return true;
}
return false;
}
int Geometry::countIntersectingTriangles(Line& line) {
const double threshold = 1e-7;
int count = 0;
for (const auto& triangle : triangles) {
Line edge1, edge2, edge3;
edge1.start = { triangle.x1, triangle.y1, triangle.z1 };
edge1.end = { triangle.x2, triangle.y2, triangle.z2 };
edge2.start = { triangle.x2, triangle.y2, triangle.z2 };
edge2.end = { triangle.x3, triangle.y3, triangle.z3 };
edge3.start = { triangle.x3, triangle.y3, triangle.z3 };
edge3.end = { triangle.x1, triangle.y1, triangle.z1 };
if (doLinesIntersect3D(line, edge1) || doLinesIntersect3D(line, edge2) || doLinesIntersect3D(line, edge3)) {
count++;
continue;
}
double px = line.start.x;
double py = line.start.y;
double pz = line.start.z;
double nx = triangle.nx;
double ny = triangle.ny;
double nz = triangle.nz;
double qx = triangle.x1;
double qy = triangle.y1;
double qz = triangle.z1;
double t = ((qx - px) * nx + (qy - py) * ny + (qz - pz) * nz)/
((line.end.x - line.start.x) * nx + (line.end.y - line.start.y) * ny + (line.end.z - line.start.z) * nz);
if (t >= 0 && t <= 1) {
double ix = px + t * (line.end.x - line.start.x);
double iy = py + t * (line.end.y - line.start.y);
double iz = pz + t * (line.end.z - line.start.z);
if (isPointInsideTriangle3D(ix, iy, iz, triangle) && t > threshold && t < 1 - threshold) {
//if (isPointInsideTriangle2D(ix, iy, triangle) && t > threshold && t < 1 - threshold) {
count++;
}
}
}
return count;
}
| |