9bd59d1c |
1 | // Copyright (c) 2008-2015 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 <math.hxx> |
15 | #include <Precision.hxx> |
16 | #include <TColStd_Array1OfReal.hxx> |
17 | #include <Standard_Assert.hxx> |
18 | #include <BRepGProp_Face.hxx> |
19 | #include <BRepGProp_Domain.hxx> |
20 | #include <BRepGProp_Gauss.hxx> |
21 | |
22 | // If the following is defined the error of algorithm is calculated by static moments |
23 | #define IS_MIN_DIM |
24 | |
25 | namespace |
26 | { |
27 | // Minimal value of interval's range for computation | minimal value of "dim" | ... |
28 | static const Standard_Real EPS_PARAM = 1.e-12; |
29 | static const Standard_Real EPS_DIM = 1.e-30; |
30 | static const Standard_Real ERROR_ALGEBR_RATIO = 2.0 / 3.0; |
31 | |
32 | // Maximum of GaussPoints on a subinterval and maximum of subintervals |
33 | static const Standard_Integer GPM = math::GaussPointsMax(); |
34 | static const Standard_Integer SUBS_POWER = 32; |
35 | static const Standard_Integer SM = SUBS_POWER * GPM + 1; |
36 | |
37 | // Auxiliary inner functions to perform arithmetic operations. |
38 | static Standard_Real Add(const Standard_Real theA, const Standard_Real theB) |
39 | { |
40 | return theA + theB; |
41 | } |
42 | |
43 | static Standard_Real AddInf(const Standard_Real theA, const Standard_Real theB) |
44 | { |
45 | if (Precision::IsPositiveInfinite(theA)) |
46 | { |
47 | if (Precision::IsNegativeInfinite(theB)) |
48 | return 0.0; |
49 | else |
50 | return Precision::Infinite(); |
51 | } |
52 | |
53 | if (Precision::IsPositiveInfinite(theB)) |
54 | { |
55 | if (Precision::IsNegativeInfinite(theA)) |
56 | return 0.0; |
57 | else |
58 | return Precision::Infinite(); |
59 | } |
60 | |
61 | if (Precision::IsNegativeInfinite(theA)) |
62 | { |
63 | if (Precision::IsPositiveInfinite(theB)) |
64 | return 0.0; |
65 | else |
66 | return -Precision::Infinite(); |
67 | } |
68 | |
69 | if (Precision::IsNegativeInfinite(theB)) |
70 | { |
71 | if (Precision::IsPositiveInfinite(theA)) |
72 | return 0.0; |
73 | else |
74 | return -Precision::Infinite(); |
75 | } |
76 | |
77 | return theA + theB; |
78 | } |
79 | |
80 | static Standard_Real Mult(const Standard_Real theA, const Standard_Real theB) |
81 | { |
82 | return theA * theB; |
83 | } |
84 | |
85 | static Standard_Real MultInf(const Standard_Real theA, const Standard_Real theB) |
86 | { |
87 | if ((theA == 0.0) || (theB == 0.0)) //strictly zerro (without any tolerances) |
88 | return 0.0; |
89 | |
90 | if (Precision::IsPositiveInfinite(theA)) |
91 | { |
92 | if (theB < 0.0) |
93 | return -Precision::Infinite(); |
94 | else |
95 | return Precision::Infinite(); |
96 | } |
97 | |
98 | if (Precision::IsPositiveInfinite(theB)) |
99 | { |
100 | if (theA < 0.0) |
101 | return -Precision::Infinite(); |
102 | else |
103 | return Precision::Infinite(); |
104 | } |
105 | |
106 | if (Precision::IsNegativeInfinite(theA)) |
107 | { |
108 | if (theB < 0.0) |
109 | return +Precision::Infinite(); |
110 | else |
111 | return -Precision::Infinite(); |
112 | } |
113 | |
114 | if (Precision::IsNegativeInfinite(theB)) |
115 | { |
116 | if (theA < 0.0) |
117 | return +Precision::Infinite(); |
118 | else |
119 | return -Precision::Infinite(); |
120 | } |
121 | |
122 | return theA * theB; |
123 | } |
124 | } |
125 | |
126 | //======================================================================= |
127 | //function : BRepGProp_Gauss::Inert::Inert |
128 | //purpose : Constructor |
129 | //======================================================================= |
130 | BRepGProp_Gauss::Inertia::Inertia() |
131 | : Mass(0.0), |
132 | Ix (0.0), |
133 | Iy (0.0), |
134 | Iz (0.0), |
135 | Ixx (0.0), |
136 | Iyy (0.0), |
137 | Izz (0.0), |
138 | Ixy (0.0), |
139 | Ixz (0.0), |
140 | Iyz (0.0) |
141 | { |
142 | } |
143 | |
144 | //======================================================================= |
145 | //function : Inertia::Reset |
146 | //purpose : Zeroes all values. |
147 | //======================================================================= |
148 | void BRepGProp_Gauss::Inertia::Reset() |
149 | { |
150 | memset(reinterpret_cast<void*>(this), 0, sizeof(BRepGProp_Gauss::Inertia)); |
151 | } |
152 | |
153 | //======================================================================= |
154 | //function : BRepGProp_Gauss |
155 | //purpose : Constructor |
156 | //======================================================================= |
157 | BRepGProp_Gauss::BRepGProp_Gauss(const BRepGProp_GaussType theType) |
158 | : myType(theType) |
159 | { |
160 | add = (::Add ); |
161 | mult = (::Mult); |
162 | } |
163 | |
164 | //======================================================================= |
165 | //function : MaxSubs |
166 | //purpose : |
167 | //======================================================================= |
168 | Standard_Integer BRepGProp_Gauss::MaxSubs(const Standard_Integer theN, |
169 | const Standard_Integer theCoeff) |
170 | { |
171 | return IntegerLast() / theCoeff < theN ? |
172 | IntegerLast() : theN * theCoeff + 1; |
173 | } |
174 | |
175 | //======================================================================= |
176 | //function : Init |
177 | //purpose : |
178 | //======================================================================= |
c04c30b3 |
179 | void BRepGProp_Gauss::Init(NCollection_Handle<math_Vector>& theOutVec, |
9bd59d1c |
180 | const Standard_Real theValue, |
181 | const Standard_Integer theFirst, |
182 | const Standard_Integer theLast) |
183 | { |
184 | if(theLast - theFirst == 0) |
185 | { |
186 | theOutVec->Init(theValue); |
187 | } |
188 | else |
189 | { |
190 | for (Standard_Integer i = theFirst; i <= theLast; ++i) |
191 | theOutVec->Value(i) = theValue; |
192 | } |
193 | } |
194 | |
195 | //======================================================================= |
196 | //function : InitMass |
197 | //purpose : |
198 | //======================================================================= |
199 | void BRepGProp_Gauss::InitMass(const Standard_Real theValue, |
200 | const Standard_Integer theFirst, |
201 | const Standard_Integer theLast, |
202 | InertiaArray& theArray) |
203 | { |
204 | if (theArray.IsNull()) |
205 | return; |
206 | |
207 | Standard_Integer aFirst = theFirst; |
208 | Standard_Integer aLast = theLast; |
209 | |
210 | if (theLast - theFirst == 0) |
211 | { |
212 | aFirst = theArray->Lower(); |
213 | aLast = theArray->Upper(); |
214 | } |
215 | |
216 | for (Standard_Integer i = aFirst; i <= aLast; ++i) |
217 | theArray->ChangeValue(i).Mass = theValue; |
218 | } |
219 | |
220 | //======================================================================= |
221 | //function : FillIntervalBounds |
222 | //purpose : |
223 | //======================================================================= |
224 | Standard_Integer BRepGProp_Gauss::FillIntervalBounds( |
225 | const Standard_Real theA, |
226 | const Standard_Real theB, |
227 | const TColStd_Array1OfReal& theKnots, |
228 | const Standard_Integer theNumSubs, |
229 | InertiaArray& theInerts, |
c04c30b3 |
230 | NCollection_Handle<math_Vector>& theParam1, |
231 | NCollection_Handle<math_Vector>& theParam2, |
232 | NCollection_Handle<math_Vector>& theError, |
233 | NCollection_Handle<math_Vector>& theCommonError) |
9bd59d1c |
234 | { |
235 | const Standard_Integer aSize = |
236 | Max(theKnots.Upper(), MaxSubs(theKnots.Upper() - 1, theNumSubs)); |
237 | |
238 | if (aSize - 1 > theParam1->Upper()) |
239 | { |
240 | theInerts = new NCollection_Array1<Inertia>(1, aSize); |
241 | theParam1 = new math_Vector(1, aSize); |
242 | theParam2 = new math_Vector(1, aSize); |
243 | theError = new math_Vector(1, aSize, 0.0); |
244 | |
245 | if (theCommonError.IsNull() == Standard_False) |
246 | theCommonError = new math_Vector(1, aSize, 0.0); |
247 | } |
248 | |
249 | Standard_Integer j = 1, k = 1; |
250 | theParam1->Value(j++) = theA; |
251 | |
252 | const Standard_Integer aLength = theKnots.Upper(); |
253 | for (Standard_Integer i = 1; i <= aLength; ++i) |
254 | { |
255 | const Standard_Real kn = theKnots(i); |
256 | if (theA < kn) |
257 | { |
258 | if (kn < theB) |
259 | { |
260 | theParam1->Value(j++) = kn; |
261 | theParam2->Value(k++) = kn; |
262 | } |
263 | else |
264 | break; |
265 | } |
266 | } |
267 | |
268 | theParam2->Value(k) = theB; |
269 | return k; |
270 | } |
271 | |
272 | |
273 | |
274 | //======================================================================= |
275 | //function : computeVInertiaOfElementaryPart |
276 | //purpose : |
277 | //======================================================================= |
278 | void BRepGProp_Gauss::computeVInertiaOfElementaryPart( |
279 | const gp_Pnt& thePoint, |
280 | const gp_Vec& theNormal, |
281 | const gp_Pnt& theLocation, |
282 | const Standard_Real theWeight, |
283 | const Standard_Real theCoeff[], |
284 | const Standard_Boolean theIsByPoint, |
285 | BRepGProp_Gauss::Inertia& theOutInertia) |
286 | { |
287 | Standard_Real x = thePoint.X() - theLocation.X(); |
288 | Standard_Real y = thePoint.Y() - theLocation.Y(); |
289 | Standard_Real z = thePoint.Z() - theLocation.Z(); |
290 | |
291 | const Standard_Real xn = theNormal.X() * theWeight; |
292 | const Standard_Real yn = theNormal.Y() * theWeight; |
293 | const Standard_Real zn = theNormal.Z() * theWeight; |
294 | |
295 | if (theIsByPoint) |
296 | { |
297 | ///////////////////// /////////////////////// |
298 | // OFV code // // Initial code // |
299 | ///////////////////// /////////////////////// |
300 | // modified by APO |
301 | |
302 | Standard_Real dv = x * xn + y * yn + z * zn; //xyz = x * y * z; |
303 | theOutInertia.Mass += dv / 3.0; //Ixyi += zn * xyz; |
304 | theOutInertia.Ix += 0.25 * x * dv; //Iyzi += xn * xyz; |
305 | theOutInertia.Iy += 0.25 * y * dv; //Ixzi += yn * xyz; |
306 | theOutInertia.Iz += 0.25 * z * dv; //xi = x * x * x * xn / 3.0; |
307 | x -= theCoeff[0]; //yi = y * y * y * yn / 3.0; |
308 | y -= theCoeff[1]; //zi = z * z * z * zn / 3.0; |
309 | z -= theCoeff[2]; //Ixxi += (yi + zi); |
310 | dv *= 0.2; //Iyyi += (xi + zi); |
311 | theOutInertia.Ixy -= x * y * dv; //Izzi += (xi + yi); |
312 | theOutInertia.Iyz -= y * z * dv; //x -= Coeff[0]; |
313 | theOutInertia.Ixz -= x * z * dv; //y -= Coeff[1]; |
314 | x *= x; //z -= Coeff[2]; |
315 | y *= y; //dv = x * xn + y * yn + z * zn; |
316 | z *= z; //dvi += dv; |
317 | theOutInertia.Ixx += (y + z) * dv; //Ixi += x * dv; |
318 | theOutInertia.Iyy += (x + z) * dv; //Iyi += y * dv; |
319 | theOutInertia.Izz += (x + y) * dv; //Izi += z * dv; |
320 | } |
321 | else |
322 | { // By plane |
323 | const Standard_Real s = xn * theCoeff[0] + yn * theCoeff[1] + zn * theCoeff[2]; |
324 | |
325 | Standard_Real d1 = theCoeff[0] * x + theCoeff[1] * y + theCoeff[2] * z - theCoeff[3]; |
326 | Standard_Real d2 = d1 * d1; |
327 | Standard_Real d3 = d1 * d2 / 3.0; |
328 | Standard_Real dv = s * d1; |
329 | |
330 | theOutInertia.Mass += dv; |
331 | theOutInertia.Ix += (x - (theCoeff[0] * d1 * 0.5)) * dv; |
332 | theOutInertia.Iy += (y - (theCoeff[1] * d1 * 0.5)) * dv; |
333 | theOutInertia.Iz += (z - (theCoeff[2] * d1 * 0.5)) * dv; |
334 | |
335 | const Standard_Real px = x - theCoeff[0] * d1; |
336 | const Standard_Real py = y - theCoeff[1] * d1; |
337 | const Standard_Real pz = z - theCoeff[2] * d1; |
338 | |
339 | x = px * px * d1 + px * theCoeff[0] * d2 + theCoeff[0] * theCoeff[0] * d3; |
340 | y = py * py * d1 + py * theCoeff[1] * d2 + theCoeff[1] * theCoeff[1] * d3; |
341 | z = pz * pz * d1 + pz * theCoeff[2] * d2 + theCoeff[2] * theCoeff[2] * d3; |
342 | |
343 | theOutInertia.Ixx += (y + z) * s; |
344 | theOutInertia.Iyy += (x + z) * s; |
345 | theOutInertia.Izz += (x + y) * s; |
346 | |
347 | d2 *= 0.5; |
348 | x = (py * pz * d1) + (py * theCoeff[2] * d2) + (pz * theCoeff[1] * d2) + (theCoeff[1] * theCoeff[2] * d3); |
349 | y = (px * pz * d1) + (pz * theCoeff[0] * d2) + (px * theCoeff[2] * d2) + (theCoeff[0] * theCoeff[2] * d3); |
350 | z = (px * py * d1) + (px * theCoeff[1] * d2) + (py * theCoeff[0] * d2) + (theCoeff[0] * theCoeff[1] * d3); |
351 | |
352 | theOutInertia.Ixy -= z * s; |
353 | theOutInertia.Iyz -= x * s; |
354 | theOutInertia.Ixz -= y * s; |
355 | } |
356 | } |
357 | |
358 | //======================================================================= |
359 | //function : computeSInertiaOfElementaryPart |
360 | //purpose : |
361 | //======================================================================= |
362 | void BRepGProp_Gauss::computeSInertiaOfElementaryPart( |
363 | const gp_Pnt& thePoint, |
364 | const gp_Vec& theNormal, |
365 | const gp_Pnt& theLocation, |
366 | const Standard_Real theWeight, |
367 | BRepGProp_Gauss::Inertia& theOutInertia) |
368 | { |
369 | // ds - Jacobien (x, y, z) -> (u, v) = ||n|| |
370 | const Standard_Real ds = mult(theNormal.Magnitude(), theWeight); |
371 | const Standard_Real x = add(thePoint.X(), -theLocation.X()); |
372 | const Standard_Real y = add(thePoint.Y(), -theLocation.Y()); |
373 | const Standard_Real z = add(thePoint.Z(), -theLocation.Z()); |
374 | |
375 | theOutInertia.Mass = add(theOutInertia.Mass, ds); |
376 | |
377 | const Standard_Real XdS = mult(x, ds); |
378 | const Standard_Real YdS = mult(y, ds); |
379 | const Standard_Real ZdS = mult(z, ds); |
380 | |
381 | theOutInertia.Ix = add(theOutInertia.Ix, XdS); |
382 | theOutInertia.Iy = add(theOutInertia.Iy, YdS); |
383 | theOutInertia.Iz = add(theOutInertia.Iz, ZdS); |
384 | theOutInertia.Ixy = add(theOutInertia.Ixy, mult(x, YdS)); |
385 | theOutInertia.Iyz = add(theOutInertia.Iyz, mult(y, ZdS)); |
386 | theOutInertia.Ixz = add(theOutInertia.Ixz, mult(x, ZdS)); |
387 | |
388 | const Standard_Real XXdS = mult(x, XdS); |
389 | const Standard_Real YYdS = mult(y, YdS); |
390 | const Standard_Real ZZdS = mult(z, ZdS); |
391 | |
392 | theOutInertia.Ixx = add(theOutInertia.Ixx, add(YYdS, ZZdS)); |
393 | theOutInertia.Iyy = add(theOutInertia.Iyy, add(XXdS, ZZdS)); |
394 | theOutInertia.Izz = add(theOutInertia.Izz, add(XXdS, YYdS)); |
395 | } |
396 | |
397 | //======================================================================= |
398 | //function : checkBounds |
399 | //purpose : |
400 | //======================================================================= |
401 | void BRepGProp_Gauss::checkBounds(const Standard_Real theU1, |
402 | const Standard_Real theU2, |
403 | const Standard_Real theV1, |
404 | const Standard_Real theV2) |
405 | { |
406 | if (Precision::IsInfinite(theU1) || Precision::IsInfinite(theU2) || |
407 | Precision::IsInfinite(theV1) || Precision::IsInfinite(theV2)) |
408 | { |
409 | add = (::AddInf); |
410 | mult = (::MultInf); |
411 | } |
412 | } |
413 | |
414 | //======================================================================= |
415 | //function : addAndRestoreInertia |
416 | //purpose : |
417 | //======================================================================= |
418 | void BRepGProp_Gauss::addAndRestoreInertia( |
419 | const BRepGProp_Gauss::Inertia& theInInertia, |
420 | BRepGProp_Gauss::Inertia& theOutInertia) |
421 | { |
422 | theOutInertia.Mass = add(theOutInertia.Mass, theInInertia.Mass); |
423 | theOutInertia.Ix = add(theOutInertia.Ix, theInInertia.Ix); |
424 | theOutInertia.Iy = add(theOutInertia.Iy, theInInertia.Iy); |
425 | theOutInertia.Iz = add(theOutInertia.Iz, theInInertia.Iz); |
426 | theOutInertia.Ixx = add(theOutInertia.Ixx, theInInertia.Ixx); |
427 | theOutInertia.Iyy = add(theOutInertia.Iyy, theInInertia.Iyy); |
428 | theOutInertia.Izz = add(theOutInertia.Izz, theInInertia.Izz); |
429 | theOutInertia.Ixy = add(theOutInertia.Ixy, theInInertia.Ixy); |
430 | theOutInertia.Ixz = add(theOutInertia.Ixz, theInInertia.Ixz); |
431 | theOutInertia.Iyz = add(theOutInertia.Iyz, theInInertia.Iyz); |
432 | } |
433 | |
434 | //======================================================================= |
435 | //function : multAndRestoreInertia |
436 | //purpose : |
437 | //======================================================================= |
438 | void BRepGProp_Gauss::multAndRestoreInertia( |
439 | const Standard_Real theValue, |
440 | BRepGProp_Gauss::Inertia& theInOutInertia) |
441 | { |
442 | theInOutInertia.Mass = mult(theInOutInertia.Mass, theValue); |
443 | theInOutInertia.Ix = mult(theInOutInertia.Ix, theValue); |
444 | theInOutInertia.Iy = mult(theInOutInertia.Iy, theValue); |
445 | theInOutInertia.Iz = mult(theInOutInertia.Iz, theValue); |
446 | theInOutInertia.Ixx = mult(theInOutInertia.Ixx, theValue); |
447 | theInOutInertia.Iyy = mult(theInOutInertia.Iyy, theValue); |
448 | theInOutInertia.Izz = mult(theInOutInertia.Izz, theValue); |
449 | theInOutInertia.Ixy = mult(theInOutInertia.Ixy, theValue); |
450 | theInOutInertia.Ixz = mult(theInOutInertia.Ixz, theValue); |
451 | theInOutInertia.Iyz = mult(theInOutInertia.Iyz, theValue); |
452 | } |
453 | |
454 | //======================================================================= |
455 | //function : convert |
456 | //purpose : |
457 | //======================================================================= |
458 | void BRepGProp_Gauss::convert(const BRepGProp_Gauss::Inertia& theInertia, |
459 | gp_Pnt& theOutGravityCenter, |
460 | gp_Mat& theOutMatrixOfInertia, |
461 | Standard_Real& theOutMass) |
462 | { |
463 | if (Abs(theInertia.Mass) >= EPS_DIM) |
464 | { |
465 | const Standard_Real anInvMass = 1.0 / theInertia.Mass; |
466 | theOutGravityCenter.SetX(theInertia.Ix * anInvMass); |
467 | theOutGravityCenter.SetY(theInertia.Iy * anInvMass); |
468 | theOutGravityCenter.SetZ(theInertia.Iz * anInvMass); |
469 | |
470 | theOutMass = theInertia.Mass; |
471 | } |
472 | else |
473 | { |
474 | theOutMass = 0.0; |
475 | theOutGravityCenter.SetCoord(0.0, 0.0, 0.0); |
476 | } |
477 | |
478 | theOutMatrixOfInertia = gp_Mat( |
479 | gp_XYZ ( theInertia.Ixx, -theInertia.Ixy, -theInertia.Ixz), |
480 | gp_XYZ (-theInertia.Ixy, theInertia.Iyy, -theInertia.Iyz), |
481 | gp_XYZ (-theInertia.Ixz, -theInertia.Iyz, theInertia.Izz)); |
482 | } |
483 | |
484 | //======================================================================= |
485 | //function : convert |
486 | //purpose : |
487 | //======================================================================= |
488 | void BRepGProp_Gauss::convert(const BRepGProp_Gauss::Inertia& theInertia, |
489 | const Standard_Real theCoeff[], |
490 | const Standard_Boolean theIsByPoint, |
491 | gp_Pnt& theOutGravityCenter, |
492 | gp_Mat& theOutMatrixOfInertia, |
493 | Standard_Real& theOutMass) |
494 | { |
495 | convert(theInertia, theOutGravityCenter, theOutMatrixOfInertia, theOutMass); |
496 | if (Abs(theInertia.Mass) >= EPS_DIM && theIsByPoint) |
497 | { |
498 | const Standard_Real anInvMass = 1.0 / theInertia.Mass; |
499 | if (theIsByPoint == Standard_True) |
500 | { |
501 | theOutGravityCenter.SetX(theCoeff[0] + theInertia.Ix * anInvMass); |
502 | theOutGravityCenter.SetY(theCoeff[1] + theInertia.Iy * anInvMass); |
503 | theOutGravityCenter.SetZ(theCoeff[2] + theInertia.Iz * anInvMass); |
504 | } |
505 | else |
506 | { |
507 | theOutGravityCenter.SetX(theInertia.Ix * anInvMass); |
508 | theOutGravityCenter.SetY(theInertia.Iy * anInvMass); |
509 | theOutGravityCenter.SetZ(theInertia.Iz * anInvMass); |
510 | } |
511 | |
512 | theOutMass = theInertia.Mass; |
513 | } |
514 | else |
515 | { |
516 | theOutMass = 0.0; |
517 | theOutGravityCenter.SetCoord(0.0, 0.0, 0.0); |
518 | } |
519 | |
520 | theOutMatrixOfInertia = gp_Mat( |
521 | gp_XYZ (theInertia.Ixx, theInertia.Ixy, theInertia.Ixz), |
522 | gp_XYZ (theInertia.Ixy, theInertia.Iyy, theInertia.Iyz), |
523 | gp_XYZ (theInertia.Ixz, theInertia.Iyz, theInertia.Izz)); |
524 | } |
525 | |
526 | //======================================================================= |
527 | //function : Compute |
528 | //purpose : |
529 | //======================================================================= |
530 | Standard_Real BRepGProp_Gauss::Compute( |
531 | BRepGProp_Face& theSurface, |
532 | BRepGProp_Domain& theDomain, |
533 | const gp_Pnt& theLocation, |
534 | const Standard_Real theEps, |
535 | const Standard_Real theCoeff[], |
536 | const Standard_Boolean theIsByPoint, |
537 | Standard_Real& theOutMass, |
538 | gp_Pnt& theOutGravityCenter, |
539 | gp_Mat& theOutInertia) |
540 | { |
541 | const Standard_Boolean isErrorCalculation = |
542 | ( 0.0 > theEps || theEps < 0.001 ) ? Standard_True : Standard_False; |
543 | const Standard_Boolean isVerifyComputation = |
544 | ( 0.0 < theEps && theEps < 0.001 ) ? Standard_True : Standard_False; |
545 | |
546 | Standard_Real anEpsilon= Abs(theEps); |
547 | |
548 | BRepGProp_Gauss::Inertia anInertia; |
549 | InertiaArray anInertiaL = new NCollection_Array1<Inertia>(1, SM); |
550 | InertiaArray anInertiaU = new NCollection_Array1<Inertia>(1, SM); |
551 | |
552 | // Prepare Gauss points and weights |
c04c30b3 |
553 | NCollection_Handle<math_Vector> LGaussP[2]; |
554 | NCollection_Handle<math_Vector> LGaussW[2]; |
555 | NCollection_Handle<math_Vector> UGaussP[2]; |
556 | NCollection_Handle<math_Vector> UGaussW[2]; |
9bd59d1c |
557 | |
558 | const Standard_Integer aNbGaussPoint = |
559 | RealToInt(Ceiling(ERROR_ALGEBR_RATIO * GPM)); |
560 | |
561 | LGaussP[0] = new math_Vector(1, GPM); |
562 | LGaussP[1] = new math_Vector(1, aNbGaussPoint); |
563 | LGaussW[0] = new math_Vector(1, GPM); |
564 | LGaussW[1] = new math_Vector(1, aNbGaussPoint); |
565 | |
566 | UGaussP[0] = new math_Vector(1, GPM); |
567 | UGaussP[1] = new math_Vector(1, aNbGaussPoint); |
568 | UGaussW[0] = new math_Vector(1, GPM); |
569 | UGaussW[1] = new math_Vector(1, aNbGaussPoint); |
570 | |
c04c30b3 |
571 | NCollection_Handle<math_Vector> L1 = new math_Vector(1, SM); |
572 | NCollection_Handle<math_Vector> L2 = new math_Vector(1, SM); |
573 | NCollection_Handle<math_Vector> U1 = new math_Vector(1, SM); |
574 | NCollection_Handle<math_Vector> U2 = new math_Vector(1, SM); |
9bd59d1c |
575 | |
c04c30b3 |
576 | NCollection_Handle<math_Vector> ErrL = new math_Vector(1, SM, 0.0); |
577 | NCollection_Handle<math_Vector> ErrU = new math_Vector(1, SM, 0.0); |
578 | NCollection_Handle<math_Vector> ErrUL = new math_Vector(1, SM, 0.0); |
9bd59d1c |
579 | |
580 | // Face parametrization in U and V direction |
581 | Standard_Real BV1, BV2, BU1, BU2; |
582 | theSurface.Bounds(BU1, BU2, BV1, BV2); |
583 | checkBounds(BU1, BU2, BV1, BV2); |
584 | |
585 | // |
586 | const Standard_Integer NumSubs = SUBS_POWER; |
5b0f2540 |
587 | const TopoDS_Face& aF = theSurface.GetFace(); |
b2d1851c |
588 | const Standard_Boolean isNaturalRestriction = (aF.NbChildren () == 0); //theSurface.NaturalRestriction(); |
9bd59d1c |
589 | |
590 | Standard_Real CIx, CIy, CIz, CIxy, CIxz, CIyz; |
591 | Standard_Real CDim[2], CIxx[2], CIyy[2], CIzz[2]; |
592 | |
593 | // Boundary curve parametrization |
594 | Standard_Real u1 = BU1, u2, l1, l2, lm, lr, l, v; |
595 | |
596 | // On the boundary curve u-v |
597 | gp_Pnt2d Puv; |
598 | gp_Vec2d Vuv; |
599 | Standard_Real Dul; // Dul = Du / Dl |
600 | |
601 | Standard_Integer iLS, iLSubEnd, iGL, iGLEnd, NbLGaussP[2], LRange[2], iL, kL, kLEnd, IL, JL; |
602 | Standard_Integer i, iUSubEnd, NbUGaussP[2], URange[2], kU, kUEnd, IU, JU; |
603 | Standard_Integer UMaxSubs, LMaxSubs; |
604 | |
605 | Standard_Real ErrorU, ErrorL, ErrorLMax = 0.0, Eps = 0.0, EpsL = 0.0, EpsU = 0.0; |
606 | iGLEnd = isErrorCalculation ? 2 : 1; |
607 | |
608 | NbUGaussP[0] = theSurface.SIntOrder(anEpsilon); |
609 | NbUGaussP[1] = RealToInt( Ceiling(ERROR_ALGEBR_RATIO * NbUGaussP[0]) ); |
610 | |
611 | math::GaussPoints (NbUGaussP[0], *UGaussP[0]); |
612 | math::GaussWeights(NbUGaussP[0], *UGaussW[0]); |
613 | math::GaussPoints (NbUGaussP[1], *UGaussP[1]); |
614 | math::GaussWeights(NbUGaussP[1], *UGaussW[1]); |
615 | |
616 | const Standard_Integer aNbUSubs = theSurface.SUIntSubs(); |
617 | TColStd_Array1OfReal UKnots(1, aNbUSubs + 1); |
618 | theSurface.UKnots(UKnots); |
619 | |
620 | while (isNaturalRestriction || theDomain.More()) |
621 | { |
622 | if (isNaturalRestriction) |
c48e2889 |
623 | { |
9bd59d1c |
624 | NbLGaussP[0] = Min(2 * NbUGaussP[0], math::GaussPointsMax()); |
625 | } |
626 | else |
627 | { |
8ff2e494 |
628 | if (!theSurface.Load(theDomain.Value())) |
629 | { |
630 | return Precision::Infinite(); |
631 | } |
9bd59d1c |
632 | NbLGaussP[0] = theSurface.LIntOrder(anEpsilon); |
633 | } |
634 | |
635 | NbLGaussP[1] = RealToInt( Ceiling(ERROR_ALGEBR_RATIO * NbLGaussP[0]) ); |
636 | |
637 | math::GaussPoints (NbLGaussP[0], *LGaussP[0]); |
638 | math::GaussWeights(NbLGaussP[0], *LGaussW[0]); |
639 | math::GaussPoints (NbLGaussP[1], *LGaussP[1]); |
640 | math::GaussWeights(NbLGaussP[1], *LGaussW[1]); |
641 | |
642 | const Standard_Integer aNbLSubs = |
643 | isNaturalRestriction ? theSurface.SVIntSubs(): theSurface.LIntSubs(); |
644 | TColStd_Array1OfReal LKnots(1, aNbLSubs + 1); |
645 | |
646 | if (isNaturalRestriction) |
647 | { |
648 | theSurface.VKnots(LKnots); |
649 | l1 = BV1; |
650 | l2 = BV2; |
651 | } |
652 | else |
653 | { |
654 | theSurface.LKnots(LKnots); |
655 | l1 = theSurface.FirstParameter(); |
656 | l2 = theSurface.LastParameter(); |
657 | } |
658 | ErrorL = 0.0; |
659 | kLEnd = 1; JL = 0; |
660 | |
661 | if (Abs(l2 - l1) > EPS_PARAM) |
662 | { |
663 | iLSubEnd = FillIntervalBounds(l1, l2, LKnots, NumSubs, anInertiaL, L1, L2, ErrL, ErrUL); |
664 | LMaxSubs = BRepGProp_Gauss::MaxSubs(iLSubEnd); |
665 | |
666 | if (LMaxSubs > SM) |
c48e2889 |
667 | { |
9bd59d1c |
668 | LMaxSubs = SM; |
c48e2889 |
669 | } |
9bd59d1c |
670 | |
671 | BRepGProp_Gauss::InitMass(0.0, 1, LMaxSubs, anInertiaL); |
672 | BRepGProp_Gauss::Init(ErrL, 0.0, 1, LMaxSubs); |
673 | BRepGProp_Gauss::Init(ErrUL, 0.0, 1, LMaxSubs); |
674 | |
675 | do // while: L |
676 | { |
677 | if (++JL > iLSubEnd) |
678 | { |
679 | LRange[0] = IL = ErrL->Max(); |
680 | LRange[1] = JL; |
681 | L1->Value(JL) = (L1->Value(IL) + L2->Value(IL)) * 0.5; |
682 | L2->Value(JL) = L2->Value(IL); |
683 | L2->Value(IL) = L1->Value(JL); |
684 | } |
685 | else |
c48e2889 |
686 | { |
9bd59d1c |
687 | LRange[0] = IL = JL; |
c48e2889 |
688 | } |
9bd59d1c |
689 | |
690 | if (JL == LMaxSubs || Abs(L2->Value(JL) - L1->Value(JL)) < EPS_PARAM) |
691 | { |
692 | if (kLEnd == 1) |
693 | { |
694 | anInertiaL->ChangeValue(JL).Reset(); |
695 | ErrL->Value(JL) = 0.0; |
696 | } |
697 | else |
698 | { |
699 | --JL; |
700 | EpsL = ErrorL; |
701 | Eps = EpsL / 0.9; |
702 | break; |
703 | } |
704 | } |
705 | else |
c48e2889 |
706 | { |
9bd59d1c |
707 | for (kL = 0; kL < kLEnd; kL++) |
708 | { |
709 | iLS = LRange[kL]; |
710 | lm = 0.5 * (L2->Value(iLS) + L1->Value(iLS)); |
711 | lr = 0.5 * (L2->Value(iLS) - L1->Value(iLS)); |
712 | |
713 | CIx = CIy = CIz = CIxy = CIxz = CIyz = 0.0; |
714 | |
715 | for (iGL = 0; iGL < iGLEnd; ++iGL) |
716 | { |
717 | CDim[iGL] = CIxx[iGL] = CIyy[iGL] = CIzz[iGL] = 0.0; |
718 | |
719 | for (iL = 1; iL <= NbLGaussP[iGL]; iL++) |
720 | { |
721 | l = lm + lr * LGaussP[iGL]->Value(iL); |
722 | if (isNaturalRestriction) |
c48e2889 |
723 | { |
9bd59d1c |
724 | v = l; |
725 | u2 = BU2; |
726 | Dul = LGaussW[iGL]->Value(iL); |
727 | } |
728 | else |
729 | { |
730 | theSurface.D12d (l, Puv, Vuv); |
731 | Dul = Vuv.Y() * LGaussW[iGL]->Value(iL); // Dul = Du / Dl |
732 | |
733 | if (Abs(Dul) < EPS_PARAM) |
734 | continue; |
735 | |
736 | v = Puv.Y(); |
737 | u2 = Puv.X(); |
738 | |
739 | // Check on cause out off bounds of value current parameter |
740 | if (v < BV1) |
741 | v = BV1; |
742 | else if (v > BV2) |
743 | v = BV2; |
744 | |
745 | if (u2 < BU1) |
746 | u2 = BU1; |
747 | else if (u2 > BU2) |
c48e2889 |
748 | u2 = BU2; |
9bd59d1c |
749 | } |
750 | |
751 | ErrUL->Value(iLS) = 0.0; |
752 | kUEnd = 1; |
753 | JU = 0; |
754 | |
755 | if (Abs(u2 - u1) < EPS_PARAM) |
756 | continue; |
757 | |
c04c30b3 |
758 | NCollection_Handle<math_Vector> aDummy; |
9bd59d1c |
759 | iUSubEnd = FillIntervalBounds(u1, u2, UKnots, NumSubs, anInertiaU, U1, U2, ErrU, aDummy); |
760 | UMaxSubs = BRepGProp_Gauss::MaxSubs(iUSubEnd); |
761 | |
762 | if (UMaxSubs > SM) |
763 | UMaxSubs = SM; |
764 | |
765 | BRepGProp_Gauss::InitMass(0.0, 1, UMaxSubs, anInertiaU); |
766 | BRepGProp_Gauss::Init(ErrU, 0.0, 1, UMaxSubs); |
767 | ErrorU = 0.0; |
768 | |
769 | do |
770 | {//while: U |
771 | if (++JU > iUSubEnd) |
772 | { |
773 | URange[0] = IU = ErrU->Max(); |
774 | URange[1] = JU; |
775 | |
776 | U1->Value(JU) = (U1->Value(IU) + U2->Value(IU)) * 0.5; |
777 | U2->Value(JU) = U2->Value(IU); |
778 | U2->Value(IU) = U1->Value(JU); |
779 | } |
780 | else |
781 | URange[0] = IU = JU; |
782 | |
783 | if (JU == UMaxSubs || Abs(U2->Value(JU) - U1->Value(JU)) < EPS_PARAM) |
784 | if (kUEnd == 1) |
785 | { |
786 | ErrU->Value(JU) = 0.0; |
787 | anInertiaU->ChangeValue(JU).Reset(); |
788 | } |
789 | else |
790 | { |
791 | --JU; |
792 | EpsU = ErrorU; |
793 | Eps = 10. * EpsU * Abs((u2 - u1) * Dul); |
794 | EpsL = 0.9 * Eps; |
795 | break; |
796 | } |
797 | else |
798 | { |
799 | gp_Pnt aPoint; |
800 | gp_Vec aNormal; |
801 | |
802 | for (kU = 0; kU < kUEnd; ++kU) |
803 | { |
804 | BRepGProp_Gauss::Inertia aLocal[2]; |
805 | |
806 | Standard_Integer iUS = URange[kU]; |
807 | const Standard_Integer aLength = iGLEnd - iGL; |
808 | |
809 | const Standard_Real um = 0.5 * (U2->Value(iUS) + U1->Value(iUS)); |
810 | const Standard_Real ur = 0.5 * (U2->Value(iUS) - U1->Value(iUS)); |
811 | |
812 | for (Standard_Integer iGU = 0; iGU < aLength; ++iGU) |
813 | { |
814 | for (Standard_Integer iU = 1; iU <= NbUGaussP[iGU]; ++iU) |
815 | { |
816 | Standard_Real w = UGaussW[iGU]->Value(iU); |
817 | const Standard_Real u = um + ur * UGaussP[iGU]->Value(iU); |
818 | |
819 | theSurface.Normal(u, v, aPoint, aNormal); |
820 | |
821 | if (myType == Vinert) |
822 | { |
823 | computeVInertiaOfElementaryPart( |
824 | aPoint, aNormal, theLocation, w, theCoeff, theIsByPoint, aLocal[iGU]); |
825 | } |
826 | else |
827 | { |
828 | if (iGU > 0) |
829 | aLocal[iGU].Mass += (w * aNormal.Magnitude()); |
830 | else |
831 | { |
832 | computeSInertiaOfElementaryPart( |
833 | aPoint, aNormal, theLocation, w, aLocal[iGU]); |
834 | } |
835 | } |
836 | } |
837 | } |
838 | |
839 | BRepGProp_Gauss::Inertia& anUI = |
840 | anInertiaU->ChangeValue(iUS); |
841 | |
842 | anUI.Mass = mult(aLocal[0].Mass, ur); |
843 | |
844 | if (myType == Vinert) |
845 | { |
846 | anUI.Ixx = mult(aLocal[0].Ixx, ur); |
847 | anUI.Iyy = mult(aLocal[0].Iyy, ur); |
848 | anUI.Izz = mult(aLocal[0].Izz, ur); |
849 | } |
850 | |
851 | if (iGL > 0) |
852 | continue; |
853 | |
854 | Standard_Real aDMass = Abs(aLocal[1].Mass - aLocal[0].Mass); |
855 | |
856 | if (myType == Vinert) |
857 | { |
858 | aLocal[1].Ixx = Abs(aLocal[1].Ixx - aLocal[0].Ixx); |
859 | aLocal[1].Iyy = Abs(aLocal[1].Iyy - aLocal[0].Iyy); |
860 | aLocal[1].Izz = Abs(aLocal[1].Izz - aLocal[0].Izz); |
861 | |
862 | anUI.Ix = mult(aLocal[0].Ix, ur); |
863 | anUI.Iy = mult(aLocal[0].Iy, ur); |
864 | anUI.Iz = mult(aLocal[0].Iz, ur); |
865 | |
866 | anUI.Ixy = mult(aLocal[0].Ixy, ur); |
867 | anUI.Ixz = mult(aLocal[0].Ixz, ur); |
868 | anUI.Iyz = mult(aLocal[0].Iyz, ur); |
869 | |
870 | #ifndef IS_MIN_DIM |
871 | aDMass = aLocal[1].Ixx + aLocal[1].Iyy + aLocal[1].Izz; |
872 | #endif |
873 | |
874 | ErrU->Value(iUS) = mult(aDMass, ur); |
875 | } |
876 | else |
877 | { |
878 | anUI.Ix = mult(aLocal[0].Ix, ur); |
879 | anUI.Iy = mult(aLocal[0].Iy, ur); |
880 | anUI.Iz = mult(aLocal[0].Iz, ur); |
881 | anUI.Ixx = mult(aLocal[0].Ixx, ur); |
882 | anUI.Iyy = mult(aLocal[0].Iyy, ur); |
883 | anUI.Izz = mult(aLocal[0].Izz, ur); |
884 | anUI.Ixy = mult(aLocal[0].Ixy, ur); |
885 | anUI.Ixz = mult(aLocal[0].Ixz, ur); |
886 | anUI.Iyz = mult(aLocal[0].Iyz, ur); |
887 | |
888 | ErrU->Value(iUS) = mult(aDMass, ur); |
889 | } |
890 | } |
891 | } |
892 | |
893 | if (JU == iUSubEnd) |
894 | { |
895 | kUEnd = 2; |
896 | ErrorU = ErrU->Value(ErrU->Max()); |
897 | } |
898 | } while ( (ErrorU - EpsU > 0.0 && EpsU != 0.0) || kUEnd == 1 ); |
899 | |
900 | for (i = 1; i <= JU; ++i) |
901 | { |
902 | const BRepGProp_Gauss::Inertia& anIU = |
903 | anInertiaU->Value(i); |
904 | |
905 | CDim[iGL] = add(CDim[iGL], mult(anIU.Mass, Dul)); |
906 | CIxx[iGL] = add(CIxx[iGL], mult(anIU.Ixx, Dul)); |
907 | CIyy[iGL] = add(CIyy[iGL], mult(anIU.Iyy, Dul)); |
908 | CIzz[iGL] = add(CIzz[iGL], mult(anIU.Izz, Dul)); |
909 | } |
910 | |
911 | if (iGL > 0) |
912 | continue; |
913 | |
914 | ErrUL->Value(iLS) = ErrorU * Abs((u2 - u1) * Dul); |
915 | |
916 | for (i = 1; i <= JU; ++i) |
917 | { |
918 | const BRepGProp_Gauss::Inertia& anIU = |
919 | anInertiaU->Value(i); |
920 | |
921 | CIx = add(CIx, mult(anIU.Ix, Dul)); |
922 | CIy = add(CIy, mult(anIU.Iy, Dul)); |
923 | CIz = add(CIz, mult(anIU.Iz, Dul)); |
924 | |
925 | CIxy = add(CIxy, mult(anIU.Ixy, Dul)); |
926 | CIxz = add(CIxz, mult(anIU.Ixz, Dul)); |
927 | CIyz = add(CIyz, mult(anIU.Iyz, Dul)); |
928 | } |
c48e2889 |
929 | }//for: iL |
9bd59d1c |
930 | }//for: iGL |
931 | |
932 | BRepGProp_Gauss::Inertia& aLI = anInertiaL->ChangeValue(iLS); |
933 | |
934 | aLI.Mass = mult(CDim[0], lr); |
935 | aLI.Ixx = mult(CIxx[0], lr); |
936 | aLI.Iyy = mult(CIyy[0], lr); |
937 | aLI.Izz = mult(CIzz[0], lr); |
938 | |
939 | if (iGLEnd == 2) |
940 | { |
941 | Standard_Real aSubDim = Abs(CDim[1] - CDim[0]); |
942 | |
943 | if (myType == Vinert) |
944 | { |
945 | ErrorU = ErrUL->Value(iLS); |
946 | |
947 | CIxx[1] = Abs(CIxx[1] - CIxx[0]); |
948 | CIyy[1] = Abs(CIyy[1] - CIyy[0]); |
949 | CIzz[1] = Abs(CIzz[1] - CIzz[0]); |
950 | |
951 | #ifndef IS_MIN_DIM |
952 | aSubDim = CIxx[1] + CIyy[1] + CIzz[1]; |
953 | #endif |
954 | |
955 | ErrL->Value(iLS) = add(mult(aSubDim, lr), ErrorU); |
956 | } |
957 | else |
958 | { |
959 | ErrL->Value(iLS) = add(mult(aSubDim, lr), ErrUL->Value(iLS)); |
960 | } |
961 | } |
962 | |
963 | aLI.Ix = mult(CIx, lr); |
964 | aLI.Iy = mult(CIy, lr); |
965 | aLI.Iz = mult(CIz, lr); |
966 | |
967 | aLI.Ixy = mult(CIxy, lr); |
968 | aLI.Ixz = mult(CIxz, lr); |
969 | aLI.Iyz = mult(CIyz, lr); |
970 | }//for: (kL)iLS |
c48e2889 |
971 | } |
9bd59d1c |
972 | |
c48e2889 |
973 | // Calculate/correct epsilon of computation by current value of dim |
974 | // That is need for not spend time for |
975 | if (JL == iLSubEnd) |
976 | { |
977 | kLEnd = 2; |
9bd59d1c |
978 | |
c48e2889 |
979 | Standard_Real DDim = 0.0; |
980 | for (i = 1; i <= JL; ++i) |
981 | DDim += anInertiaL->Value(i).Mass; |
9bd59d1c |
982 | |
c48e2889 |
983 | #ifndef IS_MIN_DIM |
984 | { |
985 | if (myType == Vinert) |
9bd59d1c |
986 | { |
c48e2889 |
987 | Standard_Real DIxx = 0.0, DIyy = 0.0, DIzz = 0.0; |
988 | for (i = 1; i <= JL; ++i) |
9bd59d1c |
989 | { |
c48e2889 |
990 | const BRepGProp_Gauss::Inertia& aLocalL = |
991 | anInertiaL->Value(i); |
9bd59d1c |
992 | |
c48e2889 |
993 | DIxx += aLocalL.Ixx; |
994 | DIyy += aLocalL.Iyy; |
995 | DIzz += aLocalL.Izz; |
9bd59d1c |
996 | } |
9bd59d1c |
997 | |
c48e2889 |
998 | DDim = Abs(DIxx) + Abs(DIyy) + Abs(DIzz); |
9bd59d1c |
999 | } |
1000 | } |
c48e2889 |
1001 | #endif |
1002 | |
1003 | DDim = Abs(DDim * anEpsilon); |
1004 | |
1005 | if (DDim > Eps) |
9bd59d1c |
1006 | { |
c48e2889 |
1007 | Eps = DDim; |
1008 | EpsL = 0.9 * Eps; |
9bd59d1c |
1009 | } |
c48e2889 |
1010 | } |
1011 | if (kLEnd == 2) |
1012 | { |
1013 | ErrorL = ErrL->Value(ErrL->Max()); |
1014 | } |
9bd59d1c |
1015 | } while ( (ErrorL - EpsL > 0.0 && isVerifyComputation) || kLEnd == 1 ); |
1016 | |
1017 | for ( i = 1; i <= JL; i++ ) |
c48e2889 |
1018 | { |
9bd59d1c |
1019 | addAndRestoreInertia(anInertiaL->Value(i), anInertia); |
c48e2889 |
1020 | } |
9bd59d1c |
1021 | |
1022 | ErrorLMax = Max(ErrorLMax, ErrorL); |
1023 | } |
1024 | |
1025 | if (isNaturalRestriction) |
1026 | break; |
1027 | |
1028 | theDomain.Next(); |
1029 | } |
1030 | |
1031 | if (myType == Vinert) |
1032 | convert(anInertia, theCoeff, theIsByPoint, theOutGravityCenter, theOutInertia, theOutMass); |
1033 | else |
1034 | convert(anInertia, theOutGravityCenter, theOutInertia, theOutMass); |
1035 | |
1036 | if (iGLEnd == 2) |
1037 | { |
1038 | if (theOutMass != 0.0) |
1039 | { |
1040 | Eps = ErrorLMax / Abs(theOutMass); |
1041 | |
1042 | #ifndef IS_MIN_DIM |
1043 | { |
1044 | if (myType == Vinert) |
1045 | Eps = ErrorLMax / (Abs(anInertia.Ixx) + |
1046 | Abs(anInertia.Iyy) + |
1047 | Abs(anInertia.Izz)); |
1048 | } |
1049 | #endif |
1050 | } |
1051 | else |
1052 | { |
1053 | Eps = 0.0; |
1054 | } |
1055 | } |
1056 | else |
1057 | { |
1058 | Eps = anEpsilon; |
1059 | } |
1060 | |
1061 | return Eps; |
1062 | } |
1063 | |
1064 | //======================================================================= |
1065 | //function : Compute |
1066 | //purpose : |
1067 | //======================================================================= |
1068 | Standard_Real BRepGProp_Gauss::Compute(BRepGProp_Face& theSurface, |
1069 | BRepGProp_Domain& theDomain, |
1070 | const gp_Pnt& theLocation, |
1071 | const Standard_Real theEps, |
1072 | Standard_Real& theOutMass, |
1073 | gp_Pnt& theOutGravityCenter, |
1074 | gp_Mat& theOutInertia) |
1075 | { |
1076 | Standard_ASSERT_RAISE(myType == Sinert, "BRepGProp_Gauss: Incorrect type"); |
1077 | |
1078 | return Compute(theSurface, |
1079 | theDomain, |
1080 | theLocation, |
1081 | theEps, |
1082 | NULL, |
1083 | Standard_True, |
1084 | theOutMass, |
1085 | theOutGravityCenter, |
1086 | theOutInertia); |
1087 | } |
1088 | |
1089 | //======================================================================= |
1090 | //function : Compute |
1091 | //purpose : |
1092 | //======================================================================= |
1093 | void BRepGProp_Gauss::Compute(BRepGProp_Face& theSurface, |
1094 | BRepGProp_Domain& theDomain, |
1095 | const gp_Pnt& theLocation, |
1096 | Standard_Real& theOutMass, |
1097 | gp_Pnt& theOutGravityCenter, |
1098 | gp_Mat& theOutInertia) |
1099 | { |
1100 | Standard_ASSERT_RAISE(myType == Sinert, "BRepGProp_Gauss: Incorrect type"); |
1101 | |
1102 | Standard_Real u1, u2, v1, v2; |
1103 | theSurface.Bounds (u1, u2, v1, v2); |
1104 | checkBounds(u1, u2, v1, v2); |
1105 | |
1106 | const Standard_Integer NbUGaussgp_Pnts = |
1107 | Min(theSurface.UIntegrationOrder(), math::GaussPointsMax()); |
1108 | |
1109 | const Standard_Integer NbVGaussgp_Pnts = |
1110 | Min(theSurface.VIntegrationOrder(), math::GaussPointsMax()); |
1111 | |
1112 | const Standard_Integer NbGaussgp_Pnts = |
1113 | Max(NbUGaussgp_Pnts, NbVGaussgp_Pnts); |
1114 | |
1115 | // Number of Gauss points for the integration on the face |
1116 | math_Vector GaussSPV (1, NbGaussgp_Pnts); |
1117 | math_Vector GaussSWV (1, NbGaussgp_Pnts); |
1118 | math::GaussPoints (NbGaussgp_Pnts, GaussSPV); |
1119 | math::GaussWeights(NbGaussgp_Pnts, GaussSWV); |
1120 | |
1121 | BRepGProp_Gauss::Inertia anInertia; |
8ff2e494 |
1122 | for (; theDomain.More(); theDomain.Next()) |
9bd59d1c |
1123 | { |
8ff2e494 |
1124 | if (!theSurface.Load(theDomain.Value())) |
1125 | { |
1126 | return; |
1127 | } |
9bd59d1c |
1128 | |
5b0f2540 |
1129 | Standard_Integer NbCGaussgp_Pnts = |
9bd59d1c |
1130 | Min(theSurface.IntegrationOrder(), math::GaussPointsMax()); |
1131 | |
5b0f2540 |
1132 | NbCGaussgp_Pnts = Max(NbCGaussgp_Pnts, NbGaussgp_Pnts); |
1133 | |
9bd59d1c |
1134 | math_Vector GaussCP(1, NbCGaussgp_Pnts); |
1135 | math_Vector GaussCW(1, NbCGaussgp_Pnts); |
1136 | math::GaussPoints (NbCGaussgp_Pnts, GaussCP); |
1137 | math::GaussWeights(NbCGaussgp_Pnts, GaussCW); |
1138 | |
1139 | |
1140 | const Standard_Real l1 = theSurface.FirstParameter(); |
1141 | const Standard_Real l2 = theSurface.LastParameter (); |
1142 | const Standard_Real lm = 0.5 * (l2 + l1); |
1143 | const Standard_Real lr = 0.5 * (l2 - l1); |
1144 | |
1145 | BRepGProp_Gauss::Inertia aCInertia; |
1146 | for (Standard_Integer i = 1; i <= NbCGaussgp_Pnts; ++i) |
1147 | { |
1148 | const Standard_Real l = lm + lr * GaussCP(i); |
1149 | |
1150 | gp_Pnt2d Puv; |
1151 | gp_Vec2d Vuv; |
1152 | theSurface.D12d(l, Puv, Vuv); |
1153 | |
1154 | const Standard_Real v = Puv.Y(); |
1155 | u2 = Puv.X(); |
1156 | |
1157 | const Standard_Real Dul = Vuv.Y() * GaussCW(i); |
1158 | const Standard_Real um = 0.5 * (u2 + u1); |
1159 | const Standard_Real ur = 0.5 * (u2 - u1); |
1160 | |
1161 | BRepGProp_Gauss::Inertia aLocalInertia; |
1162 | for (Standard_Integer j = 1; j <= NbGaussgp_Pnts; ++j) |
1163 | { |
1164 | const Standard_Real u = add(um, mult(ur, GaussSPV(j))); |
1165 | const Standard_Real aWeight = Dul * GaussSWV(j); |
1166 | |
1167 | gp_Pnt aPoint; |
1168 | gp_Vec aNormal; |
1169 | theSurface.Normal (u, v, aPoint, aNormal); |
1170 | |
1171 | computeSInertiaOfElementaryPart(aPoint, aNormal, theLocation, aWeight, aLocalInertia); |
1172 | } |
1173 | |
1174 | multAndRestoreInertia(ur, aLocalInertia); |
1175 | addAndRestoreInertia (aLocalInertia, aCInertia); |
1176 | } |
1177 | |
1178 | multAndRestoreInertia(lr, aCInertia); |
1179 | addAndRestoreInertia (aCInertia, anInertia); |
9bd59d1c |
1180 | } |
1181 | |
1182 | convert(anInertia, theOutGravityCenter, theOutInertia, theOutMass); |
1183 | } |
1184 | |
1185 | //======================================================================= |
1186 | //function : Compute |
1187 | //purpose : |
1188 | //======================================================================= |
1189 | void BRepGProp_Gauss::Compute(BRepGProp_Face& theSurface, |
1190 | BRepGProp_Domain& theDomain, |
1191 | const gp_Pnt& theLocation, |
1192 | const Standard_Real theCoeff[], |
1193 | const Standard_Boolean theIsByPoint, |
1194 | Standard_Real& theOutMass, |
1195 | gp_Pnt& theOutGravityCenter, |
1196 | gp_Mat& theOutInertia) |
1197 | { |
1198 | Standard_ASSERT_RAISE(myType == Vinert, "BRepGProp_Gauss: Incorrect type"); |
1199 | |
1200 | Standard_Real u1, v1, u2, v2; |
1201 | theSurface.Bounds (u1, u2, v1, v2); |
1202 | checkBounds(u1, u2, v1, v2); |
1203 | |
1204 | Standard_Real _u2 = u2; //OCC104 |
1205 | |
1206 | BRepGProp_Gauss::Inertia anInertia; |
8ff2e494 |
1207 | for (; theDomain.More(); theDomain.Next()) |
9bd59d1c |
1208 | { |
8ff2e494 |
1209 | if (!theSurface.Load(theDomain.Value())) |
1210 | { |
1211 | return; |
1212 | } |
9bd59d1c |
1213 | |
1214 | const Standard_Integer aVNbCGaussgp_Pnts = |
1215 | theSurface.VIntegrationOrder(); |
1216 | |
1217 | const Standard_Integer aNbGaussgp_Pnts = |
1218 | Min( Max(theSurface.IntegrationOrder(), aVNbCGaussgp_Pnts), math::GaussPointsMax() ); |
1219 | |
1220 | math_Vector GaussP(1, aNbGaussgp_Pnts); |
1221 | math_Vector GaussW(1, aNbGaussgp_Pnts); |
1222 | math::GaussPoints (aNbGaussgp_Pnts, GaussP); |
1223 | math::GaussWeights(aNbGaussgp_Pnts, GaussW); |
1224 | |
1225 | const Standard_Real l1 = theSurface.FirstParameter(); |
1226 | const Standard_Real l2 = theSurface.LastParameter(); |
1227 | const Standard_Real lm = 0.5 * (l2 + l1); |
1228 | const Standard_Real lr = 0.5 * (l2 - l1); |
1229 | |
1230 | BRepGProp_Gauss::Inertia aCInertia; |
1231 | for (Standard_Integer i = 1; i <= aNbGaussgp_Pnts; ++i) |
1232 | { |
1233 | const Standard_Real l = lm + lr * GaussP(i); |
1234 | |
1235 | gp_Pnt2d Puv; |
1236 | gp_Vec2d Vuv; |
1237 | |
1238 | theSurface.D12d(l, Puv, Vuv); |
1239 | |
1240 | u2 = Puv.X(); |
1241 | u2 = Min( Max(u1, u2), _u2 ); // OCC104 |
1242 | const Standard_Real v = Min(Max(Puv.Y(), v1), v2); |
1243 | |
1244 | const Standard_Real Dul = Vuv.Y() * GaussW(i); |
1245 | const Standard_Real um = 0.5 * (u2 + u1); |
1246 | const Standard_Real ur = 0.5 * (u2 - u1); |
1247 | |
1248 | BRepGProp_Gauss::Inertia aLocalInertia; |
1249 | for (Standard_Integer j = 1; j <= aNbGaussgp_Pnts; ++j) |
1250 | { |
1251 | const Standard_Real u = um + ur * GaussP(j); |
1252 | const Standard_Real aWeight = Dul * GaussW(j); |
1253 | |
1254 | gp_Pnt aPoint; |
1255 | gp_Vec aNormal; |
1256 | |
1257 | theSurface.Normal(u, v, aPoint, aNormal); |
1258 | |
1259 | computeVInertiaOfElementaryPart( |
1260 | aPoint, |
1261 | aNormal, |
1262 | theLocation, |
1263 | aWeight, |
1264 | theCoeff, |
1265 | theIsByPoint, |
1266 | aLocalInertia); |
1267 | } |
1268 | |
1269 | multAndRestoreInertia(ur, aLocalInertia); |
1270 | addAndRestoreInertia (aLocalInertia, aCInertia); |
1271 | } |
1272 | |
1273 | multAndRestoreInertia(lr, aCInertia); |
1274 | addAndRestoreInertia (aCInertia, anInertia); |
9bd59d1c |
1275 | } |
1276 | |
1277 | convert(anInertia, theCoeff, theIsByPoint, theOutGravityCenter, theOutInertia, theOutMass); |
1278 | } |
1279 | |
1280 | //======================================================================= |
1281 | //function : Compute |
1282 | //purpose : |
1283 | //======================================================================= |
1284 | void BRepGProp_Gauss::Compute(const BRepGProp_Face& theSurface, |
1285 | const gp_Pnt& theLocation, |
1286 | const Standard_Real theCoeff[], |
1287 | const Standard_Boolean theIsByPoint, |
1288 | Standard_Real& theOutMass, |
1289 | gp_Pnt& theOutGravityCenter, |
1290 | gp_Mat& theOutInertia) |
1291 | { |
1292 | Standard_Real LowerU, UpperU, LowerV, UpperV; |
1293 | theSurface.Bounds(LowerU, UpperU, LowerV, UpperV); |
1294 | checkBounds(LowerU, UpperU, LowerV, UpperV); |
1295 | |
1296 | const Standard_Integer UOrder = |
1297 | Min(theSurface.UIntegrationOrder(), math::GaussPointsMax()); |
1298 | const Standard_Integer VOrder = |
1299 | Min(theSurface.VIntegrationOrder(), math::GaussPointsMax()); |
1300 | |
1301 | // Gauss points and weights |
1302 | math_Vector GaussPU(1, UOrder); |
1303 | math_Vector GaussWU(1, UOrder); |
1304 | math_Vector GaussPV(1, VOrder); |
1305 | math_Vector GaussWV(1, VOrder); |
1306 | |
1307 | math::GaussPoints (UOrder, GaussPU); |
1308 | math::GaussWeights(UOrder, GaussWU); |
1309 | math::GaussPoints (VOrder, GaussPV); |
1310 | math::GaussWeights(VOrder, GaussWV); |
1311 | |
1312 | const Standard_Real um = 0.5 * add(UpperU, LowerU); |
1313 | const Standard_Real vm = 0.5 * add(UpperV, LowerV); |
1314 | Standard_Real ur = 0.5 * add(UpperU, -LowerU); |
1315 | Standard_Real vr = 0.5 * add(UpperV, -LowerV); |
1316 | |
1317 | gp_Pnt aPoint; |
1318 | gp_Vec aNormal; |
1319 | |
1320 | BRepGProp_Gauss::Inertia anInertia; |
1321 | for (Standard_Integer j = 1; j <= VOrder; ++j) |
1322 | { |
1323 | BRepGProp_Gauss::Inertia anInertiaOfElementaryPart; |
1324 | const Standard_Real v = add(vm, mult(vr, GaussPV(j))); |
1325 | |
1326 | for (Standard_Integer i = 1; i <= UOrder; ++i) |
1327 | { |
1328 | const Standard_Real aWeight = GaussWU(i); |
1329 | const Standard_Real u = add(um, mult(ur, GaussPU (i))); |
1330 | theSurface.Normal(u, v, aPoint, aNormal); |
1331 | |
1332 | if (myType == Vinert) |
1333 | { |
1334 | computeVInertiaOfElementaryPart( |
1335 | aPoint, |
1336 | aNormal, |
1337 | theLocation, |
1338 | aWeight, |
1339 | theCoeff, |
1340 | theIsByPoint, |
1341 | anInertiaOfElementaryPart); |
1342 | } |
1343 | else // Sinert |
1344 | { |
1345 | computeSInertiaOfElementaryPart( |
1346 | aPoint, |
1347 | aNormal, |
1348 | theLocation, |
1349 | aWeight, |
1350 | anInertiaOfElementaryPart); |
1351 | } |
1352 | } |
1353 | |
1354 | multAndRestoreInertia(GaussWV(j), anInertiaOfElementaryPart); |
1355 | addAndRestoreInertia (anInertiaOfElementaryPart, anInertia); |
1356 | } |
1357 | vr = mult(vr, ur); |
1358 | anInertia.Ixx = mult(vr, anInertia.Ixx); |
1359 | anInertia.Iyy = mult(vr, anInertia.Iyy); |
1360 | anInertia.Izz = mult(vr, anInertia.Izz); |
1361 | anInertia.Ixy = mult(vr, anInertia.Ixy); |
1362 | anInertia.Ixz = mult(vr, anInertia.Ixz); |
1363 | anInertia.Iyz = mult(vr, anInertia.Iyz); |
1364 | |
1365 | if (myType == Vinert) |
1366 | { |
1367 | convert(anInertia, theCoeff, theIsByPoint, theOutGravityCenter, theOutInertia, theOutMass); |
1368 | } |
1369 | else // Sinert |
1370 | { |
1371 | convert(anInertia, theOutGravityCenter, theOutInertia, theOutMass); |
1372 | } |
1373 | |
1374 | theOutMass *= vr; |
1375 | } |
1376 | |
1377 | //======================================================================= |
1378 | //function : Compute |
1379 | //purpose : |
1380 | //======================================================================= |
1381 | void BRepGProp_Gauss::Compute(const BRepGProp_Face& theSurface, |
1382 | const gp_Pnt& theLocation, |
1383 | Standard_Real& theOutMass, |
1384 | gp_Pnt& theOutGravityCenter, |
1385 | gp_Mat& theOutInertia) |
1386 | { |
1387 | Standard_ASSERT_RAISE(myType == Sinert, "BRepGProp_Gauss: Incorrect type"); |
1388 | |
1389 | Compute(theSurface, |
1390 | theLocation, |
1391 | NULL, |
1392 | Standard_True, |
1393 | theOutMass, |
1394 | theOutGravityCenter, |
1395 | theOutInertia); |
1396 | } |