2 Copyright 2012-2022 Ronald Römer
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
8 http://www.apache.org/licenses/LICENSE-2.0
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.
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>
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>
45 #include <vtkCellArray.h>
47 #include "vtkPolyDataContactFilter.h"
48 #include "Utilities.h"
52 vtkStandardNewMacro(vtkPolyDataContactFilter);
54 vtkPolyDataContactFilter::vtkPolyDataContactFilter () {
56 contLines = vtkPolyData::New();
57 contLines->Allocate(1000);
59 contPts = vtkPoints::New();
60 contPts->SetDataTypeToDouble();
61 contLines->SetPoints(contPts);
63 contA = vtkIdTypeArray::New();
64 contB = vtkIdTypeArray::New();
69 sourcesA = vtkIdTypeArray::New();
70 sourcesA->SetNumberOfComponents(2);
72 sourcesB = vtkIdTypeArray::New();
73 sourcesB->SetNumberOfComponents(2);
75 sourcesA->SetName("sourcesA");
76 sourcesB->SetName("sourcesB");
78 neigsA = vtkIdTypeArray::New();
79 neigsB = vtkIdTypeArray::New();
81 neigsA->SetName("neigsA");
82 neigsB->SetName("neigsB");
84 SetNumberOfInputPorts(2);
85 SetNumberOfOutputPorts(3);
90 accuracy = vtkIdTypeArray::New();
91 accuracy->SetName("accuracy");
94 vtkPolyDataContactFilter::~vtkPolyDataContactFilter () {
111 int vtkPolyDataContactFilter::ProcessRequest (vtkInformation *request, vtkInformationVector **inputVector, vtkInformationVector *outputVector) {
113 if (request->Has(vtkDemandDrivenPipeline::REQUEST_DATA())) {
115 vtkInformation *inInfoA = inputVector[0]->GetInformationObject(0);
116 vtkInformation *inInfoB = inputVector[1]->GetInformationObject(0);
118 vtkPolyData *_pdA = vtkPolyData::SafeDownCast(inInfoA->Get(vtkDataObject::DATA_OBJECT()));
119 vtkPolyData *_pdB = vtkPolyData::SafeDownCast(inInfoB->Get(vtkDataObject::DATA_OBJECT()));
121 vtkInformation *outInfoA = outputVector->GetInformationObject(0);
122 vtkInformation *outInfoB = outputVector->GetInformationObject(1);
123 vtkInformation *outInfoC = outputVector->GetInformationObject(2);
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()));
129 // durchführung der aufgabe
131 pdA = vtkPolyData::New();
134 pdB = vtkPolyData::New();
137 PreparePolyData(pdA);
138 PreparePolyData(pdB);
140 if (pdA->GetNumberOfCells() == 0) {
141 vtkErrorMacro("First input does not contain any supported cells.");
145 if (pdB->GetNumberOfCells() == 0) {
146 vtkErrorMacro("Second input does not contain any supported cells.");
150 GetInvalidEdges(pdA, edgesA);
151 GetInvalidEdges(pdB, edgesB);
153 // anlegen der obb-trees
155 vtkOBBTree *obbA = vtkOBBTree::New();
156 obbA->SetDataSet(pdA);
157 obbA->SetNumberOfCellsPerNode(1);
158 obbA->BuildLocator();
160 vtkOBBTree *obbB = vtkOBBTree::New();
161 obbB->SetDataSet(pdB);
162 obbB->SetNumberOfCellsPerNode(1);
163 obbB->BuildLocator();
165 vtkMatrix4x4 *mat = vtkMatrix4x4::New();
167 obbA->IntersectWithOBBTree(obbB, mat, InterOBBNodes, this);
170 vtkErrorMacro("First input has non-manifold edges.");
175 vtkErrorMacro("Second input has non-manifold edges.");
179 contLines->GetCellData()->AddArray(contA);
180 contLines->GetCellData()->AddArray(contB);
182 contLines->GetCellData()->AddArray(sourcesA);
183 contLines->GetCellData()->AddArray(sourcesB);
185 contLines->GetPointData()->AddArray(accuracy);
187 contLines->RemoveDeletedCells();
189 vtkCleanPolyData *clean = vtkCleanPolyData::New();
190 clean->SetInputData(contLines);
191 clean->ToleranceIsAbsoluteOn();
192 clean->SetAbsoluteTolerance(1e-5);
195 resultA->DeepCopy(clean->GetOutput());
197 vtkIdType i, numCellsA = resultA->GetNumberOfCells();
199 for (i = 0; i < numCellsA; i++) {
200 if (resultA->GetCellType(i) != VTK_LINE) {
201 resultA->DeleteCell(i);
205 resultA->RemoveDeletedCells();
212 resultB->DeepCopy(pdA);
213 resultC->DeepCopy(pdB);
224 void vtkPolyDataContactFilter::PreparePolyData (vtkPolyData *pd) {
226 pd->GetCellData()->Initialize();
227 pd->GetPointData()->Initialize();
229 vtkIdTypeArray *cellIds = vtkIdTypeArray::New();
231 vtkCellIterator *cellItr = pd->NewCellIterator();
232 for (cellItr->InitTraversal(); !cellItr->IsDoneWithTraversal(); cellItr->GoToNextCell()) {
233 cellIds->InsertNextValue(cellItr->GetCellId());
238 for (cellItr->InitTraversal(); !cellItr->IsDoneWithTraversal(); cellItr->GoToNextCell()) {
239 cellId = cellItr->GetCellId();
241 if (cellItr->GetCellType() == VTK_QUAD) {
242 vtkIdList *ptIds = cellItr->GetPointIds();
244 vtkPoints *pts = cellItr->GetPoints();
248 ComputeNormal(pd->GetPoints(), n, 4, ptIds->GetPointer(0));
250 double dA = vtkMath::Dot(n, pts->GetPoint(0)),
251 dB = vtkMath::Dot(n, pts->GetPoint(1))-dA;
253 if (std::abs(dB) > 1e-6) {
254 // nur wenn nicht auf einer ebene
256 dA = vtkMath::Distance2BetweenPoints(pts->GetPoint(0), pts->GetPoint(2));
257 dB = vtkMath::Distance2BetweenPoints(pts->GetPoint(1), pts->GetPoint(3));
259 vtkSmartPointer<vtkIdList> newCellA = vtkSmartPointer<vtkIdList>::New();
260 vtkSmartPointer<vtkIdList> newCellB = vtkSmartPointer<vtkIdList>::New();
262 newCellA->SetNumberOfIds(3);
263 newCellB->SetNumberOfIds(3);
266 newCellA->SetId(0, ptIds->GetId(1));
267 newCellA->SetId(1, ptIds->GetId(2));
268 newCellA->SetId(2, ptIds->GetId(3));
270 newCellB->SetId(0, ptIds->GetId(3));
271 newCellB->SetId(1, ptIds->GetId(0));
272 newCellB->SetId(2, ptIds->GetId(1));
274 newCellA->SetId(0, ptIds->GetId(0));
275 newCellA->SetId(1, ptIds->GetId(1));
276 newCellA->SetId(2, ptIds->GetId(2));
278 newCellB->SetId(0, ptIds->GetId(2));
279 newCellB->SetId(1, ptIds->GetId(3));
280 newCellB->SetId(2, ptIds->GetId(0));
283 pd->InsertNextCell(VTK_TRIANGLE, newCellA);
284 cellIds->InsertNextValue(cellId);
286 pd->InsertNextCell(VTK_TRIANGLE, newCellB);
287 cellIds->InsertNextValue(cellId);
289 pd->DeleteCell(cellId);
292 } else if (cellItr->GetCellType() == VTK_TRIANGLE_STRIP) {
293 vtkIdList *ptIds = cellItr->GetPointIds();
295 vtkCellArray *cells = vtkCellArray::New();
297 vtkTriangleStrip::DecomposeStrip(cellItr->GetNumberOfPoints(), ptIds->GetPointer(0), cells);
300 const vtkIdType *pts;
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);
312 pd->DeleteCell(cellId);
314 } else if (cellItr->GetCellType() != VTK_TRIANGLE && cellItr->GetCellType() != VTK_POLYGON) {
315 pd->DeleteCell(cellId);
321 cellIds->SetName("OrigCellIds");
322 pd->GetCellData()->SetScalars(cellIds);
326 pd->RemoveDeletedCells();
331 void vtkPolyDataContactFilter::GetInvalidEdges (vtkPolyData *pd, InvalidEdgesType &edges) {
332 vtkSmartPointer<vtkFeatureEdges> features = vtkSmartPointer<vtkFeatureEdges>::New();
333 features->SetInputData(pd);
335 features->BoundaryEdgesOff();
336 features->FeatureEdgesOff();
337 features->ManifoldEdgesOff();
338 features->NonManifoldEdgesOn();
342 vtkPolyData *lines = features->GetOutput();
345 const vtkIdType *line;
347 double ptA[3], ptB[3];
351 auto lineItr = vtk::TakeSmartPointer(lines->GetLines()->NewIterator());
353 for (lineItr->GoToFirstCell(); !lineItr->IsDoneWithTraversal(); lineItr->GoToNextCell()) {
354 lineItr->GetCurrentCell(num, line);
356 lines->GetPoint(line[0], ptA);
357 lines->GetPoint(line[1], ptB);
359 idA = pd->FindPoint(ptA);
360 idB = pd->FindPoint(ptB);
362 edges.emplace(idA, idB);
363 edges.emplace(idB, idA);
368 void vtkPolyDataContactFilter::InterEdgeLine (InterPtsType &interPts, vtkPolyData *pd, vtkIdType idA, vtkIdType idB, const double *r, const double *ptA, Src src) {
371 pd->GetPoint(idA, eA);
372 pd->GetPoint(idB, eB);
375 vtkMath::Add(ptA, r, ptB);
377 // richtungsvektor der kante bestimmen
380 vtkMath::Subtract(eB, eA, e);
381 double l = vtkMath::Normalize(e);
384 vtkMath::Subtract(eA, ptA, p);
386 double w = std::abs(vtkMath::Determinant3x3(r, e, p));
388 if (w < 1e-4) { // ~89.995deg
389 // schnittpunkt ermitteln
392 vtkMath::Cross(r, e, v);
394 double n = vtkMath::Norm(v);
396 if (n > 1e-4) { // ~0.0057deg
398 double s = vtkMath::Determinant3x3(p, r, v)/(n*n);
400 if (s > -1e-6 && s < l+1e-6) {
401 double t = vtkMath::Determinant3x3(p, e, v)/(n*n);
405 if (s > -1e-6 && s < 1e-6) {
407 } else if (s > l-1e-6 && s < l+1e-6) {
411 interPts.emplace_back(ptA[0]+t*r[0], ptA[1]+t*r[1], ptA[2]+t*r[2], t, idA, idB, end, src);
418 double vA[3], vB[3], cA[3], cB[3], dA, dB;
420 vtkMath::Subtract(eA, ptA, vA);
421 vtkMath::Subtract(eA, ptB, vB);
422 vtkMath::Cross(vA, vB, cA);
424 double dotA = vtkMath::Dot(vA, r);
426 vtkMath::Subtract(eB, ptA, vA);
427 vtkMath::Subtract(eB, ptB, vB);
428 vtkMath::Cross(vA, vB, cB);
430 double dotB = vtkMath::Dot(vA, r);
432 dA = vtkMath::Norm(cA);
433 dB = vtkMath::Norm(cB);
435 if (dA < 1e-4 || dB < 1e-4) {
437 std::cout << "congruent lines with d (" << dA << ", " << dB << ") and t (" << dotA << ", " << dotB << ") and l " << l << std::endl;
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);
453 void vtkPolyDataContactFilter::InterPolyLine (InterPtsType &interPts, vtkPolyData *pd, vtkIdType num, const vtkIdType *poly, const double *r, const double *pt, Src src, const double *n) {
456 std::cout << "InterPolyLine()" << std::endl;
459 std::vector<Pair> edges;
460 edges.reserve(static_cast<std::size_t>(num));
464 for (i = 0; i < num; i++) {
471 const vtkIdType &a = poly[i],
474 vtkPolyDataContactFilter::InterEdgeLine(interPts, pd, a, b, r, pt, src);
476 edges.emplace_back(a, b);
480 if (interPts.empty()) {
485 bool operator() (const double &l, const double &r) const {
486 long a = std::lround(l*1e5),
487 b = std::lround(r*1e5);
493 std::map<double, InterPtsType, Cmp> paired;
495 for (auto &p : interPts) {
496 paired[p.t].push_back(p);
499 std::vector<InterPtsType> _paired;
501 for (auto &p : paired) {
502 InterPtsType &pts = p.second;
504 if (pts.size() == 1 && pts.front().end != End::NONE) {
505 // hier fehlt der zweite punkt
506 pts.push_back(pts.back());
509 _paired.push_back(pts);
514 if (_paired.front().size() == 2) {
515 _paired.front().pop_back();
518 if (_paired.back().size() == 2) {
519 _paired.back().pop_back();
524 std::map<vtkIdType, double> ends;
526 for (const auto &pts : _paired) {
527 auto &last = pts.back();
529 if (last.end != End::NONE) {
530 ends.emplace(last.end == End::BEGIN ? last.edge.f : last.edge.g, last.t);
536 vtkMath::Cross(n, r, s);
537 d = vtkMath::Dot(s, pt);
539 double ptA[3], ptB[3], dA, dB, vA[3], vB[3], tA, tB;
541 vtkIdType id, prev, next;
543 for (auto &pts : _paired) {
544 auto &last = pts.back();
546 if (last.end != End::NONE) {
547 if (last.end == End::BEGIN) {
550 prev = std::find_if(edges.begin(), edges.end(), [&id](const Pair &edge) { return edge.g == id; })->f;
554 next = std::find_if(edges.begin(), edges.end(), [&id](const Pair &edge) { return edge.f == id; })->g;
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;
562 if ((last.t > ends.at(prev) && dA > 0) || (last.t < ends.at(prev) && dA < 0)) {
569 } else if (ends.count(next) == 1 && ends.count(prev) == 0) {
570 pd->GetPoint(prev, ptB);
571 dB = vtkMath::Dot(s, ptB)-d;
573 if ((last.t > ends.at(next) && dB < 0) || (last.t < ends.at(next) && dB > 0)) {
583 if (ends.count(prev) == 0 && ends.count(next) == 0) {
584 pd->GetPoint(next, ptA);
585 pd->GetPoint(prev, ptB);
587 dA = vtkMath::Dot(s, ptA)-d;
588 dB = vtkMath::Dot(s, ptB)-d;
590 if (std::signbit(dA) != std::signbit(dB)) {
591 if (pts.size() == 2) {
596 vtkMath::Subtract(ptA, pt, vA);
597 vtkMath::Subtract(ptB, pt, vB);
599 tA = vtkMath::Dot(vA, r);
600 tB = vtkMath::Dot(vB, r);
602 if ((tB > tA) == std::signbit(dA)) {
613 InterPtsType _interPts;
615 for (const auto &pts : _paired) {
616 _interPts.insert(_interPts.end(), pts.begin(), pts.end());
619 interPts.swap(_interPts);
623 void vtkPolyDataContactFilter::InterPolys (vtkIdType idA, vtkIdType idB) {
626 std::cout << "InterPolys() -> idA " << idA << ", idB " << idB << std::endl;
629 vtkIdType numA, numB;
630 const vtkIdType *polyA, *polyB;
632 pdA->GetCellPoints(idA, numA, polyA);
633 pdB->GetCellPoints(idB, numB, polyB);
637 double nA[3], nB[3], ptA[3], ptB[3], dA, dB;
639 ComputeNormal(pdA->GetPoints(), nA, numA, polyA);
640 ComputeNormal(pdB->GetPoints(), nB, numB, polyB);
642 pdA->GetPoint(polyA[0], ptA);
643 pdB->GetPoint(polyB[0], ptB);
645 dA = vtkMath::Dot(nA, ptA);
646 dB = vtkMath::Dot(nB, ptB);
651 vtkMath::Cross(nA, nB, r);
652 vtkMath::Normalize(r);
654 // std::cout << r[0] << ", "
656 // << r[2] << std::endl;
658 // lsg. des lin. gls. mittels cramerscher regel
662 for (int j = 1; j < 3; j++) {
663 if (std::abs(r[j]) > std::abs(r[i])) {
671 inds[0] = 0; inds[1] = 2;
673 inds[0] = 0; inds[1] = 1;
676 double det = nA[inds[0]]*nB[inds[1]]-nB[inds[0]]*nA[inds[1]];
678 if (std::abs(det) < 1e-12) {
682 // ein punkt auf der schnittgeraden der beiden ebenen
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;
690 std::cout << "r [" << r[0] << ", " << r[1] << ", " << r[2] << "]"
691 << ", s [" << s[0] << ", " << s[1] << ", " << s[2] << "]"
695 InterPtsType intersA, intersB;
697 vtkPolyDataContactFilter::InterPolyLine(intersA, pdA, numA, polyA, r, s, Src::A, nA);
698 vtkPolyDataContactFilter::InterPolyLine(intersB, pdB, numB, polyB, r, s, Src::B, nB);
700 // probe, ob die schnittpunkte auf den kanten liegen
701 // bei ungenauen normalen ist das manchmal nicht der fall
703 vtkPolyDataContactFilter::CheckInters(intersA, pdA, idA, idB);
704 vtkPolyDataContactFilter::CheckInters(intersB, pdB, idA, idB);
707 std::cout << "intersA " << intersA.size()
708 << ", intersB " << intersB.size()
713 if (intersA.size() != 0 && intersB.size() != 0
714 && intersA.size()%2 == 0 && intersB.size()%2 == 0) {
716 AddContactLines(intersA, intersB, idA, idB);
721 void vtkPolyDataContactFilter::OverlapLines (OverlapsType &ols, InterPtsType &intersA, InterPtsType &intersB, vtkIdType idA, vtkIdType idB) {
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();
727 pd->GetCellEdgeNeighbors(polyId, pA.edge.f, pA.edge.g, neigs);
729 assert(neigs->GetNumberOfIds() == 1);
731 return neigs->GetId(0);
737 auto Add = [](InterPt &a, InterPt &b, InterPt &c, InterPt &d, vtkIdType neigA, vtkIdType neigB) {
741 return std::make_tuple(a, b, neigA, neigB);
744 InterPtsType::iterator itr, itr2;
746 vtkIdType neigA, neigB;
748 for (itr = intersA.begin(); itr != intersA.end(); itr += 2) {
749 neigA = GetNeig(*itr, *(itr+1), pdA, idA);
751 for (itr2 = intersB.begin(); itr2 != intersB.end(); itr2 += 2) {
752 neigB = GetNeig(*itr2, *(itr2+1), pdB, idB);
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));
758 ols.push_back(Add(*itr2, *(itr+1), *itr, *(itr2+1), neigA, neigB));
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));
764 ols.push_back(Add(*itr, *(itr2+1), *itr2, *(itr+1), neigA, neigB));
772 void vtkPolyDataContactFilter::AddContactLines (InterPtsType &intersA, InterPtsType &intersB, vtkIdType idA, vtkIdType idB) {
774 OverlapsType overlaps;
775 OverlapLines(overlaps, intersA, intersB, idA, idB);
777 OverlapsType::const_iterator itr;
779 for (itr = overlaps.begin(); itr != overlaps.end(); ++itr) {
780 auto &f = std::get<0>(*itr),
781 &s = std::get<1>(*itr);
784 std::cout << "f " << f << std::endl;
785 std::cout << "s " << s << std::endl;
788 if (f.src == Src::A) {
789 if (edgesA.count(f.edge) == 1) {
794 if (s.src == Src::A) {
795 if (edgesA.count(s.edge) == 1) {
800 if (f.src == Src::B) {
801 if (edgesB.count(f.edge) == 1) {
806 if (s.src == Src::B) {
807 if (edgesB.count(s.edge) == 1) {
812 vtkIdList *linePts = vtkIdList::New();
814 linePts->InsertNextId(contPts->InsertNextPoint(f.pt));
815 linePts->InsertNextId(contPts->InsertNextPoint(s.pt));
817 contLines->InsertNextCell(VTK_LINE, linePts);
819 const vtkIdType tupleA[] = {f.srcA, s.srcA};
820 const vtkIdType tupleB[] = {f.srcB, s.srcB};
822 sourcesA->InsertNextTypedTuple(tupleA);
823 sourcesB->InsertNextTypedTuple(tupleB);
827 contA->InsertNextValue(idA);
828 contB->InsertNextValue(idB);
830 neigsA->InsertNextValue(std::get<2>(*itr));
831 neigsB->InsertNextValue(std::get<3>(*itr));
833 accuracy->InsertNextValue(f.inaccurate);
834 accuracy->InsertNextValue(s.inaccurate);
840 int vtkPolyDataContactFilter::InterOBBNodes (vtkOBBNode *nodeA, vtkOBBNode *nodeB, vtkMatrix4x4 *vtkNotUsed(mat), void *caller) {
841 vtkPolyDataContactFilter *self = reinterpret_cast<vtkPolyDataContactFilter*>(caller);
843 vtkIdList *cellsA = nodeA->Cells;
844 vtkIdList *cellsB = nodeB->Cells;
846 vtkIdType numCellsA = cellsA->GetNumberOfIds(),
847 numCellsB = cellsB->GetNumberOfIds();
849 vtkIdType i, j, ci, cj;
851 for (i = 0; i < numCellsA; i++) {
852 ci = cellsA->GetId(i);
854 for (j = 0; j < numCellsB; j++) {
855 cj = cellsB->GetId(j);
857 self->InterPolys(ci, cj);
864 void vtkPolyDataContactFilter::CheckInters (InterPtsType &interPts, vtkPolyData *pd, vtkIdType idA, vtkIdType idB) {
874 for (auto &p : interPts) {
875 pd->GetPoint(p.edge.f, ptA);
876 pd->GetPoint(p.edge.g, ptB);
878 vtkMath::Subtract(ptA, ptB, v);
879 vtkMath::Normalize(v);
880 vtkMath::Subtract(ptA, p.pt, w);
882 k = vtkMath::Norm(w);
883 l = vtkMath::Dot(v, w);
884 alpha = std::acos(l/k);
886 if (std::isnan(alpha)) {
890 d = std::sin(alpha)*k;
896 // if (p.src == Src::A) {
897 // std::cout << "? A ";
899 // std::cout << "? B ";
902 // std::cout << idA << ", " << idB << ": "
905 // << ", [" << p.pt[0] << ", " << p.pt[1] << ", " << p.pt[2] << "], "