1 #include <mystdlib.h> 2 3 #include <linalg.hpp> 4 #include <csg.hpp> 5 6 namespace netgen 7 { 8 Face(int pi1,int pi2,int pi3,const NgArray<Point<3>> & points,int ainputnr)9 Polyhedra::Face::Face (int pi1, int pi2, int pi3, 10 const NgArray<Point<3> > & points, 11 int ainputnr) 12 { 13 inputnr = ainputnr; 14 15 pnums[0] = pi1; 16 pnums[1] = pi2; 17 pnums[2] = pi3; 18 19 20 bbox.Set (points[pi1]); 21 bbox.Add (points[pi2]); 22 bbox.Add (points[pi3]); 23 24 v1 = points[pi2] - points[pi1]; 25 v2 = points[pi3] - points[pi1]; 26 27 n = Cross (v1, v2); 28 29 nn = n; 30 nn.Normalize(); 31 // PseudoInverse (v1, v2, w1, w2); 32 33 Mat<2,3> mat; 34 Mat<3,2> inv; 35 for (int i = 0; i < 3; i++) 36 { 37 mat(0,i) = v1(i); 38 mat(1,i) = v2(i); 39 } 40 CalcInverse (mat, inv); 41 for (int i = 0; i < 3; i++) 42 { 43 w1(i) = inv(i,0); 44 w2(i) = inv(i,1); 45 } 46 } 47 48 Polyhedra()49 Polyhedra :: Polyhedra () 50 { 51 surfaceactive.SetSize(0); 52 surfaceids.SetSize(0); 53 eps_base1 = 1e-8; 54 } 55 ~Polyhedra()56 Polyhedra :: ~Polyhedra () 57 { 58 ; 59 } 60 CreateDefault()61 Primitive * Polyhedra :: CreateDefault () 62 { 63 return new Polyhedra(); 64 } 65 BoxInSolid(const BoxSphere<3> & box) const66 INSOLID_TYPE Polyhedra :: BoxInSolid (const BoxSphere<3> & box) const 67 { 68 /* 69 for (i = 1; i <= faces.Size(); i++) 70 if (FaceBoxIntersection (i, box)) 71 return DOES_INTERSECT; 72 */ 73 for (int i = 0; i < faces.Size(); i++) 74 { 75 if (!faces[i].bbox.Intersect (box)) 76 continue; 77 //(*testout) << "face " << i << endl; 78 79 const Point<3> & p1 = points[faces[i].pnums[0]]; 80 const Point<3> & p2 = points[faces[i].pnums[1]]; 81 const Point<3> & p3 = points[faces[i].pnums[2]]; 82 83 if (fabs (faces[i].nn * (p1 - box.Center())) > box.Diam()/2) 84 continue; 85 86 //(*testout) << "still in loop" << endl; 87 88 double dist2 = MinDistTP2 (p1, p2, p3, box.Center()); 89 //(*testout) << "p1 " << p1 << " p2 " << p2 << " p3 " << p3 << endl 90 // << " box.Center " << box.Center() << " box.Diam() " << box.Diam() << endl 91 // << " dist2 " << dist2 << " sqr(box.Diam()/2) " << sqr(box.Diam()/2) << endl; 92 if (dist2 < sqr (box.Diam()/2)) 93 { 94 //(*testout) << "DOES_INTERSECT" << endl; 95 return DOES_INTERSECT; 96 } 97 }; 98 99 return PointInSolid (box.Center(), 1e-3 * box.Diam()); 100 } 101 102 103 // check how many faces a ray starting in p intersects PointInSolid(const Point<3> & p,double eps) const104 INSOLID_TYPE Polyhedra :: PointInSolid (const Point<3> & p, 105 double eps) const 106 { 107 if (!poly_bbox.IsIn (p, eps)) 108 return IS_OUTSIDE; 109 110 // random (?) direction: 111 Vec<3> n(-0.424621, 0.1543, 0.89212238); 112 113 int cnt = 0; 114 for (auto & face : faces) 115 { 116 Vec<3> v0 = p - points[face.pnums[0]]; 117 118 double lam3 = face.nn * v0; 119 120 if (fabs(lam3) < eps) // point is in plance of face 121 { 122 double lam1 = face.w1 * v0; 123 double lam2 = face.w2 * v0; 124 if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1) 125 return DOES_INTERSECT; 126 } 127 else 128 { 129 double lam3 = -(face.n * v0) / (face.n * n); 130 131 if (lam3 < 0) continue; // ray goes not in direction of face 132 133 Vec<3> rs = v0 + lam3 * n; 134 135 double lam1 = face.w1 * rs; 136 double lam2 = face.w2 * rs; 137 if (lam1 >= 0 && lam2 >= 0 && lam1+lam2 <= 1) 138 cnt++; 139 } 140 } 141 142 return (cnt % 2) ? IS_INSIDE : IS_OUTSIDE; 143 } 144 145 146 147 GetTangentialSurfaceIndices(const Point<3> & p,NgArray<int> & surfind,double eps) const148 void Polyhedra :: GetTangentialSurfaceIndices (const Point<3> & p, 149 NgArray<int> & surfind, double eps) const 150 { 151 for (int i = 0; i < faces.Size(); i++) 152 { 153 auto & face = faces[i]; 154 const Point<3> & p1 = points[face.pnums[0]]; 155 156 Vec<3> v0 = p - p1; 157 double lam3 = -(face.nn * v0); // n->nn 158 159 if (fabs (lam3) > eps) continue; 160 161 double lam1 = (face.w1 * v0); 162 double lam2 = (face.w2 * v0); 163 164 if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1) 165 if (!surfind.Contains (GetSurfaceId(i))) 166 surfind.Append (GetSurfaceId(i)); 167 } 168 169 } 170 VecInSolidOld(const Point<3> & p,const Vec<3> & v,double eps) const171 INSOLID_TYPE Polyhedra :: VecInSolidOld (const Point<3> & p, 172 const Vec<3> & v, 173 double eps) const 174 { 175 NgArray<int> point_on_faces; 176 INSOLID_TYPE res(DOES_INTERSECT); 177 178 Vec<3> vn = v; 179 vn.Normalize(); 180 for (int i = 0; i < faces.Size(); i++) 181 { 182 const Point<3> & p1 = points[faces[i].pnums[0]]; 183 184 Vec<3> v0 = p - p1; 185 double lam3 = -(faces[i].nn * v0); // n->nn 186 187 188 if (fabs (lam3) > eps) continue; 189 //(*testout) << "lam3 <= eps" << endl; 190 191 double lam1 = (faces[i].w1 * v0); 192 double lam2 = (faces[i].w2 * v0); 193 194 if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1) 195 { 196 point_on_faces.Append(i); 197 198 double scal = vn * faces[i].nn; // n->nn 199 200 res = DOES_INTERSECT; 201 if (scal > eps_base1) res = IS_OUTSIDE; 202 if (scal < -eps_base1) res = IS_INSIDE; 203 } 204 } 205 206 //(*testout) << "point_on_faces.Size() " << point_on_faces.Size() 207 // << " res " << res << endl; 208 209 if (point_on_faces.Size() == 0) 210 return PointInSolid (p, 0); 211 if (point_on_faces.Size() == 1) 212 return res; 213 214 215 216 217 double mindist(0); 218 bool first = true; 219 220 for(int i=0; i<point_on_faces.Size(); i++) 221 { 222 for(int j=0; j<3; j++) 223 { 224 double dist = Dist(p,points[faces[point_on_faces[i]].pnums[j]]); 225 if(dist > eps && (first || dist < mindist)) 226 { 227 mindist = dist; 228 first = false; 229 } 230 } 231 } 232 233 Point<3> p2 = p + (1e-4*mindist) * vn; 234 res = PointInSolid (p2, eps); 235 236 // (*testout) << "mindist " << mindist << " res " << res << endl; 237 238 return res; 239 } 240 241 242 243 // check how many faces a ray starting in p+alpha*v intersects VecInSolidNew(const Point<3> & p,const Vec<3> & v,double eps,bool printing) const244 INSOLID_TYPE Polyhedra :: VecInSolidNew (const Point<3> & p, 245 const Vec<3> & v, 246 double eps, bool printing) const 247 { 248 if (!poly_bbox.IsIn (p, eps)) 249 return IS_OUTSIDE; 250 251 // random (?) direction: 252 Vec<3> n(-0.424621, 0.1543, 0.89212238); 253 254 int cnt = 0; 255 for (auto & face : faces) 256 { 257 Vec<3> v0 = p - points[face.pnums[0]]; 258 if (printing) 259 { 260 *testout << "face: "; 261 for (int j = 0; j < 3; j++) 262 *testout << points[face.pnums[j]] << " "; 263 *testout << endl; 264 } 265 double lamn = face.nn * v0; 266 267 if (fabs(lamn) < eps) // point is in plane of face 268 { 269 double lam1 = face.w1 * v0; 270 double lam2 = face.w2 * v0; 271 double lam3 = 1-lam1-lam2; 272 if (printing) 273 *testout << "lam = " << lam1 << " " << lam2 << " " << lam3 << endl; 274 if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam3 >= -eps_base1) 275 { // point is close to trianlge, perturbe by alpha*v 276 double dlamn = face.nn*v; 277 278 if (fabs(dlamn) < 1e-8) // vec also in plane 279 { 280 if (printing) 281 *testout << "tang in plane" << endl; 282 double dlam1 = face.w1 * v; 283 double dlam2 = face.w2 * v; 284 double dlam3 = -dlam1-dlam2; 285 if (printing) 286 *testout << "dlam = " << dlam1 << " " << dlam2 << " " << dlam3 << endl; 287 bool in1 = lam1 > eps_base1 || dlam1 > -eps_base1; 288 bool in2 = lam2 > eps_base1 || dlam2 > -eps_base1; 289 bool in3 = lam3 > eps_base1 || dlam3 > -eps_base1; 290 if (in1 && in2 && in3) 291 return DOES_INTERSECT; 292 } 293 else // vec out of plane 294 { 295 if (printing) 296 *testout << "out of plane"; 297 double dlamn = -(face.n * v) / (face.n * n); 298 if (printing) 299 *testout << "dlamn = " << dlamn << endl; 300 if (dlamn < 0) continue; // ray goes not in direction of face 301 302 Vec<3> drs = v + dlamn * n; 303 if (printing) 304 { 305 *testout << "drs = " << drs << endl; 306 *testout << "face.w1 = " << face.w1 << endl; 307 *testout << "face.w2 = " << face.w2 << endl; 308 } 309 310 double dlam1 = face.w1 * drs; 311 double dlam2 = face.w2 * drs; 312 double dlam3 = -dlam1-dlam2; 313 314 if (printing) 315 *testout << "dlam = " << dlam1 << " " << dlam2 << " " << dlam3 << endl; 316 317 bool in1 = lam1 > eps_base1 || dlam1 > -eps_base1; 318 bool in2 = lam2 > eps_base1 || dlam2 > -eps_base1; 319 bool in3 = lam3 > eps_base1 || dlam3 > -eps_base1; 320 321 if (in1 && in2 && in3) 322 { 323 if (printing) 324 *testout << "hit" << endl; 325 cnt++; 326 } 327 } 328 } 329 } 330 else 331 { 332 double lamn = -(face.n * v0) / (face.n * n); 333 334 if (lamn < 0) continue; // ray goes not in direction of face 335 336 Vec<3> rs = v0 + lamn * n; 337 338 double lam1 = face.w1 * rs; 339 double lam2 = face.w2 * rs; 340 double lam3 = 1-lam1-lam2; 341 if (lam1 >= 0 && lam2 >= 0 && lam3 >= 0) 342 { 343 if (printing) 344 *testout << "hit" << endl; 345 cnt++; 346 } 347 } 348 } 349 350 return (cnt % 2) ? IS_INSIDE : IS_OUTSIDE; 351 } 352 353 VecInSolid(const Point<3> & p,const Vec<3> & v,double eps) const354 INSOLID_TYPE Polyhedra :: VecInSolid (const Point<3> & p, 355 const Vec<3> & v, 356 double eps) const 357 { 358 return VecInSolidNew (p, v, eps); 359 /* 360 auto oldval = VecInSolidOld (p, v, eps); 361 auto newval = VecInSolidNew (p, v, eps); 362 if (oldval != newval) 363 { 364 *testout << "different decision: oldval = " << oldval 365 << " newval = " << newval << endl; 366 *testout << "p = " << p << ", v = " << v << endl; 367 VecInSolidNew (p, v, eps, true); 368 *testout << "Poly:" << endl; 369 for (auto & face : faces) 370 { 371 for (int j = 0; j < 3; j++) 372 *testout << points[face.pnums[j]] << " "; 373 *testout << endl; 374 } 375 } 376 return newval; 377 */ 378 } 379 380 381 382 383 384 385 386 387 /* 388 INSOLID_TYPE Polyhedra :: VecInSolid2 (const Point<3> & p, 389 const Vec<3> & v1, 390 const Vec<3> & v2, 391 double eps) const 392 { 393 INSOLID_TYPE res; 394 395 res = VecInSolid(p,v1,eps); 396 if(res != DOES_INTERSECT) 397 return res; 398 399 int point_on_n_faces = 0; 400 401 Vec<3> v1n = v1; 402 v1n.Normalize(); 403 Vec<3> v2n = v2; 404 v2n.Normalize(); 405 406 407 for (int i = 0; i < faces.Size(); i++) 408 { 409 const Point<3> & p1 = points[faces[i].pnums[0]]; 410 411 Vec<3> v0 = p - p1; 412 double lam3 = -(faces[i].n * v0); 413 414 if (fabs (lam3) > eps) continue; 415 416 double lam1 = (faces[i].w1 * v0); 417 double lam2 = (faces[i].w2 * v0); 418 419 if (lam1 >= -eps && lam2 >= -eps && lam1+lam2 <= 1+eps) 420 { 421 double scal1 = v1n * faces[i].n; 422 if (fabs (scal1) > eps) continue; 423 424 425 point_on_n_faces++; 426 427 double scal2 = v2n * faces[i].n; 428 res = DOES_INTERSECT; 429 if (scal2 > eps) res = IS_OUTSIDE; 430 if (scal2 < -eps) res = IS_INSIDE; 431 } 432 } 433 434 if (point_on_n_faces == 1) 435 return res; 436 437 cerr << "primitive::vecinsolid2 makes nonsense for polyhedra" << endl; 438 439 return Primitive :: VecInSolid2 (p, v1, v2, eps); 440 } 441 */ 442 443 444 // #define OLDVECINSOLID2 445 #ifdef OLDVECINSOLID2 VecInSolid2(const Point<3> & p,const Vec<3> & v1,const Vec<3> & v2,double eps) const446 INSOLID_TYPE Polyhedra :: VecInSolid2 (const Point<3> & p, 447 const Vec<3> & v1, 448 const Vec<3> & v2, 449 double eps) const 450 { 451 //(*testout) << "VecInSolid2 eps " << eps << endl; 452 INSOLID_TYPE res = VecInSolid(p,v1,eps); 453 //(*testout) << "VecInSolid = " <<res <<endl; 454 455 if(res != DOES_INTERSECT) 456 return res; 457 458 int point_on_n_faces = 0; 459 460 Vec<3> v1n = v1; 461 v1n.Normalize(); 462 Vec<3> v2n = v2 - (v2 * v1n) * v1n; 463 v2n.Normalize(); 464 465 double cosv2, cosv2max = -99; 466 467 468 for (int i = 0; i < faces.Size(); i++) 469 { 470 const Point<3> & p1 = points[faces[i].pnums[0]]; 471 472 Vec<3> v0 = p - p1; 473 if (fabs (faces[i].nn * v0) > eps) continue; // n->nn 474 if (fabs (v1n * faces[i].nn) > eps_base1) continue; // n->nn 475 476 double lam1 = (faces[i].w1 * v0); 477 double lam2 = (faces[i].w2 * v0); 478 479 if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1) 480 { 481 // v1 is in face 482 483 Point<3> fc = Center (points[faces[i].pnums[0]], 484 points[faces[i].pnums[1]], 485 points[faces[i].pnums[2]]); 486 487 Vec<3> vpfc = fc - p; 488 cosv2 = (v2n * vpfc) / vpfc.Length(); 489 if (cosv2 > cosv2max) 490 { 491 cosv2max = cosv2; 492 point_on_n_faces++; 493 494 double scal2 = v2n * faces[i].nn; // n->nn 495 res = DOES_INTERSECT; 496 if (scal2 > eps_base1) res = IS_OUTSIDE; 497 if (scal2 < -eps_base1) res = IS_INSIDE; 498 499 } 500 } 501 } 502 503 if (point_on_n_faces >= 1) 504 return res; 505 506 (*testout) << "primitive::vecinsolid2 makes nonsense for polyhedra" << endl; 507 cerr << "primitive::vecinsolid2 makes nonsense for polyhedra" << endl; 508 509 return Primitive :: VecInSolid2 (p, v1, v2, eps); 510 } 511 512 513 514 #else 515 516 517 // check how many faces a ray starting in p+alpha*v+alpha^2/2 v2 intersects: 518 // if p + alpha v is in plane, use v2 VecInSolid2(const Point<3> & p,const Vec<3> & v,const Vec<3> & v2,double eps) const519 INSOLID_TYPE Polyhedra :: VecInSolid2 (const Point<3> & p, 520 const Vec<3> & v, 521 const Vec<3> & v2, 522 double eps) const 523 { 524 if (!poly_bbox.IsIn (p, eps)) 525 return IS_OUTSIDE; 526 527 // random (?) direction: 528 Vec<3> n(-0.424621, 0.1543, 0.89212238); 529 530 int cnt = 0; 531 for (auto & face : faces) 532 { 533 Vec<3> v0 = p - points[face.pnums[0]]; 534 double lamn = face.nn * v0; 535 536 if (fabs(lamn) < eps) // point is in plane of face 537 { 538 double lam1 = face.w1 * v0; 539 double lam2 = face.w2 * v0; 540 double lam3 = 1-lam1-lam2; 541 542 if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam3 >= -eps_base1) 543 { // point is close to trianlge, perturbe by alpha*v 544 double dlamn = face.nn*v; 545 546 if (fabs(dlamn) < 1e-8) // vec also in plane 547 { 548 double dlam1 = face.w1 * v; 549 double dlam2 = face.w2 * v; 550 double dlam3 = -dlam1-dlam2; 551 552 bool in1 = lam1 > eps_base1 || dlam1 > -eps_base1; 553 bool in2 = lam2 > eps_base1 || dlam2 > -eps_base1; 554 bool in3 = lam3 > eps_base1 || dlam3 > -eps_base1; 555 556 // and the same thing for v2 557 if (in1 && in2 && in3) 558 { 559 double ddlamn = face.nn*v2; 560 561 if (fabs(ddlamn) < 1e-8) // vec2 also in plane 562 { 563 double ddlam1 = face.w1 * v2; 564 double ddlam2 = face.w2 * v2; 565 double ddlam3 = -ddlam1-ddlam2; 566 567 bool ddin1 = lam1 > eps_base1 || dlam1 > eps_base1 || ddlam1 > -eps_base1; 568 bool ddin2 = lam2 > eps_base1 || dlam2 > eps_base1 || ddlam2 > -eps_base1; 569 bool ddin3 = lam3 > eps_base1 || dlam3 > eps_base1 || ddlam3 > -eps_base1; 570 if (ddin1 && ddin2 && ddin3) 571 return DOES_INTERSECT; 572 } 573 else // vec2 out of plane 574 { 575 double ddlamn = -(face.n * v2) / (face.n * n); 576 if (ddlamn < 0) continue; // ray goes not in direction of face 577 578 Vec<3> drs = v; // + dlamn * n; but dlamn==0 579 Vec<3> ddrs = v2 + ddlamn * n; 580 581 double dlam1 = face.w1 * drs; 582 double dlam2 = face.w2 * drs; 583 double dlam3 = -dlam1-dlam2; 584 585 double ddlam1 = face.w1 * ddrs; 586 double ddlam2 = face.w2 * ddrs; 587 double ddlam3 = -ddlam1-ddlam2; 588 589 bool ddin1 = lam1 > eps_base1 || dlam1 > eps_base1 || ddlam1 > -eps_base1; 590 bool ddin2 = lam2 > eps_base1 || dlam2 > eps_base1 || ddlam2 > -eps_base1; 591 bool ddin3 = lam3 > eps_base1 || dlam3 > eps_base1 || ddlam3 > -eps_base1; 592 593 if (ddin1 && ddin2 && ddin3) 594 cnt++; 595 } 596 } 597 } 598 else // vec out of plane 599 { 600 double dlamn = -(face.n * v) / (face.n * n); 601 if (dlamn < 0) continue; // ray goes not in direction of face 602 603 Vec<3> drs = v + dlamn * n; 604 605 double dlam1 = face.w1 * drs; 606 double dlam2 = face.w2 * drs; 607 double dlam3 = -dlam1-dlam2; 608 609 bool in1 = lam1 > eps_base1 || dlam1 > -eps_base1; 610 bool in2 = lam2 > eps_base1 || dlam2 > -eps_base1; 611 bool in3 = lam3 > eps_base1 || dlam3 > -eps_base1; 612 613 if (in1 && in2 && in3) 614 cnt++; 615 616 } 617 } 618 } 619 else 620 { 621 double lamn = -(face.n * v0) / (face.n * n); 622 623 if (lamn < 0) continue; // ray goes not in direction of face 624 625 Vec<3> rs = v0 + lamn * n; 626 627 double lam1 = face.w1 * rs; 628 double lam2 = face.w2 * rs; 629 double lam3 = 1-lam1-lam2; 630 if (lam1 >= 0 && lam2 >= 0 && lam3 >= 0) 631 cnt++; 632 } 633 } 634 635 return (cnt % 2) ? IS_INSIDE : IS_OUTSIDE; 636 } 637 #endif 638 639 640 641 642 643 644 VecInSolid3(const Point<3> & p,const Vec<3> & v1,const Vec<3> & v2,double eps) const645 INSOLID_TYPE Polyhedra :: VecInSolid3 (const Point<3> & p, 646 const Vec<3> & v1, 647 const Vec<3> & v2, 648 double eps) const 649 { 650 return VecInSolid2 (p, v1, v2, eps); 651 } 652 VecInSolid4(const Point<3> & p,const Vec<3> & v,const Vec<3> & v2,const Vec<3> & m,double eps) const653 INSOLID_TYPE Polyhedra :: VecInSolid4 (const Point<3> & p, 654 const Vec<3> & v, 655 const Vec<3> & v2, 656 const Vec<3> & m, 657 double eps) const 658 { 659 auto res = VecInSolid2 (p, v, v2, eps); 660 661 if (res == DOES_INTERSECT) // following edge second order, let m decide 662 return VecInSolid2 (p, v, m, eps); 663 664 return res; 665 } 666 667 668 669 GetTangentialVecSurfaceIndices2(const Point<3> & p,const Vec<3> & v1,const Vec<3> & v2,NgArray<int> & surfind,double eps) const670 void Polyhedra :: GetTangentialVecSurfaceIndices2 (const Point<3> & p, const Vec<3> & v1, const Vec<3> & v2, 671 NgArray<int> & surfind, double eps) const 672 { 673 Vec<3> v1n = v1; 674 v1n.Normalize(); 675 Vec<3> v2n = v2; // - (v2 * v1n) * v1n; 676 v2n.Normalize(); 677 678 679 for (int i = 0; i < faces.Size(); i++) 680 { 681 const Point<3> & p1 = points[faces[i].pnums[0]]; 682 683 Vec<3> v0 = p - p1; 684 if (fabs (v0 * faces[i].nn) > eps) continue; // n->nn 685 if (fabs (v1n * faces[i].nn) > eps_base1) continue; // n->nn 686 if (fabs (v2n * faces[i].nn) > eps_base1) continue; // n->nn 687 688 double lam01 = (faces[i].w1 * v0); 689 double lam02 = (faces[i].w2 * v0); 690 double lam03 = 1-lam01-lam02; 691 double lam11 = (faces[i].w1 * v1); 692 double lam12 = (faces[i].w2 * v1); 693 double lam13 = -lam11-lam12; 694 double lam21 = (faces[i].w1 * v2); 695 double lam22 = (faces[i].w2 * v2); 696 double lam23 = -lam21-lam22; 697 698 bool ok1 = lam01 > eps_base1 || 699 (lam01 > -eps_base1 && lam11 > eps_base1) || 700 (lam01 > -eps_base1 && lam11 > -eps_base1 && lam21 > eps_base1); 701 702 bool ok2 = lam02 > eps_base1 || 703 (lam02 > -eps_base1 && lam12 > eps_base1) || 704 (lam02 > -eps_base1 && lam12 > -eps_base1 && lam22 > eps_base1); 705 706 bool ok3 = lam03 > eps_base1 || 707 (lam03 > -eps_base1 && lam13 > eps_base1) || 708 (lam03 > -eps_base1 && lam13 > -eps_base1 && lam23 > eps_base1); 709 710 if (ok1 && ok2 && ok3) 711 { 712 if (!surfind.Contains (GetSurfaceId(faces[i].planenr))) 713 surfind.Append (GetSurfaceId(faces[i].planenr)); 714 } 715 } 716 } 717 718 719 720 721 722 723 724 725 726 727 728 GetPrimitiveData(const char * & classname,NgArray<double> & coeffs) const729 void Polyhedra :: GetPrimitiveData (const char *& classname, 730 NgArray<double> & coeffs) const 731 { 732 classname = "Polyhedra"; 733 coeffs.SetSize(0); 734 coeffs.Append (points.Size()); 735 coeffs.Append (faces.Size()); 736 coeffs.Append (planes.Size()); 737 738 /* 739 int i, j; 740 for (i = 1; i <= planes.Size(); i++) 741 { 742 planes.Elem(i)->Print (*testout); 743 } 744 for (i = 1; i <= faces.Size(); i++) 745 { 746 (*testout) << "face " << i << " has plane " << faces.Get(i).planenr << endl; 747 for (j = 1; j <= 3; j++) 748 (*testout) << points.Get(faces.Get(i).pnums[j-1]); 749 (*testout) << endl; 750 } 751 */ 752 } 753 SetPrimitiveData(NgArray<double> &)754 void Polyhedra :: SetPrimitiveData (NgArray<double> & /* coeffs */) 755 { 756 ; 757 } 758 Reduce(const BoxSphere<3> & box)759 void Polyhedra :: Reduce (const BoxSphere<3> & box) 760 { 761 for (int i = 0; i < planes.Size(); i++) 762 surfaceactive[i] = 0; 763 764 for (int i = 0; i < faces.Size(); i++) 765 if (FaceBoxIntersection (i, box)) 766 surfaceactive[faces[i].planenr] = 1; 767 } 768 UnReduce()769 void Polyhedra :: UnReduce () 770 { 771 for (int i = 0; i < planes.Size(); i++) 772 surfaceactive[i] = 1; 773 } 774 AddPoint(const Point<3> & p)775 int Polyhedra :: AddPoint (const Point<3> & p) 776 { 777 if(points.Size() == 0) 778 poly_bbox.Set(p); 779 else 780 poly_bbox.Add(p); 781 782 points.Append (p); 783 return points.Size(); 784 } 785 AddFace(int pi1,int pi2,int pi3,int inputnum)786 int Polyhedra :: AddFace (int pi1, int pi2, int pi3, int inputnum) 787 { 788 (*testout) << "polyhedra, add face " << pi1 << ", " << pi2 << ", " << pi3 << endl; 789 790 if(pi1 == pi2 || pi2 == pi3 || pi3 == pi1) 791 { 792 ostringstream msg; 793 msg << "Illegal point numbers for polyhedron face: " << pi1+1 << ", " << pi2+1 << ", " << pi3+1; 794 throw NgException(msg.str()); 795 } 796 797 faces.Append (Face (pi1, pi2, pi3, points, inputnum)); 798 799 Point<3> p1 = points[pi1]; 800 Point<3> p2 = points[pi2]; 801 Point<3> p3 = points[pi3]; 802 803 Vec<3> v1 = p2 - p1; 804 Vec<3> v2 = p3 - p1; 805 806 Vec<3> n = Cross (v1, v2); 807 n.Normalize(); 808 809 Plane pl (p1, n); 810 // int inverse; 811 // int identicto = -1; 812 // for (int i = 0; i < planes.Size(); i++) 813 // if (pl.IsIdentic (*planes[i], inverse, 1e-9*max3(v1.Length(),v2.Length(),Dist(p2,p3)))) 814 // { 815 // if (!inverse) 816 // identicto = i; 817 // } 818 // // cout << "is identic = " << identicto << endl; 819 // identicto = -1; // changed April 10, JS 820 821 // if (identicto != -1) 822 // faces.Last().planenr = identicto; 823 // else 824 { 825 planes.Append (new Plane (p1, n)); 826 surfaceactive.Append (1); 827 surfaceids.Append (0); 828 faces.Last().planenr = planes.Size()-1; 829 } 830 831 // (*testout) << "is plane nr " << faces.Last().planenr << endl; 832 833 return faces.Size(); 834 } 835 836 837 FaceBoxIntersection(int fnr,const BoxSphere<3> & box) const838 int Polyhedra :: FaceBoxIntersection (int fnr, const BoxSphere<3> & box) const 839 { 840 /* 841 (*testout) << "check face box intersection, fnr = " << fnr << endl; 842 (*testout) << "box = " << box << endl; 843 (*testout) << "face-box = " << faces[fnr].bbox << endl; 844 */ 845 846 if (!faces[fnr].bbox.Intersect (box)) 847 return 0; 848 849 const Point<3> & p1 = points[faces[fnr].pnums[0]]; 850 const Point<3> & p2 = points[faces[fnr].pnums[1]]; 851 const Point<3> & p3 = points[faces[fnr].pnums[2]]; 852 853 double dist2 = MinDistTP2 (p1, p2, p3, box.Center()); 854 /* 855 (*testout) << "p1 = " << p1 << endl; 856 (*testout) << "p2 = " << p2 << endl; 857 (*testout) << "p3 = " << p3 << endl; 858 859 (*testout) << "box.Center() = " << box.Center() << endl; 860 (*testout) << "center = " << box.Center() << endl; 861 (*testout) << "dist2 = " << dist2 << endl; 862 (*testout) << "diam = " << box.Diam() << endl; 863 */ 864 if (dist2 < sqr (box.Diam()/2)) 865 { 866 // (*testout) << "intersect" << endl; 867 return 1; 868 } 869 return 0; 870 } 871 872 GetPolySurfs(NgArray<NgArray<int> * > & polysurfs)873 void Polyhedra :: GetPolySurfs(NgArray < NgArray<int> * > & polysurfs) 874 { 875 int maxnum = -1; 876 877 for(int i = 0; i<faces.Size(); i++) 878 { 879 if(faces[i].inputnr > maxnum) 880 maxnum = faces[i].inputnr; 881 } 882 883 polysurfs.SetSize(maxnum+1); 884 for(int i=0; i<polysurfs.Size(); i++) 885 polysurfs[i] = new NgArray<int>; 886 887 for(int i = 0; i<faces.Size(); i++) 888 polysurfs[faces[i].inputnr]->Append(faces[i].planenr); 889 } 890 891 CalcSpecialPoints(NgArray<Point<3>> & pts) const892 void Polyhedra::CalcSpecialPoints (NgArray<Point<3> > & pts) const 893 { 894 for (int i = 0; i < points.Size(); i++) 895 pts.Append (points[i]); 896 } 897 898 AnalyzeSpecialPoint(const Point<3> &,NgArray<Point<3>> &) const899 void Polyhedra :: AnalyzeSpecialPoint (const Point<3> & /* pt */, 900 NgArray<Point<3> > & /* specpts */) const 901 { 902 ; 903 } 904 SpecialPointTangentialVector(const Point<3> & p,int s1,int s2) const905 Vec<3> Polyhedra :: SpecialPointTangentialVector (const Point<3> & p, int s1, int s2) const 906 { 907 const double eps = 1e-10*poly_bbox.Diam(); 908 909 for (int fi1 = 0; fi1 < faces.Size(); fi1++) 910 for (int fi2 = 0; fi2 < faces.Size(); fi2++) 911 { 912 int si1 = faces[fi1].planenr; 913 int si2 = faces[fi2].planenr; 914 915 if (surfaceids[si1] != s1 || surfaceids[si2] != s2) continue; 916 917 //(*testout) << "check pair fi1/fi2 " << fi1 << "/" << fi2 << endl; 918 919 Vec<3> n1 = GetSurface(si1) . GetNormalVector (p); 920 Vec<3> n2 = GetSurface(si2) . GetNormalVector (p); 921 Vec<3> t = Cross (n1, n2); 922 923 //(*testout) << "t = " << t << endl; 924 925 926 /* 927 int samepts = 0; 928 for (int j = 0; j < 3; j++) 929 for (int k = 0; k < 3; k++) 930 if (Dist(points[faces[fi1].pnums[j]], 931 points[faces[fi2].pnums[k]]) < eps) 932 samepts++; 933 if (samepts < 2) continue; 934 */ 935 936 bool shareedge = false; 937 for(int j = 0; !shareedge && j < 3; j++) 938 { 939 Vec<3> v1 = points[faces[fi1].pnums[(j+1)%3]] - points[faces[fi1].pnums[j]]; 940 double smax = v1.Length(); 941 v1 *= 1./smax; 942 943 int pospos; 944 if(fabs(v1(0)) > 0.5) 945 pospos = 0; 946 else if(fabs(v1(1)) > 0.5) 947 pospos = 1; 948 else 949 pospos = 2; 950 951 double sp = (p(pospos) - points[faces[fi1].pnums[j]](pospos)) / v1(pospos); 952 if(sp < -eps || sp > smax+eps) 953 continue; 954 955 for (int k = 0; !shareedge && k < 3; k ++) 956 { 957 Vec<3> v2 = points[faces[fi2].pnums[(k+1)%3]] - points[faces[fi2].pnums[k]]; 958 v2.Normalize(); 959 if(v2 * v1 > 0) 960 v2 -= v1; 961 else 962 v2 += v1; 963 964 //(*testout) << "v2.Length2() " << v2.Length2() << endl; 965 966 if(v2.Length2() > 1e-18) 967 continue; 968 969 double sa,sb; 970 971 sa = (points[faces[fi2].pnums[k]](pospos) - points[faces[fi1].pnums[j]](pospos)) / v1(pospos); 972 sb = (points[faces[fi2].pnums[(k+1)%3]](pospos) - points[faces[fi1].pnums[j]](pospos)) / v1(pospos); 973 974 975 if(Dist(points[faces[fi1].pnums[j]] + sa*v1, points[faces[fi2].pnums[k]]) > eps) 976 continue; 977 978 if(sa > sb) 979 { 980 double aux = sa; sa = sb; sb = aux; 981 } 982 983 //testout->precision(20); 984 //(*testout) << "sa " << sa << " sb " << sb << " smax " << smax << " sp " << sp << " v1 " << v1 << endl; 985 //testout->precision(8); 986 987 988 shareedge = (sa < -eps && sb > eps) || 989 (sa < smax-eps && sb > smax+eps) || 990 (sa > -eps && sb < smax+eps); 991 992 if(!shareedge) 993 continue; 994 995 sa = max2(sa,0.); 996 sb = min2(sb,smax); 997 998 if(sp < sa+eps) 999 shareedge = (t * v1 > 0); 1000 else if (sp > sb-eps) 1001 shareedge = (t * v1 < 0); 1002 1003 } 1004 } 1005 if (!shareedge) continue; 1006 1007 t.Normalize(); 1008 1009 1010 return t; 1011 } 1012 1013 return Vec<3> (0,0,0); 1014 } 1015 1016 1017 } 1018 1019 1020