Форум на Исходниках.RU

#include <cmath>

#include <cstdio>

#include <cstdlib>

using namespace std;

typedef struct XYZ {double x; double y; double z;};

const double M_PI = 3.14159265358979323846;

const double RTOD = 180. / M_PI;

const double EPS = 1.0E-9;

void Normalize(XYZ* p) {

double r;

r = sqrt(p->x * p->x + p->y * p->y + p->z * p->z);

p->x /= r; p->y /= r; p->z /= r;

}

bool LineFacet(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ* p) {

double d;

double a1, a2, a3;

double total, denom, mu;

XYZ n, pa1, pa2, pa3;

n.x = (pb.y - pa.y) * (pc.z - pa.z) - (pb.z - pa.z) * (pc.y - pa.y);

n.y = (pb.z - pa.z) * (pc.x - pa.x) - (pb.x - pa.x) * (pc.z - pa.z);

n.z = (pb.x - pa.x) * (pc.y - pa.y) - (pb.y - pa.y) * (pc.x - pa.x);

Normalize(&n);

d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;

denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);

if (abs(denom) < EPS)

return false;

mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;

p->x = p1.x + mu * (p2.x - p1.x);

p->y = p1.y + mu * (p2.y - p1.y);

p->z = p1.z + mu * (p2.z - p1.z);

if (mu < 0 || mu > 1)

return false;

pa1.x = pa.x - p->x;

pa1.y = pa.y - p->y;

pa1.z = pa.z - p->z;

Normalize(&pa1);

pa2.x = pb.x - p->x;

pa2.y = pb.y - p->y;

pa2.z = pb.z - p->z;

Normalize(&pa2);

pa3.x = pc.x - p->x;

pa3.y = pc.y - p->y;

pa3.z = pc.z - p->z;

Normalize(&pa3);

a1 = pa1.x * pa2.x + pa1.y * pa2.y + pa1.z * pa2.z;

a2 = pa2.x * pa3.x + pa2.y * pa3.y + pa2.z * pa3.z;

a3 = pa3.x * pa1.x + pa3.y * pa1.y + pa3.z * pa1.z;

total = (acos(a1) + acos(a2) + acos(a3)) * RTOD;

if (abs(total - 360) > EPS)

return false;

return true;

}

int main() {

//#define fp stdin

FILE* fp = fopen("D:\LineFacet_test.txt", "wt");

XYZ t1, t2, t3, p1, p2, p;

int cnt = 0;

while (cnt < 5) {

t1.x = (double)rand();

t1.y = (double)rand();

t1.z = (double)rand();

t2.x = (double)rand();

t2.y = (double)rand();

t2.z = (double)rand();

t3.x = (double)rand();

t3.y = (double)rand();

t3.z = (double)rand();

p1.x = (double)rand();

p1.y = (double)rand();

p1.z = (double)rand();

p2.x = (double)rand();

p2.y = (double)rand();

p2.z = (double)rand();

if (LineFacet(p1, p2, t1, t2, t3, &p)) {

++cnt;

fprintf(fp, "%d:\n", cnt);

fprintf(fp, "Triangle:\n");

fprintf(fp, "%.0lf %.0lf %.0lf\n", t1.x, t1.y, t1.z);

fprintf(fp, "%.0lf %.0lf %.0lf\n", t2.x, t2.y, t2.z);

fprintf(fp, "%.0lf %.0lf %.0lf\n\n", t3.x, t3.y, t3.z);

fprintf(fp, "Line segment:\n");

fprintf(fp, "%.0lf %.0lf %.0lf\n", p1.x, p1.y, p1.z);

fprintf(fp, "%.0lf %.0lf %.0lf\n\n", p2.x, p2.y, p2.z);

fprintf(fp, "Point of intersection:\n");

fprintf(fp, "%.2lf %.2lf %.2lf\n\n", p.x, p.y, p.z);

}

fclose(fp);

system("pause");

return 0;

}

Triangle:

8281 4734 53

1999 26418 27938

6900 3788 18127

Line segment:

467 3728 14893

24648 22483 17807

Point of intersection:

6314.27 8263.20 15597.64

Triangle:

6038 24179 18190

29657 7958 6191

19815 22888 19156

Line segment:

11511 16202 2634

24272 20055 20328

Point of intersection:

20818.91 19012.39 15540.05

Triangle:

22646 26362 4886

18875 28433 29869

20142 23844 1416

Line segment:

21881 31998 10322

18651 10021 5699

Point of intersection:

20992.43 25952.14 9050.22

Triangle:

8313 4474 28022

2168 14018 18787

9905 17958 7391

Line segment:

10202 3625 26477

4414 9314 25824

Point of intersection:

7776.84 6008.68 26203.39

Triangle:

23216 1626 9357

8526 13357 29337

23271 23869 29361

Line segment:

12896 13022 29617

10112 12717 18696

Point of intersection:

12138.62 12939.03 26645.98