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