0030924: Coding - incorrect header in file OSD_ThreadPool.hxx
[occt.git] / src / BRepGProp / BRepGProp_MeshProps.cxx
1 // Copyright (c) 2018 OPEN CASCADE SAS
2 //
3 // This file is part of Open CASCADE Technology software library.
4 //
5 // This library is free software; you can redistribute it and/or modify it under
6 // the terms of the GNU Lesser General Public License version 2.1 as published
7 // by the Free Software Foundation, with special exception defined in the file
8 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
9 // distribution for complete text of the license and disclaimer of any warranty.
10 //
11 // Alternatively, this file may be used under the terms of Open CASCADE
12 // commercial license or contractual agreement.
13
14 #include <BRepGProp_MeshProps.hxx>
15
16 #include <BRepGProp.hxx>
17 #include <ElSLib.hxx>
18 #include <gp_Ax3.hxx>
19 #include <gp_Pnt.hxx>
20 #include <GProp.hxx>
21 #include <Poly_Triangulation.hxx>
22 #include <Poly_Triangle.hxx>
23 #include <TopLoc_Location.hxx>
24
25 //=======================================================================
26 //function : CalculateElSProps
27 //purpose  : Calculate one Gauss point for surface properties 
28 //           of triangle p1, p2, p3
29 //           relatively point Apex 
30 //=======================================================================
31 static void CalculateElSProps(const Standard_Real x, 
32   const Standard_Real y,
33   const Standard_Real z, const Standard_Real ds, Standard_Real* GProps)
34 {
35   //GProps[0] = Volume
36   // Static moments of inertia.
37   //GProps[1] = Ix, GProps[2] = Iy, GProps[3] = Iz
38   //Matrix of moments of inertia.
39   //GProps[4] = Ixx, GProps[5] = Iyy, GProps[6] = Izz,
40   //GProps[7] = Ixy, aGProps[8] = Ixz, GProps[9] = Iyz,
41   //
42   Standard_Real x2, y2, z2;
43   x2 = x * x;
44   y2 = y * y;
45   z2 = z * z;
46
47   GProps[0] += ds;       //Area       
48   GProps[1] += x * ds;  //Ix
49   GProps[2] += y * ds;  //Iy
50   GProps[3] += z * ds;  //Iz
51   //
52   GProps[7] += x * y * ds; //Ixy
53   GProps[8] += x * z * ds; //Ixz
54   GProps[9] += y * z * ds; //Iyz
55   GProps[4] += (y2 + z2) * ds; //Ixx
56   GProps[5] += (x2 + z2) * ds; //Iyy
57   GProps[6] += (x2 + y2) * ds; //Izz
58 }
59 //=======================================================================
60 //function : CalculateElVProps
61 //purpose  : Calculate one Gauss point for volume properties of pyramid, 
62 //           based on triangle p1, p2, p3 with apex Apex 
63 //=======================================================================
64 static void CalculateElVProps(const Standard_Real x, 
65   const Standard_Real y,
66   const Standard_Real z, const Standard_Real dv, Standard_Real* GProps)
67 {
68   Standard_Real x2, y2, z2;
69   x2 = x * x;
70   y2 = y * y;
71   z2 = z * z;
72   GProps[0] += dv / 3.0;       //Volume       
73   GProps[1] += 0.25 * x * dv;  //Ix
74   GProps[2] += 0.25 * y * dv;  //Iy
75   GProps[3] += 0.25 * z * dv;  //Iz
76   Standard_Real dv1 = 0.2 * dv;
77   GProps[7] += x * y * dv1; //Ixy
78   GProps[8] += x * z * dv1; //Ixz
79   GProps[9] += y * z * dv1; //Iyz
80   GProps[4] += (y2 + z2) * dv1; //Ixx
81   GProps[5] += (x2 + z2) * dv1; //Iyy
82   GProps[6] += (x2 + y2) * dv1; //Izz
83 }
84 //=======================================================================
85 //function : CalculateProps
86 //purpose  : Calculate global surface properties of triangle 
87 //           or volume properties of pyramid, based on triangle
88 //           p1, p2, p3 with apex Apex by Gauss integration over triangle area.
89 //=======================================================================
90  void BRepGProp_MeshProps::CalculateProps(const gp_Pnt& p1, const gp_Pnt& p2, 
91                                           const gp_Pnt& p3,
92                                           const gp_Pnt& Apex,
93                                           const Standard_Boolean isVolume,
94                                           Standard_Real GProps[10],
95                                           const Standard_Integer NbGaussPoints,
96                                           const Standard_Real* GaussPnts)
97 {
98   //GProps[0] = Volume
99   // Static moments of inertia.
100   //GProps[1] = Ix, GProps[2] = Iy, GProps[3] = Iz
101   //Matrix of moments of inertia.
102   //GProps[4] = Ixx, GProps[5] = Iyy, GProps[6] = Izz,
103   //GProps[7] = Ixy, aGProps[8] = Ixz, GProps[9] = Iyz,
104   //
105
106   //Define plane and coordinates of triangle nodes on plane 
107   gp_Vec aV12(p2, p1);
108   gp_Vec aV23(p3, p2);
109   gp_Vec aNorm = aV12 ^ aV23;
110   Standard_Real aDet = aNorm.Magnitude();
111   if (aDet <= gp::Resolution())
112   {
113     return;
114   }
115   gp_XYZ aCenter = (p1.XYZ() + p2.XYZ() + p3.XYZ()) / 3.;
116   gp_Pnt aPC(aCenter);
117   gp_Dir aDN(aNorm);
118   gp_Ax3 aPosPln(aPC, aDN);
119   //Coordinates of nodes on plane
120   Standard_Real x1, y1, x2, y2, x3, y3;
121   ElSLib::PlaneParameters(aPosPln, p1, x1, y1);
122   ElSLib::PlaneParameters(aPosPln, p2, x2, y2);
123   ElSLib::PlaneParameters(aPosPln, p3, x3, y3);
124   //
125   Standard_Real l1, l2; //barycentriche coordinates
126   Standard_Real x, y, z;
127   Standard_Real w; //weight
128   Standard_Integer i;
129   for (i = 0; i < NbGaussPoints; ++i)
130   {
131     Standard_Integer ind = 3 * i;
132     l1 = GaussPnts[ind];
133     l2 = GaussPnts[ind + 1];
134     w = GaussPnts[ind + 2];
135     w *= aDet;
136     x = l1*(x1 - x3) + l2*(x2 - x3) + x3;
137     y = l1*(y1 - y3) + l2*(y2 - y3) + y3;
138     gp_Pnt aP = ElSLib::PlaneValue(x, y, aPosPln);
139     x = aP.X() - Apex.X();
140     y = aP.Y() - Apex.Y();
141     z = aP.Z() - Apex.Z();
142     //
143     if (isVolume)
144     {
145       Standard_Real xn = aDN.X() * w;
146       Standard_Real yn = aDN.Y() * w;
147       Standard_Real zn = aDN.Z() * w;
148       Standard_Real dv = x * xn + y * yn + z * zn;
149       CalculateElVProps(x, y, z, dv, GProps);
150     }
151     else
152     {
153       Standard_Real ds = w;
154       CalculateElSProps(x, y, z, ds, GProps);
155     }
156
157   }
158 }
159
160 //=======================================================================
161 //function : Perform
162 //purpose  : 
163 //=======================================================================
164 void BRepGProp_MeshProps::Perform(const Handle(Poly_Triangulation)& theMesh,
165                                   const TopLoc_Location& theLoc,
166                                   const TopAbs_Orientation theOri)
167 {
168   if (theLoc.IsIdentity())
169   {
170     Perform(theMesh->Nodes(), theMesh->Triangles(), theOri);
171   }
172   else
173   {
174     const gp_Trsf& aTr = theLoc.Transformation();
175     //
176     Standard_Boolean isToCopy = aTr.ScaleFactor()*aTr.HVectorialPart().Determinant() < 0. ||
177                                 Abs(Abs(aTr.ScaleFactor()) - 1.) > gp::Resolution();
178     if (isToCopy)
179     {
180       TColgp_Array1OfPnt aNodes(1, theMesh->NbNodes());
181       const TColgp_Array1OfPnt& aMeshNodes = theMesh->Nodes();
182       Standard_Integer i;
183       for (i = 1; i <= aMeshNodes.Length(); ++i)
184       {
185         aNodes(i) = aMeshNodes.Value(i).Transformed(aTr);
186       }
187       Perform(aNodes, theMesh->Triangles(), theOri);
188       return;
189     }
190     //
191     gp_Trsf aTrInv = aTr.Inverted();
192     gp_Pnt loc_save = loc;
193     loc.Transform(aTrInv);
194     Perform(theMesh->Nodes(), theMesh->Triangles(), theOri);
195     //Computes the inertia tensor at mesh gravity center
196     gp_Mat HMat, inertia0;
197     gp_Pnt g0 = g;
198     g.SetXYZ(g.XYZ() + loc.XYZ());
199     if (g0.XYZ().Modulus() > gp::Resolution())
200     {
201       GProp::HOperator(g, loc, dim, HMat);
202       inertia0 = inertia - HMat;
203     }
204     else
205     {
206       inertia0 = inertia;
207     }
208     //Transform inertia tensor for rotation
209     gp_Mat HVec = aTrInv.HVectorialPart();
210     gp_Mat HVecT = HVec.Transposed();
211     HVecT.Multiply(inertia0);
212     inertia0 = HVecT.Multiplied(HVec);
213     //Put gravity center in true position of mesh  
214     g.Transform(aTr);
215     g0 = g;
216     g.SetXYZ(g.XYZ() - loc_save.XYZ());
217     loc = loc_save;
218     //
219     //Computes the inertia tensor for loc
220     GProp::HOperator(g0, loc, dim, HMat);
221     inertia = inertia0 + HMat;
222   }
223 }
224 //=======================================================================
225 //function : Perform
226 //purpose  : 
227 //=======================================================================
228 void BRepGProp_MeshProps::Perform(const TColgp_Array1OfPnt& theNodes,
229                                    const Poly_Array1OfTriangle& theTriangles,
230                                    const TopAbs_Orientation theOri)
231 {
232   //
233   // Gauss points for barycentriche coordinates
234   static const Standard_Real GPtsWg[] =
235   {  1. / 6., 1. / 6., 1. / 6., /*3 points*/
236      2. / 3., 1. / 6., 1. / 6.,
237      1. / 6., 2. / 3., 1. / 6. };
238   //
239   Standard_Integer aNbGaussPoints = 3;
240   
241   // Array to store global properties
242   Standard_Real aGProps[10] = { 0., 0., 0., 0., 0., 0., 0., 0., 0., 0 };
243   //aGProps[0] = Volume
244   // Static moments of inertia.
245   //aGProps[1] = Ix, aGProps[2] = Iy, aGProps[3] = Iz
246   //Matrix of moments of inertia.
247   //aGProps[4] = Ixx, aGProps[5] = Iyy, aGProps[6] = Izz,
248   //aGProps[7] = Ixy, aGProps[8] = Ixz, aGProps[9] = Iyz,
249
250   Standard_Boolean isVolume = myType == Vinert;
251   Standard_Integer i;
252   Standard_Integer n1, n2, n3; //node indeces
253   for (i = theTriangles.Lower(); i <= theTriangles.Upper(); ++i)
254   {
255     const Poly_Triangle& aTri = theTriangles(i);
256     aTri.Get(n1, n2, n3);
257     if (theOri == TopAbs_REVERSED)
258     {
259       Standard_Integer nn = n2;
260       n2 = n3;
261       n3 = nn;
262     }
263     // Calculate properties of a pyramid built on face and apex
264     const gp_Pnt& p1 = theNodes(n1);
265     const gp_Pnt& p2 = theNodes(n2);
266     const gp_Pnt& p3 = theNodes(n3);
267     CalculateProps(p1, p2, p3, loc, isVolume, aGProps, aNbGaussPoints, GPtsWg);
268   }
269
270   dim = aGProps[0];
271   if (Abs(dim) >= 1.e-20) //To be consistent with GProp_GProps
272   {
273     g.SetX(aGProps[1] / dim);
274     g.SetY(aGProps[2] / dim);
275     g.SetZ(aGProps[3] / dim);
276   }
277   else
278   {
279     g.SetX(aGProps[1]);
280     g.SetY(aGProps[2]);
281     g.SetZ(aGProps[3]);
282   }
283   inertia(1, 1) = aGProps[4];
284   inertia(1, 2) = -aGProps[7];
285   inertia(1, 3) = -aGProps[8];
286   inertia(2, 1) = -aGProps[7];
287   inertia(2, 2) = aGProps[5];
288   inertia(2, 3) = -aGProps[9];
289   inertia(3, 1) = -aGProps[8];
290   inertia(3, 2) = -aGProps[9];
291   inertia(3, 3) = aGProps[6];
292 }