0026485: Problem with the normals of a triangulated shape
[occt.git] / src / Poly / Poly.cxx
1 // Created on: 1995-03-06
2 // Created by: Laurent PAINNOT
3 // Copyright (c) 1995-1999 Matra Datavision
4 // Copyright (c) 1999-2014 OPEN CASCADE SAS
5 //
6 // This file is part of Open CASCADE Technology software library.
7 //
8 // This library is free software; you can redistribute it and/or modify it under
9 // the terms of the GNU Lesser General Public License version 2.1 as published
10 // by the Free Software Foundation, with special exception defined in the file
11 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
12 // distribution for complete text of the license and disclaimer of any warranty.
13 //
14 // Alternatively, this file may be used under the terms of Open CASCADE
15 // commercial license or contractual agreement.
16
17
18 #include <gp_Pnt.hxx>
19 #include <gp_Pnt2d.hxx>
20 #include <gp_XY.hxx>
21 #include <Poly.hxx>
22 #include <Poly_Array1OfTriangle.hxx>
23 #include <Poly_ListOfTriangulation.hxx>
24 #include <Poly_Polygon2D.hxx>
25 #include <Poly_Polygon3D.hxx>
26 #include <Poly_Triangle.hxx>
27 #include <Poly_Triangulation.hxx>
28 #include <Precision.hxx>
29 #include <Standard_Stream.hxx>
30 #include <TColgp_Array1OfPnt.hxx>
31 #include <TColStd_Array1OfReal.hxx>
32 #include <TShort_Array1OfShortReal.hxx>
33 #include <TShort_HArray1OfShortReal.hxx>
34
35 //=======================================================================
36 //function : Catenate
37 //purpose  : Join several triangulations to one new triangulation object
38 //=======================================================================
39 Handle(Poly_Triangulation) Poly::Catenate (const Poly_ListOfTriangulation& lstTri)
40 {
41   Standard_Integer nNodes(0);
42   Standard_Integer nTrian(0);
43
44   // Sum up the total number of nodes.
45   Poly_ListOfTriangulation::Iterator anIter(lstTri);
46   for (; anIter.More(); anIter.Next()) {
47     const Handle(Poly_Triangulation)& aTri = anIter.Value();
48     if (aTri.IsNull() == Standard_False) {
49       nNodes += aTri->NbNodes();
50       nTrian += aTri->NbTriangles();
51     }
52   }
53
54   Handle(Poly_Triangulation) aResult;
55   if (nNodes > 0) {
56     aResult = new Poly_Triangulation(nNodes, nTrian, Standard_False);
57     Standard_Integer i, iNode[3];
58     nNodes = 0;
59     nTrian = 0;
60     TColgp_Array1OfPnt&    arrNode  = aResult->ChangeNodes();
61     Poly_Array1OfTriangle& arrTrian = aResult->ChangeTriangles();
62     for (anIter.Init(lstTri); anIter.More(); anIter.Next()) {
63       const Handle(Poly_Triangulation)& aTri = anIter.Value();
64       if (aTri.IsNull() == Standard_False) {
65         const TColgp_Array1OfPnt&    srcNode  = aTri->Nodes();
66         const Poly_Array1OfTriangle& srcTrian = aTri->Triangles();
67         const Standard_Integer nbNodes = aTri->NbNodes(); 
68         const Standard_Integer nbTrian = aTri->NbTriangles(); 
69         for (i = 1; i <= nbNodes; i++) {
70           arrNode.SetValue(i + nNodes, srcNode(i));
71         }
72         for (i = 1; i <= nbTrian; i++) {
73           srcTrian(i).Get(iNode[0], iNode[1], iNode[2]);
74           arrTrian.SetValue(i + nTrian, Poly_Triangle(iNode[0] + nNodes,
75                                                       iNode[1] + nNodes,
76                                                       iNode[2] + nNodes));
77         }
78         nNodes += nbNodes;
79         nTrian += nbTrian;
80       }
81     }
82   }
83   return aResult;
84 }
85
86 //=======================================================================
87 //function : Write
88 //purpose  :
89 //=======================================================================
90
91 void Poly::Write(const Handle(Poly_Triangulation)& T,
92                  Standard_OStream& OS,
93                  const Standard_Boolean Compact)
94 {
95   OS << "Poly_Triangulation\n";
96   if (Compact) {
97     OS << T->NbNodes() << " " << T->NbTriangles() << " ";
98     OS << ((T->HasUVNodes()) ? "1" : "0") << "\n";
99   }
100   else {
101     OS << setw(8) << T->NbNodes() << " Nodes\n";
102     OS << setw(8) << T->NbTriangles() << " Triangles\n";
103     OS << ((T->HasUVNodes()) ? "with" : "without") << " UV nodes\n";
104   }
105
106   // write the deflection
107
108   if (!Compact) OS << "Deflection : ";
109   OS << T->Deflection() << "\n";
110
111   // write the 3d nodes
112
113   if (!Compact) OS << "\n3D Nodes :\n";
114
115   Standard_Integer i, nbNodes = T->NbNodes();
116   const TColgp_Array1OfPnt& Nodes = T->Nodes();
117   for (i = 1; i <= nbNodes; i++) {
118     if (!Compact) OS << setw(10) << i << " : ";
119     if (!Compact) OS << setw(17);
120     OS << Nodes(i).X() << " ";
121     if (!Compact) OS << setw(17);
122     OS << Nodes(i).Y() << " ";
123     if (!Compact) OS << setw(17);
124     OS << Nodes(i).Z() << "\n";
125   }
126
127   if (T->HasUVNodes()) {
128     if (!Compact) OS << "\nUV Nodes :\n";
129     const TColgp_Array1OfPnt2d& UVNodes = T->UVNodes();
130     for (i = 1; i <= nbNodes; i++) {
131       if (!Compact) OS << setw(10) << i << " : ";
132     if (!Compact) OS << setw(17);
133       OS << UVNodes(i).X() << " ";
134     if (!Compact) OS << setw(17);
135       OS << UVNodes(i).Y() << "\n";
136     }
137   }
138
139   if (!Compact) OS << "\nTriangles :\n";
140   Standard_Integer nbTriangles = T->NbTriangles();
141   Standard_Integer n1, n2, n3;
142   const Poly_Array1OfTriangle& Triangles = T->Triangles();
143   for (i = 1; i <= nbTriangles; i++) {
144     if (!Compact) OS << setw(10) << i << " : ";
145     Triangles(i).Get(n1, n2, n3);
146     if (!Compact) OS << setw(10);
147     OS << n1 << " ";
148     if (!Compact) OS << setw(10);
149     OS << n2 << " ";
150     if (!Compact) OS << setw(10);
151     OS << n3 << "\n";
152   }
153
154 }
155
156 //=======================================================================
157 //function : Write
158 //purpose  :
159 //=======================================================================
160
161 void Poly::Write(const Handle(Poly_Polygon3D)& P,
162                  Standard_OStream&             OS,
163                  const Standard_Boolean        Compact)
164 {
165   OS << "Poly_Polygon3D\n";
166   if (Compact) {
167     OS << P->NbNodes() << " ";
168     OS << ((P->HasParameters()) ? "1" : "0") << "\n";
169   }
170   else {
171     OS << setw(8) << P->NbNodes() << " Nodes\n";
172     OS << ((P->HasParameters()) ? "with" : "without") << " parameters\n";
173   }
174
175   // write the deflection
176
177   if (!Compact) OS << "Deflection : ";
178   OS << P->Deflection() << "\n";
179
180   // write the nodes
181
182   if (!Compact) OS << "\nNodes :\n";
183
184   Standard_Integer i, nbNodes = P->NbNodes();
185   const TColgp_Array1OfPnt& Nodes = P->Nodes();
186   for (i = 1; i <= nbNodes; i++) {
187     if (!Compact) OS << setw(10) << i << " : ";
188     if (!Compact) OS << setw(17);
189     OS << Nodes(i).X() << " ";
190     if (!Compact) OS << setw(17);
191     OS << Nodes(i).Y() << " ";
192     if (!Compact) OS << setw(17);
193     OS << Nodes(i).Z() << "\n";
194   }
195
196   if (P->HasParameters()) {
197     if (!Compact) OS << "\nParameters :\n";
198     const TColStd_Array1OfReal& Param = P->Parameters();
199     for (i = 1; i <= nbNodes; i++) {
200       OS << Param(i) << " ";
201     }
202     OS <<"\n";
203   }
204
205
206 }
207
208
209 //=======================================================================
210 //function : Write
211 //purpose  :
212 //=======================================================================
213
214 void Poly::Write(const Handle(Poly_Polygon2D)& P,
215                  Standard_OStream&             OS,
216                  const Standard_Boolean        Compact)
217 {
218   OS << "Poly_Polygon2D\n";
219   if (Compact) {
220     OS << P->NbNodes() << " ";
221   }
222   else {
223     OS << setw(8) << P->NbNodes() << " Nodes\n";
224   }
225
226   // write the deflection
227
228   if (!Compact) OS << "Deflection : ";
229   OS << P->Deflection() << "\n";
230
231   // write the nodes
232
233   if (!Compact) OS << "\nNodes :\n";
234
235   Standard_Integer i, nbNodes = P->NbNodes();
236   const TColgp_Array1OfPnt2d& Nodes = P->Nodes();
237   for (i = 1; i <= nbNodes; i++) {
238     if (!Compact) OS << setw(10) << i << " : ";
239     if (!Compact) OS << setw(17);
240     OS << Nodes(i).X() << " ";
241     if (!Compact) OS << setw(17);
242     OS << Nodes(i).Y() << "\n";
243   }
244 }
245
246
247
248 //=======================================================================
249 //function : Dump
250 //purpose  :
251 //=======================================================================
252
253 void Poly::Dump(const Handle(Poly_Triangulation)& T, Standard_OStream& OS)
254 {
255   Poly::Write(T,OS,Standard_False);
256 }
257
258
259 //=======================================================================
260 //function : Dump
261 //purpose  :
262 //=======================================================================
263
264 void Poly::Dump(const Handle(Poly_Polygon3D)& P, Standard_OStream& OS)
265 {
266   Poly::Write(P,OS,Standard_False);
267 }
268
269
270 //=======================================================================
271 //function : Dump
272 //purpose  :
273 //=======================================================================
274
275 void Poly::Dump(const Handle(Poly_Polygon2D)& P, Standard_OStream& OS)
276 {
277   Poly::Write(P,OS,Standard_False);
278 }
279
280
281 //=======================================================================
282 //function : ReadTriangulation
283 //purpose  :
284 //=======================================================================
285
286 Handle(Poly_Triangulation) Poly::ReadTriangulation(Standard_IStream& IS)
287 {
288   // Read a triangulation
289
290   char line[100];
291   IS >> line;
292   if (strcmp(line,"Poly_Triangulation")) {
293 #ifdef OCCT_DEBUG
294     cout << "Not a Triangulation in the file" << endl;
295 #endif
296     return Handle(Poly_Triangulation)();
297   }
298
299   Standard_Integer nbNodes, nbTriangles;
300   Standard_Boolean hasUV;
301   IS >> nbNodes >> nbTriangles >> hasUV;
302
303   Standard_Real d;
304   IS >> d;
305
306   // read the 3d nodes
307   Standard_Real x,y,z;
308   Standard_Integer i;
309   TColgp_Array1OfPnt Nodes(1, nbNodes);
310   TColgp_Array1OfPnt2d UVNodes(1, nbNodes);
311
312   for (i = 1; i <= nbNodes; i++) {
313     IS >> x >> y >> z;
314     Nodes(i).SetCoord(x,y,z);
315   }
316
317   // read the UV points if necessary
318
319   if (hasUV) {
320     for (i = 1; i <= nbNodes; i++) {
321       IS >> x >> y;
322       UVNodes(i).SetCoord(x,y);
323     }
324   }
325
326
327   // read the triangles
328   Standard_Integer n1,n2,n3;
329   Poly_Array1OfTriangle Triangles(1, nbTriangles);
330   for (i = 1; i <= nbTriangles; i++) {
331     IS >> n1 >> n2 >> n3;
332     Triangles(i).Set(n1,n2,n3);
333   }
334
335
336   Handle(Poly_Triangulation) T;
337
338   if (hasUV) T =  new Poly_Triangulation(Nodes,UVNodes,Triangles);
339   else T = new Poly_Triangulation(Nodes,Triangles);
340
341   T->Deflection(d);
342
343   return T;
344 }
345
346
347 //=======================================================================
348 //function : ReadPolygon3D
349 //purpose  :
350 //=======================================================================
351
352 Handle(Poly_Polygon3D) Poly::ReadPolygon3D(Standard_IStream& IS)
353 {
354   // Read a 3d polygon
355
356   char line[100];
357   IS >> line;
358   if (strcmp(line,"Poly_Polygon3D")) {
359 #ifdef OCCT_DEBUG
360     cout << "Not a Polygon3D in the file" << endl;
361 #endif
362     return Handle(Poly_Polygon3D)();
363   }
364
365   Standard_Integer nbNodes;
366   IS >> nbNodes;
367
368   Standard_Boolean hasparameters;
369   IS >> hasparameters;
370
371   Standard_Real d;
372   IS >> d;
373
374   // read the nodes
375   Standard_Real x,y,z;
376   Standard_Integer i;
377   TColgp_Array1OfPnt Nodes(1, nbNodes);
378
379   for (i = 1; i <= nbNodes; i++) {
380     IS >> x >> y >> z;
381     Nodes(i).SetCoord(x,y,z);
382   }
383
384   TColStd_Array1OfReal Param(1,nbNodes);
385   if (hasparameters) {
386     for (i = 1; i <= nbNodes; i++) {
387       IS >> Param(i);
388     }
389   }
390
391   Handle(Poly_Polygon3D) P;
392   if (!hasparameters)
393     P = new Poly_Polygon3D(Nodes);
394   else
395     P = new Poly_Polygon3D(Nodes, Param);
396
397   P->Deflection(d);
398
399   return P;
400 }
401
402 //=======================================================================
403 //function : ReadPolygon3D
404 //purpose  :
405 //=======================================================================
406
407 Handle(Poly_Polygon2D) Poly::ReadPolygon2D(Standard_IStream& IS)
408 {
409   // Read a 2d polygon
410
411   char line[100];
412   IS >> line;
413   if (strcmp(line,"Poly_Polygon2D")) {
414 #ifdef OCCT_DEBUG
415     cout << "Not a Polygon2D in the file" << endl;
416 #endif
417     return Handle(Poly_Polygon2D)();
418   }
419
420   Standard_Integer nbNodes;
421   IS >> nbNodes;
422
423   Standard_Real d;
424   IS >> d;
425
426   // read the nodes
427   Standard_Real x,y;
428   Standard_Integer i;
429   TColgp_Array1OfPnt2d Nodes(1, nbNodes);
430
431   for (i = 1; i <= nbNodes; i++) {
432     IS >> x >> y;
433     Nodes(i).SetCoord(x,y);
434   }
435
436   Handle(Poly_Polygon2D) P =
437     new Poly_Polygon2D(Nodes);
438
439   P->Deflection(d);
440
441   return P;
442 }
443
444 //=======================================================================
445 //function : ComputeNormals
446 //purpose  :
447 //=======================================================================
448
449 void  Poly::ComputeNormals(const Handle(Poly_Triangulation)& Tri)
450 {
451   const TColgp_Array1OfPnt&     arrNodes = Tri->Nodes();
452   const Poly_Array1OfTriangle & arrTri   = Tri->Triangles();
453   Standard_Integer              nbNormVal  = Tri->NbNodes() * 3;
454   const Handle(TShort_HArray1OfShortReal) Normals =
455     new TShort_HArray1OfShortReal(1, nbNormVal);
456   Normals->Init(0.F);
457
458   Standard_ShortReal            * arrNormal = &(Normals->ChangeValue(1));
459
460   Standard_Real                 aCoord[3];
461   Standard_Integer              iNode[3] = {0, 0, 0};
462   Standard_Integer              iN, iTri;
463   const Standard_Real eps2 = gp::Resolution();
464
465   for (iTri = 1; iTri <= arrTri.Length(); iTri++) {
466     // Get the nodes of the current triangle
467     arrTri(iTri).Get (iNode[0], iNode[1], iNode[2]);
468       const gp_XYZ aVec[2] = {
469         arrNodes(iNode[1]).XYZ() - arrNodes(iNode[0]).XYZ(),
470         arrNodes(iNode[2]).XYZ() - arrNodes(iNode[0]).XYZ()
471       };
472
473     // Find the normal vector of the current triangle
474     gp_XYZ aNorm = aVec[0] ^ aVec[1];
475     const Standard_Real aMod = aNorm.SquareModulus();
476     if (aMod > eps2) {
477       aNorm /= sqrt(aMod);
478       aNorm.Coord (aCoord[0], aCoord[1], aCoord[2]);
479       iNode[0] = (iNode[0]-1)*3;
480       iNode[1] = (iNode[1]-1)*3;
481       iNode[2] = (iNode[2]-1)*3;
482       arrNormal[iNode[0]+0] += (Standard_ShortReal)aCoord[0];
483       arrNormal[iNode[0]+1] += (Standard_ShortReal)aCoord[1];
484       arrNormal[iNode[0]+2] += (Standard_ShortReal)aCoord[2];
485       arrNormal[iNode[1]+0] += (Standard_ShortReal)aCoord[0];
486       arrNormal[iNode[1]+1] += (Standard_ShortReal)aCoord[1];
487       arrNormal[iNode[1]+2] += (Standard_ShortReal)aCoord[2];
488       arrNormal[iNode[2]+0] += (Standard_ShortReal)aCoord[0];
489       arrNormal[iNode[2]+1] += (Standard_ShortReal)aCoord[1];
490       arrNormal[iNode[2]+2] += (Standard_ShortReal)aCoord[2];
491     }
492   }
493   // Normalize all vectors
494   for (iN = 0; iN < nbNormVal; iN+=3) {
495     Standard_Real aMod (arrNormal[iN+0]*arrNormal[iN+0] +
496                         arrNormal[iN+1]*arrNormal[iN+1] +
497                         arrNormal[iN+2]*arrNormal[iN+2]);
498     if (aMod < eps2) {
499       arrNormal[iN+0] = 0.f;
500       arrNormal[iN+1] = 0.f;
501       arrNormal[iN+2] = 1.f;
502     } else {
503       aMod = sqrt(aMod);
504       arrNormal[iN+0] = Standard_ShortReal(arrNormal[iN+0]/aMod);
505       arrNormal[iN+1] = Standard_ShortReal(arrNormal[iN+1]/aMod);
506       arrNormal[iN+2] = Standard_ShortReal(arrNormal[iN+2]/aMod);
507     }
508   }
509
510   Tri->SetNormals(Normals);
511 }
512
513 //=======================================================================
514 //function : PointOnTriangle
515 //purpose  : 
516 //=======================================================================
517
518 Standard_Real Poly::PointOnTriangle (const gp_XY& theP1, const gp_XY& theP2, const gp_XY& theP3, 
519                                      const gp_XY& theP, gp_XY& theUV)
520 {
521   gp_XY aDP = theP  - theP1;
522   gp_XY aDU = theP2 - theP1;
523   gp_XY aDV = theP3 - theP1;
524   Standard_Real aDet = aDU ^ aDV;
525
526   // case of non-degenerated triangle
527   if ( Abs (aDet) > gp::Resolution() )
528   {
529     Standard_Real aU =  (aDP ^ aDV) / aDet;
530     Standard_Real aV = -(aDP ^ aDU) / aDet;
531
532     // if point is inside triangle, just return parameters
533     if ( aU > -gp::Resolution() &&
534          aV > -gp::Resolution() &&
535          1. - aU - aV > -gp::Resolution() ) 
536     {
537       theUV.SetCoord (aU, aV);
538       return 0.;
539     }
540
541     // else find closest point on triangle sides; note that in general case  
542     // triangle can be very distorted and it is necessary to check 
543     // projection on all sides regardless of values of computed parameters
544
545     // project on side U=0
546     aU = 0.;
547     aV = Min (1., Max (0., (aDP * aDV) / aDV.SquareModulus()));
548     Standard_Real aD = (aV * aDV - aDP).SquareModulus();
549
550     // project on side V=0
551     Standard_Real u = Min (1., Max (0., (aDP * aDU) / aDU.SquareModulus()));
552     Standard_Real d = (u * aDU - aDP).SquareModulus();
553     if ( d < aD )
554     {
555       aU = u;
556       aV = 0.;
557       aD = d;
558     }
559
560     // project on side U+V=1
561     gp_XY aDUV = aDV - aDU;
562     Standard_Real v = Min (1., Max (0., ((aDP - aDU) * aDUV) / aDUV.SquareModulus()));
563     d = (theP2 + v * aDUV - theP).SquareModulus();
564     if ( d < aD )
565     {
566       aU = 1. - v;
567       aV = v;
568       aD = d;
569     }
570
571     theUV.SetCoord (aU, aV);
572     return aD;
573   }
574
575   // degenerated triangle
576   Standard_Real aL2U = aDU.SquareModulus();
577   Standard_Real aL2V = aDV.SquareModulus();
578   if ( aL2U < gp::Resolution() ) // side 1-2 is degenerated
579   {
580     if ( aL2V < gp::Resolution() ) // whole triangle is degenerated to point
581     {
582       theUV.SetCoord (0., 0.);
583       return (theP - theP1).SquareModulus();
584     }
585     else
586     {
587       theUV.SetCoord (0., (aDP * aDV) / aL2V);
588       return (theP - (theP1 + theUV.Y() * aDV)).SquareModulus();
589     }
590   }
591   else if ( aL2V < gp::Resolution() ) // side 1-3 is degenerated
592   {
593     theUV.SetCoord ((aDP * aDU) / aL2U, 0.);
594     return (theP - (theP1 + theUV.X() * aDU)).SquareModulus();
595   }
596   else // sides 1-2 and 1-3 are collinear
597   {
598     // select parameter on one of sides so as to have points closer to picked
599     Standard_Real aU = Min (1., Max (0., (aDP * aDU) / aL2U));
600     Standard_Real aV = Min (1., Max (0., (aDP * aDV) / aL2V));
601     Standard_Real aD1 = (aDP - aU * aDU).SquareModulus();
602     Standard_Real aD2 = (aDP - aV * aDV).SquareModulus();
603     if ( aD1 < aD2 )
604     {
605       theUV.SetCoord ((aDP * aDU) / aL2U, 0.);
606       return aD1;
607     }
608     else
609     {
610       theUV.SetCoord (0., (aDP * aDV) / aL2V);
611       return aD2;
612     }
613   }
614 }
615