lib/creaVtk/vtkPolyDataContactFilter.cxx

   1 /*
   2 Copyright 2012-2022 Ronald Römer
   3
   4 Licensed under the Apache License, Version 2.0 (the "License");
   5 you may not use this file except in compliance with the License.
   6 You may obtain a copy of the License at
   7
   8     http://www.apache.org/licenses/LICENSE-2.0
   9
  10 Unless required by applicable law or agreed to in writing, software
  11 distributed under the License is distributed on an "AS IS" BASIS,
  12 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13 See the License for the specific language governing permissions and
  14 limitations under the License.
  15 */
  16
  17 #include <cmath>
  18 #include <vector>
  19 #include <map>
  20 #include <set>
  21 #include <algorithm>
  22
  23 #include <vtkInformation.h>
  24 #include <vtkInformationVector.h>
  25 #include <vtkDemandDrivenPipeline.h>
  26 #include <vtkObjectFactory.h>
  27 #include <vtkPolyDataAlgorithm.h>
  28 #include <vtkPolyData.h>
  29 #include <vtkOBBTree.h>
  30 #include <vtkMatrix4x4.h>
  31 #include <vtkIdList.h>
  32 #include <vtkPoints.h>
  33 #include <vtkMath.h>
  34 #include <vtkIdTypeArray.h>
  35 #include <vtkCellData.h>
  36 #include <vtkPointData.h>
  37 #include <vtkCleanPolyData.h>
  38 #include <vtkTriangleStrip.h>
  39 #include <vtkSmartPointer.h>
  40 #include <vtkPolyDataConnectivityFilter.h>
  41 #include <vtkFeatureEdges.h>
  42 #include <vtkCellIterator.h>
  43 #include <vtkCellArrayIterator.h>
  44
  45 #include <vtkCellArray.h>
  46
  47 #include "vtkPolyDataContactFilter.h"
  48 #include "Utilities.h"
  49
  50 #undef DEBUG
  51
  52 vtkStandardNewMacro(vtkPolyDataContactFilter);
  53
  54 vtkPolyDataContactFilter::vtkPolyDataContactFilter () {
  55
  56     contLines = vtkPolyData::New();
  57     contLines->Allocate(1000);
  58
  59     contPts = vtkPoints::New();
  60     contPts->SetDataTypeToDouble();
  61     contLines->SetPoints(contPts);
  62
  63     contA = vtkIdTypeArray::New();
  64     contB = vtkIdTypeArray::New();
  65
  66     contA->SetName("cA");
  67     contB->SetName("cB");
  68
  69     sourcesA = vtkIdTypeArray::New();
  70     sourcesA->SetNumberOfComponents(2);
  71
  72     sourcesB = vtkIdTypeArray::New();
  73     sourcesB->SetNumberOfComponents(2);
  74
  75     sourcesA->SetName("sourcesA");
  76     sourcesB->SetName("sourcesB");
  77
  78     neigsA = vtkIdTypeArray::New();
  79     neigsB = vtkIdTypeArray::New();
  80
  81     neigsA->SetName("neigsA");
  82     neigsB->SetName("neigsB");
  83
  84     SetNumberOfInputPorts(2);
  85     SetNumberOfOutputPorts(3);
  86
  87     invalidA = false;
  88     invalidB = false;
  89
  90     accuracy = vtkIdTypeArray::New();
  91     accuracy->SetName("accuracy");
  92 }
  93
  94 vtkPolyDataContactFilter::~vtkPolyDataContactFilter () {
  95     neigsB->Delete();
  96     neigsA->Delete();
  97
  98     sourcesB->Delete();
  99     sourcesA->Delete();
 100
 101     contB->Delete();
 102     contA->Delete();
 103
 104     contPts->Delete();
 105     contLines->Delete();
 106
 107     accuracy->Delete();
 108
 109 }
 110
 111 int vtkPolyDataContactFilter::ProcessRequest (vtkInformation *request, vtkInformationVector **inputVector, vtkInformationVector *outputVector) {
 112
 113     if (request->Has(vtkDemandDrivenPipeline::REQUEST_DATA())) {
 114
 115         vtkInformation *inInfoA = inputVector[0]->GetInformationObject(0);
 116         vtkInformation *inInfoB = inputVector[1]->GetInformationObject(0);
 117
 118         vtkPolyData *_pdA = vtkPolyData::SafeDownCast(inInfoA->Get(vtkDataObject::DATA_OBJECT()));
 119         vtkPolyData *_pdB = vtkPolyData::SafeDownCast(inInfoB->Get(vtkDataObject::DATA_OBJECT()));
 120
 121         vtkInformation *outInfoA = outputVector->GetInformationObject(0);
 122         vtkInformation *outInfoB = outputVector->GetInformationObject(1);
 123         vtkInformation *outInfoC = outputVector->GetInformationObject(2);
 124
 125         vtkPolyData *resultA = vtkPolyData::SafeDownCast(outInfoA->Get(vtkDataObject::DATA_OBJECT()));
 126         vtkPolyData *resultB = vtkPolyData::SafeDownCast(outInfoB->Get(vtkDataObject::DATA_OBJECT()));
 127         vtkPolyData *resultC = vtkPolyData::SafeDownCast(outInfoC->Get(vtkDataObject::DATA_OBJECT()));
 128
 129         // durchführung der aufgabe
 130
 131         pdA = vtkPolyData::New();
 132         pdA->DeepCopy(_pdA);
 133
 134         pdB = vtkPolyData::New();
 135         pdB->DeepCopy(_pdB);
 136
 137         PreparePolyData(pdA);
 138         PreparePolyData(pdB);
 139
 140         if (pdA->GetNumberOfCells() == 0) {
 141             vtkErrorMacro("First input does not contain any supported cells.");
 142             return 1;
 143         }
 144
 145         if (pdB->GetNumberOfCells() == 0) {
 146             vtkErrorMacro("Second input does not contain any supported cells.");
 147             return 1;
 148         }
 149
 150         GetInvalidEdges(pdA, edgesA);
 151         GetInvalidEdges(pdB, edgesB);
 152
 153         // anlegen der obb-trees
 154
 155         vtkOBBTree *obbA = vtkOBBTree::New();
 156         obbA->SetDataSet(pdA);
 157         obbA->SetNumberOfCellsPerNode(1);
 158         obbA->BuildLocator();
 159
 160         vtkOBBTree *obbB = vtkOBBTree::New();
 161         obbB->SetDataSet(pdB);
 162         obbB->SetNumberOfCellsPerNode(1);
 163         obbB->BuildLocator();
 164
 165         vtkMatrix4x4 *mat = vtkMatrix4x4::New();
 166
 167         obbA->IntersectWithOBBTree(obbB, mat, InterOBBNodes, this);
 168
 169         if (invalidA) {
 170             vtkErrorMacro("First input has non-manifold edges.");
 171             return 1;
 172         }
 173
 174         if (invalidB) {
 175             vtkErrorMacro("Second input has non-manifold edges.");
 176             return 1;
 177         }
 178
 179         contLines->GetCellData()->AddArray(contA);
 180         contLines->GetCellData()->AddArray(contB);
 181
 182         contLines->GetCellData()->AddArray(sourcesA);
 183         contLines->GetCellData()->AddArray(sourcesB);
 184
 185         contLines->GetPointData()->AddArray(accuracy);
 186
 187         contLines->RemoveDeletedCells();
 188
 189         vtkCleanPolyData *clean = vtkCleanPolyData::New();
 190         clean->SetInputData(contLines);
 191         clean->ToleranceIsAbsoluteOn();
 192         clean->SetAbsoluteTolerance(1e-5);
 193         clean->Update();
 194
 195         resultA->DeepCopy(clean->GetOutput());
 196
 197         vtkIdType i, numCellsA = resultA->GetNumberOfCells();
 198
 199         for (i = 0; i < numCellsA; i++) {
 200             if (resultA->GetCellType(i) != VTK_LINE) {
 201                 resultA->DeleteCell(i);
 202             }
 203         }
 204
 205         resultA->RemoveDeletedCells();
 206
 207         clean->Delete();
 208         mat->Delete();
 209         obbB->Delete();
 210         obbA->Delete();
 211
 212         resultB->DeepCopy(pdA);
 213         resultC->DeepCopy(pdB);
 214
 215         pdB->Delete();
 216         pdA->Delete();
 217
 218     }
 219
 220     return 1;
 221
 222 }
 223
 224 void vtkPolyDataContactFilter::PreparePolyData (vtkPolyData *pd) {
 225
 226     pd->GetCellData()->Initialize();
 227     pd->GetPointData()->Initialize();
 228
 229     vtkIdTypeArray *cellIds = vtkIdTypeArray::New();
 230
 231     vtkCellIterator *cellItr = pd->NewCellIterator();
 232     for (cellItr->InitTraversal(); !cellItr->IsDoneWithTraversal(); cellItr->GoToNextCell()) {
 233         cellIds->InsertNextValue(cellItr->GetCellId());
 234     }
 235
 236     vtkIdType cellId;
 237
 238     for (cellItr->InitTraversal(); !cellItr->IsDoneWithTraversal(); cellItr->GoToNextCell()) {
 239         cellId = cellItr->GetCellId();
 240
 241         if (cellItr->GetCellType() == VTK_QUAD) {
 242             vtkIdList *ptIds = cellItr->GetPointIds();
 243
 244             vtkPoints *pts = cellItr->GetPoints();
 245
 246             double n[3];
 247
 248             ComputeNormal(pd->GetPoints(), n, 4, ptIds->GetPointer(0));
 249
 250             double dA = vtkMath::Dot(n, pts->GetPoint(0)),
 251                 dB = vtkMath::Dot(n, pts->GetPoint(1))-dA;
 252
 253             if (std::abs(dB) > 1e-6) {
 254                 // nur wenn nicht auf einer ebene
 255
 256                 dA = vtkMath::Distance2BetweenPoints(pts->GetPoint(0), pts->GetPoint(2));
 257                 dB = vtkMath::Distance2BetweenPoints(pts->GetPoint(1), pts->GetPoint(3));
 258
 259                 vtkSmartPointer<vtkIdList> newCellA = vtkSmartPointer<vtkIdList>::New();
 260                 vtkSmartPointer<vtkIdList> newCellB = vtkSmartPointer<vtkIdList>::New();
 261
 262                 newCellA->SetNumberOfIds(3);
 263                 newCellB->SetNumberOfIds(3);
 264
 265                 if (dA < dB) {
 266                     newCellA->SetId(0, ptIds->GetId(1));
 267                     newCellA->SetId(1, ptIds->GetId(2));
 268                     newCellA->SetId(2, ptIds->GetId(3));
 269
 270                     newCellB->SetId(0, ptIds->GetId(3));
 271                     newCellB->SetId(1, ptIds->GetId(0));
 272                     newCellB->SetId(2, ptIds->GetId(1));
 273                 } else {
 274                     newCellA->SetId(0, ptIds->GetId(0));
 275                     newCellA->SetId(1, ptIds->GetId(1));
 276                     newCellA->SetId(2, ptIds->GetId(2));
 277
 278                     newCellB->SetId(0, ptIds->GetId(2));
 279                     newCellB->SetId(1, ptIds->GetId(3));
 280                     newCellB->SetId(2, ptIds->GetId(0));
 281                 }
 282
 283                 pd->InsertNextCell(VTK_TRIANGLE, newCellA);
 284                 cellIds->InsertNextValue(cellId);
 285
 286                 pd->InsertNextCell(VTK_TRIANGLE, newCellB);
 287                 cellIds->InsertNextValue(cellId);
 288
 289                 pd->DeleteCell(cellId);
 290             }
 291
 292         } else if (cellItr->GetCellType() == VTK_TRIANGLE_STRIP) {
 293             vtkIdList *ptIds = cellItr->GetPointIds();
 294
 295             vtkCellArray *cells = vtkCellArray::New();
 296
 297             vtkTriangleStrip::DecomposeStrip(cellItr->GetNumberOfPoints(), ptIds->GetPointer(0), cells);
 298
 299             vtkIdType n;
 300             const vtkIdType *pts;
 301
 302             for (cells->InitTraversal(); cells->GetNextCell(n, pts);) {
 303                 if (pts[0] != pts[1] && pts[1] != pts[2] && pts[2] != pts[0]) {
 304                     pd->InsertNextCell(VTK_TRIANGLE, n, pts);
 305                     cellIds->InsertNextValue(cellId);
 306                 }
 307
 308             }
 309
 310             cells->Delete();
 311
 312             pd->DeleteCell(cellId);
 313
 314         } else if (cellItr->GetCellType() != VTK_TRIANGLE && cellItr->GetCellType() != VTK_POLYGON) {
 315             pd->DeleteCell(cellId);
 316         }
 317     }
 318
 319     cellItr->Delete();
 320
 321     cellIds->SetName("OrigCellIds");
 322     pd->GetCellData()->SetScalars(cellIds);
 323
 324     cellIds->Delete();
 325
 326     pd->RemoveDeletedCells();
 327     pd->BuildLinks();
 328
 329 }
 330
 331 void vtkPolyDataContactFilter::GetInvalidEdges (vtkPolyData *pd, InvalidEdgesType &edges) {
 332     vtkSmartPointer<vtkFeatureEdges> features = vtkSmartPointer<vtkFeatureEdges>::New();
 333     features->SetInputData(pd);
 334
 335     features->BoundaryEdgesOff();
 336     features->FeatureEdgesOff();
 337     features->ManifoldEdgesOff();
 338     features->NonManifoldEdgesOn();
 339
 340     features->Update();
 341
 342     vtkPolyData *lines = features->GetOutput();
 343
 344     vtkIdType num;
 345     const vtkIdType *line;
 346
 347     double ptA[3], ptB[3];
 348
 349     vtkIdType idA, idB;
 350
 351     auto lineItr = vtk::TakeSmartPointer(lines->GetLines()->NewIterator());
 352
 353     for (lineItr->GoToFirstCell(); !lineItr->IsDoneWithTraversal(); lineItr->GoToNextCell()) {
 354         lineItr->GetCurrentCell(num, line);
 355
 356         lines->GetPoint(line[0], ptA);
 357         lines->GetPoint(line[1], ptB);
 358
 359         idA = pd->FindPoint(ptA);
 360         idB = pd->FindPoint(ptB);
 361
 362         edges.emplace(idA, idB);
 363         edges.emplace(idB, idA);
 364
 365     }
 366 }
 367
 368 void vtkPolyDataContactFilter::InterEdgeLine (InterPtsType &interPts, vtkPolyData *pd, vtkIdType idA, vtkIdType idB, const double *r, const double *ptA, Src src) {
 369     double eA[3], eB[3];
 370
 371     pd->GetPoint(idA, eA);
 372     pd->GetPoint(idB, eB);
 373
 374     double ptB[3];
 375     vtkMath::Add(ptA, r, ptB);
 376
 377     // richtungsvektor der kante bestimmen
 378
 379     double e[3];
 380     vtkMath::Subtract(eB, eA, e);
 381     double l = vtkMath::Normalize(e);
 382
 383     double p[3];
 384     vtkMath::Subtract(eA, ptA, p);
 385
 386     double w = std::abs(vtkMath::Determinant3x3(r, e, p));
 387
 388     if (w < 1e-4) { // ~89.995deg
 389         // schnittpunkt ermitteln
 390
 391         double v[3];
 392         vtkMath::Cross(r, e, v);
 393
 394         double n = vtkMath::Norm(v);
 395
 396         if (n > 1e-4) { // ~0.0057deg
 397
 398             double s = vtkMath::Determinant3x3(p, r, v)/(n*n);
 399
 400             if (s > -1e-6 && s < l+1e-6) {
 401                 double t = vtkMath::Determinant3x3(p, e, v)/(n*n);
 402
 403                 End end = End::NONE;
 404
 405                 if (s > -1e-6 && s < 1e-6) {
 406                     end = End::BEGIN;
 407                 } else if (s > l-1e-6 && s < l+1e-6) {
 408                     end = End::END;
 409                 }
 410
 411                 interPts.emplace_back(ptA[0]+t*r[0], ptA[1]+t*r[1], ptA[2]+t*r[2], t, idA, idB, end, src);
 412
 413             }
 414
 415         } else {
 416             // parallel
 417
 418             double vA[3], vB[3], cA[3], cB[3], dA, dB;
 419
 420             vtkMath::Subtract(eA, ptA, vA);
 421             vtkMath::Subtract(eA, ptB, vB);
 422             vtkMath::Cross(vA, vB, cA);
 423
 424             double dotA = vtkMath::Dot(vA, r);
 425
 426             vtkMath::Subtract(eB, ptA, vA);
 427             vtkMath::Subtract(eB, ptB, vB);
 428             vtkMath::Cross(vA, vB, cB);
 429
 430             double dotB = vtkMath::Dot(vA, r);
 431
 432             dA = vtkMath::Norm(cA);
 433             dB = vtkMath::Norm(cB);
 434
 435             if (dA < 1e-4 || dB < 1e-4) {
 436 #ifdef DEBUG
 437                 std::cout << "congruent lines with d (" << dA << ", " << dB << ") and t (" << dotA << ", " << dotB << ") and l " << l << std::endl;
 438 #endif
 439
 440                 interPts.emplace_back(ptA[0]+dotA*r[0], ptA[1]+dotA*r[1], ptA[2]+dotA*r[2], dotA, idA, idB, End::BEGIN, src);
 441                 interPts.emplace_back(ptA[0]+dotB*r[0], ptA[1]+dotB*r[1], ptA[2]+dotB*r[2], dotB, idA, idB, End::END, src);
 442
 443             }
 444
 445         }
 446
 447     } else {
 448         // windschief
 449     }
 450
 451 }
 452
 453 void vtkPolyDataContactFilter::InterPolyLine (InterPtsType &interPts, vtkPolyData *pd, vtkIdType num, const vtkIdType *poly, const double *r, const double *pt, Src src, const double *n) {
 454
 455 #ifdef DEBUG
 456     std::cout << "InterPolyLine()" << std::endl;
 457 #endif
 458
 459     std::vector<Pair> edges;
 460     edges.reserve(static_cast<std::size_t>(num));
 461
 462     vtkIdType i, j;
 463
 464     for (i = 0; i < num; i++) {
 465         j = i+1;
 466
 467         if (j == num) {
 468             j = 0;
 469         }
 470
 471         const vtkIdType &a = poly[i],
 472             &b = poly[j];
 473
 474         vtkPolyDataContactFilter::InterEdgeLine(interPts, pd, a, b, r, pt, src);
 475
 476         edges.emplace_back(a, b);
 477
 478     }
 479
 480     if (interPts.empty()) {
 481         return;
 482     }
 483
 484     struct Cmp {
 485         bool operator() (const double &l, const double &r) const {
 486             long a = std::lround(l*1e5),
 487                 b = std::lround(r*1e5);
 488
 489             return a < b;
 490         }
 491     };
 492
 493     std::map<double, InterPtsType, Cmp> paired;
 494
 495     for (auto &p : interPts) {
 496         paired[p.t].push_back(p);
 497     }
 498
 499     std::vector<InterPtsType> _paired;
 500
 501     for (auto &p : paired) {
 502         InterPtsType &pts = p.second;
 503
 504         if (pts.size() == 1 && pts.front().end != End::NONE) {
 505             // hier fehlt der zweite punkt
 506             pts.push_back(pts.back());
 507         }
 508
 509         _paired.push_back(pts);
 510     }
 511
 512     // trivial
 513
 514     if (_paired.front().size() == 2) {
 515         _paired.front().pop_back();
 516     }
 517
 518     if (_paired.back().size() == 2) {
 519         _paired.back().pop_back();
 520     }
 521
 522     // ...
 523
 524     std::map<vtkIdType, double> ends;
 525
 526     for (const auto &pts : _paired) {
 527         auto &last = pts.back();
 528
 529         if (last.end != End::NONE) {
 530             ends.emplace(last.end == End::BEGIN ? last.edge.f : last.edge.g, last.t);
 531         }
 532     }
 533
 534     double s[3], d;
 535
 536     vtkMath::Cross(n, r, s);
 537     d = vtkMath::Dot(s, pt);
 538
 539     double ptA[3], ptB[3], dA, dB, vA[3], vB[3], tA, tB;
 540
 541     vtkIdType id, prev, next;
 542
 543     for (auto &pts : _paired) {
 544         auto &last = pts.back();
 545
 546         if (last.end != End::NONE) {
 547             if (last.end == End::BEGIN) {
 548                 id = last.edge.f;
 549                 next = last.edge.g;
 550                 prev = std::find_if(edges.begin(), edges.end(), [&id](const Pair &edge) { return edge.g == id; })->f;
 551             } else {
 552                 id = last.edge.g;
 553                 prev = last.edge.f;
 554                 next = std::find_if(edges.begin(), edges.end(), [&id](const Pair &edge) { return edge.f == id; })->g;
 555             }
 556
 557             if (pts.size() == 2) {
 558                 if (ends.count(next) == 0 && ends.count(prev) == 1) {
 559                     pd->GetPoint(next, ptA);
 560                     dA = vtkMath::Dot(s, ptA)-d;
 561
 562                     if ((last.t > ends.at(prev) && dA > 0) || (last.t < ends.at(prev) && dA < 0)) {
 563                         // tasche
 564                         pts.pop_back();
 565                     }
 566
 567                     continue;
 568
 569                 } else if (ends.count(next) == 1 && ends.count(prev) == 0) {
 570                     pd->GetPoint(prev, ptB);
 571                     dB = vtkMath::Dot(s, ptB)-d;
 572
 573                     if ((last.t > ends.at(next) && dB < 0) || (last.t < ends.at(next) && dB > 0)) {
 574                         // tasche
 575                         pts.pop_back();
 576                     }
 577
 578                     continue;
 579
 580                 }
 581             }
 582
 583             if (ends.count(prev) == 0 && ends.count(next) == 0) {
 584                 pd->GetPoint(next, ptA);
 585                 pd->GetPoint(prev, ptB);
 586
 587                 dA = vtkMath::Dot(s, ptA)-d;
 588                 dB = vtkMath::Dot(s, ptB)-d;
 589
 590                 if (std::signbit(dA) != std::signbit(dB)) {
 591                     if (pts.size() == 2) {
 592                         pts.pop_back();
 593                     }
 594
 595                 } else {
 596                     vtkMath::Subtract(ptA, pt, vA);
 597                     vtkMath::Subtract(ptB, pt, vB);
 598
 599                     tA = vtkMath::Dot(vA, r);
 600                     tB = vtkMath::Dot(vB, r);
 601
 602                     if ((tB > tA) == std::signbit(dA)) {
 603                         pts.clear();
 604                     }
 605
 606                 }
 607             }
 608         }
 609     }
 610
 611     // ...
 612
 613     InterPtsType _interPts;
 614
 615     for (const auto &pts : _paired) {
 616         _interPts.insert(_interPts.end(), pts.begin(), pts.end());
 617     }
 618
 619     interPts.swap(_interPts);
 620
 621 }
 622
 623 void vtkPolyDataContactFilter::InterPolys (vtkIdType idA, vtkIdType idB) {
 624
 625 #ifdef DEBUG
 626     std::cout << "InterPolys() -> idA " << idA << ", idB " << idB << std::endl;
 627 #endif
 628
 629     vtkIdType numA, numB;
 630     const vtkIdType *polyA, *polyB;
 631
 632     pdA->GetCellPoints(idA, numA, polyA);
 633     pdB->GetCellPoints(idB, numB, polyB);
 634
 635     // ebenen aufstellen
 636
 637     double nA[3], nB[3], ptA[3], ptB[3], dA, dB;
 638
 639     ComputeNormal(pdA->GetPoints(), nA, numA, polyA);
 640     ComputeNormal(pdB->GetPoints(), nB, numB, polyB);
 641
 642     pdA->GetPoint(polyA[0], ptA);
 643     pdB->GetPoint(polyB[0], ptB);
 644
 645     dA = vtkMath::Dot(nA, ptA);
 646     dB = vtkMath::Dot(nB, ptB);
 647
 648     // richtungsvektor
 649
 650     double r[3];
 651     vtkMath::Cross(nA, nB, r);
 652     vtkMath::Normalize(r);
 653
 654     // std::cout << r[0] << ", "
 655     //      << r[1] << ", "
 656     //       << r[2] << std::endl;
 657
 658     // lsg. des lin. gls. mittels cramerscher regel
 659
 660     int i = 0;
 661
 662     for (int j = 1; j < 3; j++) {
 663         if (std::abs(r[j]) > std::abs(r[i])) {
 664             i = j;
 665         }
 666     }
 667
 668     int inds[] = {1, 2};
 669
 670     if (i == 1) {
 671         inds[0] = 0; inds[1] = 2;
 672     } else if (i == 2) {
 673         inds[0] = 0; inds[1] = 1;
 674     }
 675
 676     double det = nA[inds[0]]*nB[inds[1]]-nB[inds[0]]*nA[inds[1]];
 677
 678     if (std::abs(det) < 1e-12) {
 679         return;
 680     }
 681
 682     // ein punkt auf der schnittgeraden der beiden ebenen
 683
 684     double s[3];
 685     s[inds[0]] = (dA*nB[inds[1]]-dB*nA[inds[1]])/det;
 686     s[inds[1]] = (nA[inds[0]]*dB-nB[inds[0]]*dA)/det;
 687     s[i] = 0;
 688
 689 #ifdef DEBUG
 690     std::cout << "r [" << r[0] << ", " << r[1] << ", " << r[2] << "]"
 691         << ", s [" << s[0] << ", " << s[1] << ", " << s[2] << "]"
 692         << std::endl;
 693 #endif
 694
 695     InterPtsType intersA, intersB;
 696
 697     vtkPolyDataContactFilter::InterPolyLine(intersA, pdA, numA, polyA, r, s, Src::A, nA);
 698     vtkPolyDataContactFilter::InterPolyLine(intersB, pdB, numB, polyB, r, s, Src::B, nB);
 699
 700     // probe, ob die schnittpunkte auf den kanten liegen
 701     // bei ungenauen normalen ist das manchmal nicht der fall
 702
 703     vtkPolyDataContactFilter::CheckInters(intersA, pdA, idA, idB);
 704     vtkPolyDataContactFilter::CheckInters(intersB, pdB, idA, idB);
 705
 706 #ifdef DEBUG
 707     std::cout << "intersA " << intersA.size()
 708         << ", intersB " << intersB.size()
 709         << std::endl;
 710 #endif
 711
 712
 713     if (intersA.size() != 0 && intersB.size() != 0
 714         && intersA.size()%2 == 0 && intersB.size()%2 == 0) {
 715
 716         AddContactLines(intersA, intersB, idA, idB);
 717     }
 718
 719 }
 720
 721 void vtkPolyDataContactFilter::OverlapLines (OverlapsType &ols, InterPtsType &intersA, InterPtsType &intersB, vtkIdType idA, vtkIdType idB) {
 722
 723     auto GetNeig = [](const InterPt &pA, const InterPt &pB, vtkPolyData *pd, vtkIdType polyId) -> vtkIdType {
 724         if (pA.edge == pB.edge) {
 725             vtkSmartPointer<vtkIdList> neigs = vtkSmartPointer<vtkIdList>::New();
 726
 727             pd->GetCellEdgeNeighbors(polyId, pA.edge.f, pA.edge.g, neigs);
 728
 729             assert(neigs->GetNumberOfIds() == 1);
 730
 731             return neigs->GetId(0);
 732         }
 733
 734         return NOTSET;
 735     };
 736
 737     auto Add = [](InterPt &a, InterPt &b, InterPt &c, InterPt &d, vtkIdType neigA, vtkIdType neigB) {
 738         a.Merge(c);
 739         b.Merge(d);
 740
 741         return std::make_tuple(a, b, neigA, neigB);
 742     };
 743
 744     InterPtsType::iterator itr, itr2;
 745
 746     vtkIdType neigA, neigB;
 747
 748     for (itr = intersA.begin(); itr != intersA.end(); itr += 2) {
 749         neigA = GetNeig(*itr, *(itr+1), pdA, idA);
 750
 751         for (itr2 = intersB.begin(); itr2 != intersB.end(); itr2 += 2) {
 752             neigB = GetNeig(*itr2, *(itr2+1), pdB, idB);
 753
 754             if (itr->t <= itr2->t && (itr+1)->t > itr2->t) {
 755                 if ((itr2+1)->t < (itr+1)->t) {
 756                     ols.push_back(Add(*itr2, *(itr2+1), *itr, *(itr+1), neigA, neigB));
 757                 } else {
 758                     ols.push_back(Add(*itr2, *(itr+1), *itr, *(itr2+1), neigA, neigB));
 759                 }
 760             } else if (itr2->t <= itr->t && (itr2+1)->t > itr->t) {
 761                 if ((itr+1)->t < (itr2+1)->t) {
 762                     ols.push_back(Add(*itr, *(itr+1), *itr2, *(itr2+1), neigA, neigB));
 763                 } else {
 764                     ols.push_back(Add(*itr, *(itr2+1), *itr2, *(itr+1), neigA, neigB));
 765                 }
 766             }
 767         }
 768     }
 769
 770 }
 771
 772 void vtkPolyDataContactFilter::AddContactLines (InterPtsType &intersA, InterPtsType &intersB, vtkIdType idA, vtkIdType idB) {
 773
 774     OverlapsType overlaps;
 775     OverlapLines(overlaps, intersA, intersB, idA, idB);
 776
 777     OverlapsType::const_iterator itr;
 778
 779     for (itr = overlaps.begin(); itr != overlaps.end(); ++itr) {
 780         auto &f = std::get<0>(*itr),
 781             &s = std::get<1>(*itr);
 782
 783 #ifdef DEBUG
 784         std::cout << "f " << f << std::endl;
 785         std::cout << "s " << s << std::endl;
 786 #endif
 787
 788         if (f.src == Src::A) {
 789             if (edgesA.count(f.edge) == 1) {
 790                 invalidA = true;
 791             }
 792         }
 793
 794         if (s.src == Src::A) {
 795             if (edgesA.count(s.edge) == 1) {
 796                 invalidA = true;
 797             }
 798         }
 799
 800         if (f.src == Src::B) {
 801             if (edgesB.count(f.edge) == 1) {
 802                 invalidB = true;
 803             }
 804         }
 805
 806         if (s.src == Src::B) {
 807             if (edgesB.count(s.edge) == 1) {
 808                 invalidB = true;
 809             }
 810         }
 811
 812         vtkIdList *linePts = vtkIdList::New();
 813
 814         linePts->InsertNextId(contPts->InsertNextPoint(f.pt));
 815         linePts->InsertNextId(contPts->InsertNextPoint(s.pt));
 816
 817         contLines->InsertNextCell(VTK_LINE, linePts);
 818
 819         const vtkIdType tupleA[] = {f.srcA, s.srcA};
 820         const vtkIdType tupleB[] = {f.srcB, s.srcB};
 821
 822         sourcesA->InsertNextTypedTuple(tupleA);
 823         sourcesB->InsertNextTypedTuple(tupleB);
 824
 825         linePts->Delete();
 826
 827         contA->InsertNextValue(idA);
 828         contB->InsertNextValue(idB);
 829
 830         neigsA->InsertNextValue(std::get<2>(*itr));
 831         neigsB->InsertNextValue(std::get<3>(*itr));
 832
 833         accuracy->InsertNextValue(f.inaccurate);
 834         accuracy->InsertNextValue(s.inaccurate);
 835
 836     }
 837
 838 }
 839
 840 int vtkPolyDataContactFilter::InterOBBNodes (vtkOBBNode *nodeA, vtkOBBNode *nodeB, vtkMatrix4x4 *vtkNotUsed(mat), void *caller) {
 841     vtkPolyDataContactFilter *self = reinterpret_cast<vtkPolyDataContactFilter*>(caller);
 842
 843     vtkIdList *cellsA = nodeA->Cells;
 844     vtkIdList *cellsB = nodeB->Cells;
 845
 846     vtkIdType numCellsA = cellsA->GetNumberOfIds(),
 847         numCellsB = cellsB->GetNumberOfIds();
 848
 849     vtkIdType i, j, ci, cj;
 850
 851     for (i = 0; i < numCellsA; i++) {
 852         ci = cellsA->GetId(i);
 853
 854         for (j = 0; j < numCellsB; j++) {
 855             cj = cellsB->GetId(j);
 856
 857             self->InterPolys(ci, cj);
 858         }
 859     }
 860
 861     return 0;
 862 }
 863
 864 void vtkPolyDataContactFilter::CheckInters (InterPtsType &interPts, vtkPolyData *pd, vtkIdType idA, vtkIdType idB) {
 865     double ptA[3],
 866         ptB[3],
 867         v[3],
 868         w[3],
 869         k,
 870         l,
 871         alpha,
 872         d;
 873
 874     for (auto &p : interPts) {
 875         pd->GetPoint(p.edge.f, ptA);
 876         pd->GetPoint(p.edge.g, ptB);
 877
 878         vtkMath::Subtract(ptA, ptB, v);
 879         vtkMath::Normalize(v);
 880         vtkMath::Subtract(ptA, p.pt, w);
 881
 882         k = vtkMath::Norm(w);
 883         l = vtkMath::Dot(v, w);
 884         alpha = std::acos(l/k);
 885
 886         if (std::isnan(alpha)) {
 887             continue;
 888         }
 889
 890         d = std::sin(alpha)*k;
 891
 892         if (d < 1e-5) {
 893             continue;
 894         }
 895
 896         // if (p.src == Src::A) {
 897         //     std::cout << "? A ";
 898         // } else {
 899         //     std::cout << "? B ";
 900         // }
 901
 902         // std::cout << idA << ", " << idB << ": "
 903         //     << std::fixed
 904         //     << d
 905         //     << ", [" << p.pt[0] << ", " << p.pt[1] << ", " << p.pt[2] << "], "
 906         //     << p.edge
 907         //     << std::endl;
 908
 909         p.inaccurate = true;
 910     }
 911
 912 }