4b114473 |
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 | } |