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