| 1 | #include <Standard_NotImplemented.hxx> |
| 2 | #include <math_Vector.hxx> |
| 3 | #include <math.hxx> |
| 4 | #include <gp_Pnt2d.hxx> |
| 5 | #include <gp_Vec2d.hxx> |
| 6 | #include <gp_Pnt.hxx> |
| 7 | #include <gp_Vec.hxx> |
| 8 | |
| 9 | #include <TColStd_Array1OfReal.hxx> |
| 10 | #include <Precision.hxx> |
| 11 | |
| 12 | class HMath_Vector{ |
| 13 | math_Vector *pvec; |
| 14 | void operator=(const math_Vector&){} |
| 15 | public: |
| 16 | HMath_Vector(){ pvec = 0;} |
| 17 | HMath_Vector(math_Vector* pv){ pvec = pv;} |
| 18 | ~HMath_Vector(){ if(pvec != 0) delete pvec;} |
| 19 | void operator=(math_Vector* pv){ if(pvec != pv && pvec != 0) delete pvec; pvec = pv;} |
| 20 | Standard_Real& operator()(Standard_Integer i){ return (*pvec).operator()(i);} |
| 21 | const Standard_Real& operator()(Standard_Integer i) const{ return (*pvec).operator()(i);} |
| 22 | const math_Vector* operator->() const{ return pvec;} |
| 23 | math_Vector* operator->(){ return pvec;} |
| 24 | math_Vector* Vector(){ return pvec;} |
| 25 | math_Vector* Init(Standard_Real v, Standard_Integer i = 0, Standard_Integer iEnd = 0){ |
| 26 | if(pvec == 0) return pvec; |
| 27 | if(iEnd - i == 0) pvec->Init(v); |
| 28 | else { Standard_Integer End = (iEnd <= pvec->Upper()) ? iEnd : pvec->Upper(); |
| 29 | for(; i <= End; i++) pvec->operator()(i) = v; } |
| 30 | return pvec; |
| 31 | } |
| 32 | }; |
| 33 | |
| 34 | static Standard_Real EPS_PARAM = 1.e-12; |
| 35 | static Standard_Real EPS_DIM = 1.e-20; |
| 36 | static Standard_Real ERROR_ALGEBR_RATIO = 2.0/3.0; |
| 37 | |
| 38 | static Standard_Integer GPM = 61; |
| 39 | static Standard_Integer SUBS_POWER = 32; |
| 40 | static Standard_Integer SM = 1953; |
| 41 | |
| 42 | static math_Vector LGaussP0(1,GPM); |
| 43 | static math_Vector LGaussW0(1,GPM); |
| 44 | static math_Vector LGaussP1(1,RealToInt(Ceiling(ERROR_ALGEBR_RATIO*GPM))); |
| 45 | static math_Vector LGaussW1(1,RealToInt(Ceiling(ERROR_ALGEBR_RATIO*GPM))); |
| 46 | |
| 47 | static math_Vector* LGaussP[] = {&LGaussP0,&LGaussP1}; |
| 48 | static math_Vector* LGaussW[] = {&LGaussW0,&LGaussW1}; |
| 49 | |
| 50 | static HMath_Vector L1 = new math_Vector(1,SM,0.0); |
| 51 | static HMath_Vector L2 = new math_Vector(1,SM,0.0); |
| 52 | static HMath_Vector DimL = new math_Vector(1,SM,0.0); |
| 53 | static HMath_Vector ErrL = new math_Vector(1,SM,0.0); |
| 54 | static HMath_Vector ErrUL = new math_Vector(1,SM,0.0); |
| 55 | static HMath_Vector IxL = new math_Vector(1,SM,0.0); |
| 56 | static HMath_Vector IyL = new math_Vector(1,SM,0.0); |
| 57 | static HMath_Vector IzL = new math_Vector(1,SM,0.0); |
| 58 | static HMath_Vector IxxL = new math_Vector(1,SM,0.0); |
| 59 | static HMath_Vector IyyL = new math_Vector(1,SM,0.0); |
| 60 | static HMath_Vector IzzL = new math_Vector(1,SM,0.0); |
| 61 | static HMath_Vector IxyL = new math_Vector(1,SM,0.0); |
| 62 | static HMath_Vector IxzL = new math_Vector(1,SM,0.0); |
| 63 | static HMath_Vector IyzL = new math_Vector(1,SM,0.0); |
| 64 | |
| 65 | static math_Vector UGaussP0(1,GPM); |
| 66 | static math_Vector UGaussW0(1,GPM); |
| 67 | static math_Vector UGaussP1(1,RealToInt(Ceiling(ERROR_ALGEBR_RATIO*GPM))); |
| 68 | static math_Vector UGaussW1(1,RealToInt(Ceiling(ERROR_ALGEBR_RATIO*GPM))); |
| 69 | |
| 70 | static math_Vector* UGaussP[] = {&UGaussP0,&UGaussP1}; |
| 71 | static math_Vector* UGaussW[] = {&UGaussW0,&UGaussW1}; |
| 72 | |
| 73 | static HMath_Vector U1 = new math_Vector(1,SM,0.0); |
| 74 | static HMath_Vector U2 = new math_Vector(1,SM,0.0); |
| 75 | static HMath_Vector DimU = new math_Vector(1,SM,0.0); |
| 76 | static HMath_Vector ErrU = new math_Vector(1,SM,0.0); |
| 77 | static HMath_Vector IxU = new math_Vector(1,SM,0.0); |
| 78 | static HMath_Vector IyU = new math_Vector(1,SM,0.0); |
| 79 | static HMath_Vector IzU = new math_Vector(1,SM,0.0); |
| 80 | static HMath_Vector IxxU = new math_Vector(1,SM,0.0); |
| 81 | static HMath_Vector IyyU = new math_Vector(1,SM,0.0); |
| 82 | static HMath_Vector IzzU = new math_Vector(1,SM,0.0); |
| 83 | static HMath_Vector IxyU = new math_Vector(1,SM,0.0); |
| 84 | static HMath_Vector IxzU = new math_Vector(1,SM,0.0); |
| 85 | static HMath_Vector IyzU = new math_Vector(1,SM,0.0); |
| 86 | |
| 87 | static Standard_Integer FillIntervalBounds(Standard_Real A, |
| 88 | Standard_Real B, |
| 89 | const TColStd_Array1OfReal& Knots, |
| 90 | HMath_Vector& VA, |
| 91 | HMath_Vector& VB) |
| 92 | { |
| 93 | Standard_Integer i = 1, iEnd = Knots.Upper(), j = 1, k = 1; |
| 94 | VA(j++) = A; |
| 95 | for(; i <= iEnd; i++){ |
| 96 | Standard_Real kn = Knots(i); |
| 97 | if(A < kn) |
| 98 | if(kn < B) VA(j++) = VB(k++) = kn; else break; |
| 99 | } |
| 100 | VB(k) = B; |
| 101 | return k; |
| 102 | } |
| 103 | |
| 104 | static inline Standard_Integer MaxSubs(Standard_Integer n, Standard_Integer coeff = SUBS_POWER){ |
| 105 | // return n = IntegerLast()/coeff < n? IntegerLast(): n*coeff + 1; |
| 106 | return Min((n * coeff + 1),SM); |
| 107 | } |
| 108 | |
| 109 | static Standard_Integer LFillIntervalBounds(Standard_Real A, |
| 110 | Standard_Real B, |
| 111 | const TColStd_Array1OfReal& Knots, |
| 112 | const Standard_Integer NumSubs) |
| 113 | { |
| 114 | Standard_Integer iEnd = Knots.Upper(), jEnd = L1->Upper(); |
| 115 | if(iEnd - 1 > jEnd){ |
| 116 | iEnd = MaxSubs(iEnd-1,NumSubs); |
| 117 | L1 = new math_Vector(1,iEnd); |
| 118 | L2 = new math_Vector(1,iEnd); |
| 119 | DimL = new math_Vector(1,iEnd); |
| 120 | ErrL = new math_Vector(1,iEnd,0.0); |
| 121 | ErrUL = new math_Vector(1,iEnd,0.0); |
| 122 | IxL = new math_Vector(1,iEnd); |
| 123 | IyL = new math_Vector(1,iEnd); |
| 124 | IzL = new math_Vector(1,iEnd); |
| 125 | IxxL = new math_Vector(1,iEnd); |
| 126 | IyyL = new math_Vector(1,iEnd); |
| 127 | IzzL = new math_Vector(1,iEnd); |
| 128 | IxyL = new math_Vector(1,iEnd); |
| 129 | IxzL = new math_Vector(1,iEnd); |
| 130 | IyzL = new math_Vector(1,iEnd); |
| 131 | } |
| 132 | return FillIntervalBounds(A, B, Knots, L1, L2); |
| 133 | } |
| 134 | |
| 135 | static Standard_Integer UFillIntervalBounds(Standard_Real A, |
| 136 | Standard_Real B, |
| 137 | const TColStd_Array1OfReal& Knots, |
| 138 | const Standard_Integer NumSubs) |
| 139 | { |
| 140 | Standard_Integer iEnd = Knots.Upper(), jEnd = U1->Upper(); |
| 141 | if(iEnd - 1 > jEnd){ |
| 142 | iEnd = MaxSubs(iEnd-1,NumSubs); |
| 143 | U1 = new math_Vector(1,iEnd); |
| 144 | U2 = new math_Vector(1,iEnd); |
| 145 | DimU = new math_Vector(1,iEnd); |
| 146 | ErrU = new math_Vector(1,iEnd,0.0); |
| 147 | IxU = new math_Vector(1,iEnd); |
| 148 | IyU = new math_Vector(1,iEnd); |
| 149 | IzU = new math_Vector(1,iEnd); |
| 150 | IxxU = new math_Vector(1,iEnd); |
| 151 | IyyU = new math_Vector(1,iEnd); |
| 152 | IzzU = new math_Vector(1,iEnd); |
| 153 | IxyU = new math_Vector(1,iEnd); |
| 154 | IxzU = new math_Vector(1,iEnd); |
| 155 | IyzU = new math_Vector(1,iEnd); |
| 156 | } |
| 157 | return FillIntervalBounds(A, B, Knots, U1, U2); |
| 158 | } |
| 159 | |
| 160 | static Standard_Real CCompute(Face& S, |
| 161 | Domain& D, |
| 162 | const gp_Pnt& loc, |
| 163 | Standard_Real& Dim, |
| 164 | gp_Pnt& g, |
| 165 | gp_Mat& inertia, |
| 166 | const Standard_Real EpsDim, |
| 167 | const Standard_Boolean isErrorCalculation, |
| 168 | const Standard_Boolean isVerifyComputation) |
| 169 | { |
| 170 | Standard_Boolean isNaturalRestriction = S.NaturalRestriction(); |
| 171 | |
| 172 | Standard_Integer NumSubs = SUBS_POWER; |
| 173 | |
| 174 | Standard_Real Ix, Iy, Iz, Ixx, Iyy, Izz, Ixy, Ixz, Iyz; |
| 175 | Dim = Ix = Iy = Iz = Ixx = Iyy = Izz = Ixy = Ixz = Iyz = 0.0; |
| 176 | Standard_Real x, y, z; |
| 177 | //boundary curve parametrization |
| 178 | Standard_Real l1, l2, lm, lr, l; |
| 179 | //Face parametrization in U and V direction |
| 180 | Standard_Real BV1, BV2, v; |
| 181 | Standard_Real BU1, BU2, u1, u2, um, ur, u; |
| 182 | S.Bounds (BU1, BU2, BV1, BV2); u1 = BU1; |
| 183 | //location point used to compute the inertia |
| 184 | Standard_Real xloc, yloc, zloc; |
| 185 | loc.Coord (xloc, yloc, zloc); // use member of parent class |
| 186 | //Jacobien (x, y, z) -> (u, v) = ||n|| |
| 187 | Standard_Real ds; |
| 188 | //On the Face |
| 189 | gp_Pnt Ps; |
| 190 | gp_Vec VNor; |
| 191 | //On the boundary curve u-v |
| 192 | gp_Pnt2d Puv; |
| 193 | gp_Vec2d Vuv; |
| 194 | Standard_Real Dul; // Dul = Du / Dl |
| 195 | Standard_Real CDim[2], CIx, CIy, CIz, CIxx, CIyy, CIzz, CIxy, CIxz, CIyz; |
| 196 | Standard_Real LocDim[2], LocIx, LocIy, LocIz, LocIxx, LocIyy, LocIzz, LocIxy, LocIxz, LocIyz; |
| 197 | |
| 198 | Standard_Real ErrorU, ErrorL, ErrorLMax = 0.0, Eps=0.0, EpsL=0.0, EpsU=0.0; |
| 199 | |
| 200 | Standard_Integer iD = 0, NbLSubs, iLS, iLSubEnd, iGL, iGLEnd, NbLGaussP[2], LRange[2], iL, kL, kLEnd, IL, JL; |
| 201 | Standard_Integer i, NbUSubs, iUS, iUSubEnd, iGU, iGUEnd, NbUGaussP[2], URange[2], iU, kU, kUEnd, IU, JU; |
| 202 | Standard_Integer UMaxSubs, LMaxSubs; |
| 203 | iGLEnd = isErrorCalculation? 2: 1; |
| 204 | for(i = 0; i < 2; i++) { |
| 205 | LocDim[i] = 0.0; |
| 206 | CDim[i] = 0.0; |
| 207 | } |
| 208 | |
| 209 | NbUGaussP[0] = S.SIntOrder(EpsDim); |
| 210 | NbUGaussP[1] = RealToInt(Ceiling(ERROR_ALGEBR_RATIO*NbUGaussP[0])); |
| 211 | math::GaussPoints(NbUGaussP[0],UGaussP0); math::GaussWeights(NbUGaussP[0],UGaussW0); |
| 212 | math::GaussPoints(NbUGaussP[1],UGaussP1); math::GaussWeights(NbUGaussP[1],UGaussW1); |
| 213 | |
| 214 | NbUSubs = S.SUIntSubs(); |
| 215 | TColStd_Array1OfReal UKnots(1,NbUSubs+1); |
| 216 | S.UKnots(UKnots); |
| 217 | |
| 218 | |
| 219 | while (isNaturalRestriction || D.More()) { |
| 220 | if(isNaturalRestriction){ |
| 221 | NbLGaussP[0] = Min(2*NbUGaussP[0],math::GaussPointsMax()); |
| 222 | }else{ |
| 223 | S.Load(D.Value()); ++iD; |
| 224 | NbLGaussP[0] = S.LIntOrder(EpsDim); |
| 225 | } |
| 226 | |
| 227 | |
| 228 | NbLGaussP[1] = RealToInt(Ceiling(ERROR_ALGEBR_RATIO*NbLGaussP[0])); |
| 229 | math::GaussPoints(NbLGaussP[0],LGaussP0); math::GaussWeights(NbLGaussP[0],LGaussW0); |
| 230 | math::GaussPoints(NbLGaussP[1],LGaussP1); math::GaussWeights(NbLGaussP[1],LGaussW1); |
| 231 | |
| 232 | NbLSubs = isNaturalRestriction? S.SVIntSubs(): S.LIntSubs(); |
| 233 | |
| 234 | TColStd_Array1OfReal LKnots(1,NbLSubs+1); |
| 235 | if(isNaturalRestriction){ |
| 236 | S.VKnots(LKnots); |
| 237 | l1 = BV1; l2 = BV2; |
| 238 | }else{ |
| 239 | S.LKnots(LKnots); |
| 240 | l1 = S.FirstParameter(); l2 = S.LastParameter(); |
| 241 | } |
| 242 | ErrorL = 0.0; |
| 243 | kLEnd = 1; JL = 0; |
| 244 | //OCC503(apo): if(Abs(l2-l1) < EPS_PARAM) continue; |
| 245 | if(Abs(l2-l1) > EPS_PARAM) { |
| 246 | iLSubEnd = LFillIntervalBounds(l1, l2, LKnots, NumSubs); |
| 247 | LMaxSubs = MaxSubs(iLSubEnd); |
| 248 | if(LMaxSubs > DimL.Vector()->Upper()) LMaxSubs = DimL.Vector()->Upper(); |
| 249 | DimL.Init(0.0,1,LMaxSubs); ErrL.Init(0.0,1,LMaxSubs); ErrUL.Init(0.0,1,LMaxSubs); |
| 250 | do{// while: L |
| 251 | if(++JL > iLSubEnd){ |
| 252 | LRange[0] = IL = ErrL->Max(); LRange[1] = JL; |
| 253 | L1(JL) = (L1(IL) + L2(IL))/2.0; L2(JL) = L2(IL); L2(IL) = L1(JL); |
| 254 | }else LRange[0] = IL = JL; |
| 255 | if(JL == LMaxSubs || Abs(L2(JL) - L1(JL)) < EPS_PARAM) |
| 256 | if(kLEnd == 1){ |
| 257 | DimL(JL) = ErrL(JL) = IxL(JL) = IyL(JL) = IzL(JL) = |
| 258 | IxxL(JL) = IyyL(JL) = IzzL(JL) = IxyL(JL) = IxzL(JL) = IyzL(JL) = 0.0; |
| 259 | }else{ |
| 260 | JL--; |
| 261 | EpsL = ErrorL; Eps = EpsL/0.9; |
| 262 | break; |
| 263 | } |
| 264 | else |
| 265 | for(kL=0; kL < kLEnd; kL++){ |
| 266 | iLS = LRange[kL]; |
| 267 | lm = 0.5*(L2(iLS) + L1(iLS)); |
| 268 | lr = 0.5*(L2(iLS) - L1(iLS)); |
| 269 | CIx = CIy = CIz = CIxx = CIyy = CIzz = CIxy = CIxz = CIyz = 0.0; |
| 270 | for(iGL=0; iGL < iGLEnd; iGL++){// |
| 271 | CDim[iGL] = 0.0; |
| 272 | for(iL=1; iL<=NbLGaussP[iGL]; iL++){ |
| 273 | l = lm + lr*(*LGaussP[iGL])(iL); |
| 274 | if(isNaturalRestriction){ |
| 275 | v = l; u2 = BU2; Dul = (*LGaussW[iGL])(iL); |
| 276 | }else{ |
| 277 | S.D12d (l, Puv, Vuv); |
| 278 | Dul = Vuv.Y()*(*LGaussW[iGL])(iL); // Dul = Du / Dl |
| 279 | if(Abs(Dul) < EPS_PARAM) continue; |
| 280 | v = Puv.Y(); u2 = Puv.X(); |
| 281 | //Check on cause out off bounds of value current parameter |
| 282 | if(v < BV1) v = BV1; else if(v > BV2) v = BV2; |
| 283 | if(u2 < BU1) u2 = BU1; else if(u2 > BU2) u2 = BU2; |
| 284 | } |
| 285 | ErrUL(iLS) = 0.0; |
| 286 | kUEnd = 1; JU = 0; |
| 287 | if(Abs(u2-u1) < EPS_PARAM) continue; |
| 288 | iUSubEnd = UFillIntervalBounds(u1, u2, UKnots, NumSubs); |
| 289 | UMaxSubs = MaxSubs(iUSubEnd); |
| 290 | if(UMaxSubs > DimU.Vector()->Upper()) UMaxSubs = DimU.Vector()->Upper(); |
| 291 | DimU.Init(0.0,1,UMaxSubs); ErrU.Init(0.0,1,UMaxSubs); ErrorU = 0.0; |
| 292 | do{//while: U |
| 293 | if(++JU > iUSubEnd){ |
| 294 | URange[0] = IU = ErrU->Max(); URange[1] = JU; |
| 295 | U1(JU) = (U1(IU)+U2(IU))/2.0; U2(JU) = U2(IU); U2(IU) = U1(JU); |
| 296 | }else URange[0] = IU = JU; |
| 297 | if(JU == UMaxSubs || Abs(U2(JU) - U1(JU)) < EPS_PARAM) |
| 298 | if(kUEnd == 1){ |
| 299 | DimU(JU) = ErrU(JU) = IxU(JU) = IyU(JU) = IzU(JU) = |
| 300 | IxxU(JU) = IyyU(JU) = IzzU(JU) = IxyU(JU) = IxzU(JU) = IyzU(JU) = 0.0; |
| 301 | }else{ |
| 302 | JU--; |
| 303 | EpsU = ErrorU; Eps = EpsU*Abs((u2-u1)*Dul)/0.1; EpsL = 0.9*Eps; |
| 304 | break; |
| 305 | } |
| 306 | else |
| 307 | for(kU=0; kU < kUEnd; kU++){ |
| 308 | iUS = URange[kU]; |
| 309 | um = 0.5*(U2(iUS) + U1(iUS)); |
| 310 | ur = 0.5*(U2(iUS) - U1(iUS)); |
| 311 | LocIx = LocIy = LocIz = LocIxx = LocIyy = LocIzz = LocIxy = LocIxz = LocIyz = 0.0; |
| 312 | iGUEnd = iGLEnd - iGL; |
| 313 | for(iGU=0; iGU < iGUEnd; iGU++){// |
| 314 | LocDim[iGU] = 0.0; |
| 315 | for(iU=1; iU<=NbUGaussP[iGU]; iU++){ |
| 316 | u = um + ur*(*UGaussP[iGU])(iU); |
| 317 | S.Normal(u, v, Ps, VNor); |
| 318 | ds = VNor.Magnitude(); //Jacobien(x,y,z) -> (u,v)=||n|| |
| 319 | ds *= (*UGaussW[iGU])(iU); |
| 320 | LocDim[iGU] += ds; |
| 321 | if(iGU > 0) continue; |
| 322 | Ps.Coord(x, y, z); |
| 323 | x -= xloc; y -= yloc; z -= zloc; |
| 324 | LocIx += x*ds; LocIy += y*ds; LocIz += z*ds; |
| 325 | LocIxy += x*y*ds; LocIyz += y*z*ds; LocIxz += x*z*ds; |
| 326 | x *= x; y *= y; z *= z; |
| 327 | LocIxx += (y+z)*ds; LocIyy += (x+z)*ds; LocIzz += (x+y)*ds; |
| 328 | }//for: iU |
| 329 | }//for: iGU |
| 330 | DimU(iUS) = LocDim[0]*ur; |
| 331 | if(iGL > 0) continue; |
| 332 | ErrU(iUS) = Abs(LocDim[1]-LocDim[0])*ur; |
| 333 | IxU(iUS) = LocIx*ur; IyU(iUS) = LocIy*ur; IzU(iUS) = LocIz*ur; |
| 334 | IxxU(iUS) = LocIxx*ur; IyyU(iUS) = LocIyy*ur; IzzU(iUS) = LocIzz*ur; |
| 335 | IxyU(iUS) = LocIxy*ur; IxzU(iUS) = LocIxz*ur; IyzU(iUS) = LocIyz*ur; |
| 336 | }//for: kU (iUS) |
| 337 | if(JU == iUSubEnd) kUEnd = 2; |
| 338 | if(kUEnd == 2) ErrorU = ErrU(ErrU->Max()); |
| 339 | }while((ErrorU - EpsU > 0.0 && EpsU != 0.0) || kUEnd == 1); |
| 340 | for(i=1; i<=JU; i++) CDim[iGL] += DimU(i)*Dul; |
| 341 | if(iGL > 0) continue; |
| 342 | ErrUL(iLS) = ErrorU*Abs((u2-u1)*Dul); |
| 343 | for(i=1; i<=JU; i++){ |
| 344 | CIx += IxU(i)*Dul; CIy += IyU(i)*Dul; CIz += IzU(i)*Dul; |
| 345 | CIxx += IxxU(i)*Dul; CIyy += IyyU(i)*Dul; CIzz += IzzU(i)*Dul; |
| 346 | CIxy += IxyU(i)*Dul; CIxz += IxzU(i)*Dul; CIyz += IyzU(i)*Dul; |
| 347 | } |
| 348 | }//for: iL |
| 349 | }//for: iGL |
| 350 | DimL(iLS) = CDim[0]*lr; |
| 351 | if(iGLEnd == 2) ErrL(iLS) = Abs(CDim[1]-CDim[0])*lr + ErrUL(iLS); |
| 352 | IxL(iLS) = CIx*lr; IyL(iLS) = CIy*lr; IzL(iLS) = CIz*lr; |
| 353 | IxxL(iLS) = CIxx*lr; IyyL(iLS) = CIyy*lr; IzzL(iLS) = CIzz*lr; |
| 354 | IxyL(iLS) = CIxy*lr; IxzL(iLS) = CIxz*lr; IyzL(iLS) = CIyz*lr; |
| 355 | }//for: (kL)iLS |
| 356 | // Calculate/correct epsilon of computation by current value of Dim |
| 357 | //That is need for not spend time for |
| 358 | if(JL == iLSubEnd){ |
| 359 | kLEnd = 2; |
| 360 | Standard_Real DDim = 0.0; |
| 361 | for(i=1; i<=JL; i++) DDim += DimL(i); |
| 362 | DDim = Abs(DDim*EpsDim); |
| 363 | if(DDim > Eps) { |
| 364 | Eps = DDim; EpsL = 0.9*Eps; |
| 365 | } |
| 366 | } |
| 367 | if(kLEnd == 2) ErrorL = ErrL(ErrL->Max()); |
| 368 | }while((ErrorL - EpsL > 0.0 && isVerifyComputation) || kLEnd == 1); |
| 369 | for(i=1; i<=JL; i++){ |
| 370 | Dim += DimL(i); |
| 371 | Ix += IxL(i); Iy += IyL(i); Iz += IzL(i); |
| 372 | Ixx += IxxL(i); Iyy += IyyL(i); Izz += IzzL(i); |
| 373 | Ixy += IxyL(i); Ixz += IxzL(i); Iyz += IyzL(i); |
| 374 | } |
| 375 | ErrorLMax = Max(ErrorLMax, ErrorL); |
| 376 | } |
| 377 | if(isNaturalRestriction) break; |
| 378 | D.Next(); |
| 379 | } |
| 380 | if(Abs(Dim) >= EPS_DIM){ |
| 381 | Ix /= Dim; Iy /= Dim; Iz /= Dim; |
| 382 | g.SetCoord (Ix, Iy, Iz); |
| 383 | }else{ |
| 384 | Dim =0.0; |
| 385 | g.SetCoord (0., 0.,0.); |
| 386 | } |
| 387 | inertia = gp_Mat (gp_XYZ (Ixx, -Ixy, -Ixz), |
| 388 | gp_XYZ (-Ixy, Iyy, -Iyz), |
| 389 | gp_XYZ (-Ixz, -Iyz, Izz)); |
| 390 | |
| 391 | if(iGLEnd == 2) Eps = Dim != 0.0? ErrorLMax/Abs(Dim): 0.0; |
| 392 | else Eps = EpsDim; |
| 393 | return Eps; |
| 394 | } |
| 395 | |
| 396 | static Standard_Real Compute(Face& S, const gp_Pnt& loc, Standard_Real& Dim, gp_Pnt& g, gp_Mat& inertia, |
| 397 | Standard_Real EpsDim) |
| 398 | { |
| 399 | Standard_Boolean isErrorCalculation = 0.0 > EpsDim || EpsDim < 0.001? 1: 0; |
| 400 | Standard_Boolean isVerifyComputation = 0.0 < EpsDim && EpsDim < 0.001? 1: 0; |
| 401 | EpsDim = Abs(EpsDim); |
| 402 | Domain D; |
| 403 | return CCompute(S,D,loc,Dim,g,inertia,EpsDim,isErrorCalculation,isVerifyComputation); |
| 404 | } |
| 405 | |
| 406 | static Standard_Real Compute(Face& S, Domain& D, const gp_Pnt& loc, Standard_Real& Dim, gp_Pnt& g, gp_Mat& inertia, |
| 407 | Standard_Real EpsDim) |
| 408 | { |
| 409 | Standard_Boolean isErrorCalculation = 0.0 > EpsDim || EpsDim < 0.001? 1: 0; |
| 410 | Standard_Boolean isVerifyComputation = 0.0 < EpsDim && EpsDim < 0.001? 1: 0; |
| 411 | EpsDim = Abs(EpsDim); |
| 412 | return CCompute(S,D,loc,Dim,g,inertia,EpsDim,isErrorCalculation,isVerifyComputation); |
| 413 | } |
| 414 | |
| 415 | static void Compute(Face& S, Domain& D, const gp_Pnt& loc, Standard_Real& dim, gp_Pnt& g, gp_Mat& inertia){ |
| 416 | Standard_Real Ix, Iy, Iz, Ixx, Iyy, Izz, Ixy, Ixz, Iyz; |
| 417 | dim = Ix = Iy = Iz = Ixx = Iyy = Izz = Ixy = Ixz = Iyz = 0.0; |
| 418 | |
| 419 | Standard_Real x, y, z; |
| 420 | Standard_Integer NbCGaussgp_Pnts = 0; |
| 421 | |
| 422 | Standard_Real l1, l2, lm, lr, l; //boundary curve parametrization |
| 423 | Standard_Real v1, v2, vm, vr, v; //Face parametrization in v direction |
| 424 | Standard_Real u1, u2, um, ur, u; |
| 425 | Standard_Real ds; //Jacobien (x, y, z) -> (u, v) = ||n|| |
| 426 | |
| 427 | gp_Pnt P; //On the Face |
| 428 | gp_Vec VNor; |
| 429 | |
| 430 | gp_Pnt2d Puv; //On the boundary curve u-v |
| 431 | gp_Vec2d Vuv; |
| 432 | Standard_Real Dul; // Dul = Du / Dl |
| 433 | Standard_Real CArea, CIx, CIy, CIz, CIxx, CIyy, CIzz, CIxy, CIxz, CIyz; |
| 434 | Standard_Real LocArea, LocIx, LocIy, LocIz, LocIxx, LocIyy, LocIzz, LocIxy, |
| 435 | LocIxz, LocIyz; |
| 436 | |
| 437 | |
| 438 | S.Bounds (u1, u2, v1, v2); |
| 439 | |
| 440 | Standard_Integer NbUGaussgp_Pnts = Min(S.UIntegrationOrder (), |
| 441 | math::GaussPointsMax()); |
| 442 | Standard_Integer NbVGaussgp_Pnts = Min(S.VIntegrationOrder (), |
| 443 | math::GaussPointsMax()); |
| 444 | |
| 445 | Standard_Integer NbGaussgp_Pnts = Max(NbUGaussgp_Pnts, NbVGaussgp_Pnts); |
| 446 | |
| 447 | //Number of Gauss points for the integration |
| 448 | //on the Face |
| 449 | math_Vector GaussSPV (1, NbGaussgp_Pnts); |
| 450 | math_Vector GaussSWV (1, NbGaussgp_Pnts); |
| 451 | math::GaussPoints (NbGaussgp_Pnts,GaussSPV); |
| 452 | math::GaussWeights (NbGaussgp_Pnts,GaussSWV); |
| 453 | |
| 454 | |
| 455 | //location point used to compute the inertia |
| 456 | Standard_Real xloc, yloc, zloc; |
| 457 | loc.Coord (xloc, yloc, zloc); |
| 458 | |
| 459 | while (D.More()) { |
| 460 | |
| 461 | S.Load(D.Value()); |
| 462 | NbCGaussgp_Pnts = Min(S.IntegrationOrder (), math::GaussPointsMax()); |
| 463 | |
| 464 | math_Vector GaussCP (1, NbCGaussgp_Pnts); |
| 465 | math_Vector GaussCW (1, NbCGaussgp_Pnts); |
| 466 | math::GaussPoints (NbCGaussgp_Pnts,GaussCP); |
| 467 | math::GaussWeights (NbCGaussgp_Pnts,GaussCW); |
| 468 | |
| 469 | CArea = 0.0; |
| 470 | CIx = CIy = CIz = CIxx = CIyy = CIzz = CIxy = CIxz = CIyz = 0.0; |
| 471 | l1 = S.FirstParameter (); |
| 472 | l2 = S.LastParameter (); |
| 473 | lm = 0.5 * (l2 + l1); |
| 474 | lr = 0.5 * (l2 - l1); |
| 475 | |
| 476 | Puv = S.Value2d (lm); |
| 477 | vm = Puv.Y(); |
| 478 | Puv = S.Value2d (lr); |
| 479 | vr = Puv.Y(); |
| 480 | |
| 481 | for (Standard_Integer i = 1; i <= NbCGaussgp_Pnts; i++) { |
| 482 | l = lm + lr * GaussCP (i); |
| 483 | S.D12d(l, Puv, Vuv); |
| 484 | v = Puv.Y(); |
| 485 | u2 = Puv.X(); |
| 486 | Dul = Vuv.Y(); |
| 487 | Dul *= GaussCW (i); |
| 488 | um = 0.5 * (u2 + u1); |
| 489 | ur = 0.5 * (u2 - u1); |
| 490 | LocArea = LocIx = LocIy = LocIz = LocIxx = LocIyy = LocIzz = |
| 491 | LocIxy = LocIxz = LocIyz = 0.0; |
| 492 | for (Standard_Integer j = 1; j <= NbGaussgp_Pnts; j++) { |
| 493 | u = um + ur * GaussSPV (j); |
| 494 | S.Normal (u, v, P, VNor); |
| 495 | ds = VNor.Magnitude(); //normal.Magnitude |
| 496 | ds = ds * Dul * GaussSWV (j); |
| 497 | LocArea += ds; |
| 498 | P.Coord (x, y, z); |
| 499 | x -= xloc; |
| 500 | y -= yloc; |
| 501 | z -= zloc; |
| 502 | LocIx += x * ds; |
| 503 | LocIy += y * ds; |
| 504 | LocIz += z * ds; |
| 505 | LocIxy += x * y * ds; |
| 506 | LocIyz += y * z * ds; |
| 507 | LocIxz += x * z * ds; |
| 508 | x *= x; |
| 509 | y *= y; |
| 510 | z *= z; |
| 511 | LocIxx += (y + z) * ds; |
| 512 | LocIyy += (x + z) * ds; |
| 513 | LocIzz += (x + y) * ds; |
| 514 | } |
| 515 | CArea += LocArea * ur; |
| 516 | CIx += LocIx * ur; |
| 517 | CIy += LocIy * ur; |
| 518 | CIz += LocIz * ur; |
| 519 | CIxx += LocIxx * ur; |
| 520 | CIyy += LocIyy * ur; |
| 521 | CIzz += LocIzz * ur; |
| 522 | CIxy += LocIxy * ur; |
| 523 | CIxz += LocIxz * ur; |
| 524 | CIyz += LocIyz * ur; |
| 525 | } |
| 526 | dim += CArea * lr; |
| 527 | Ix += CIx * lr; |
| 528 | Iy += CIy * lr; |
| 529 | Iz += CIz * lr; |
| 530 | Ixx += CIxx * lr; |
| 531 | Iyy += CIyy * lr; |
| 532 | Izz += CIzz * lr; |
| 533 | Ixy += CIxy * lr; |
| 534 | Ixz += CIxz * lr; |
| 535 | Iyz += CIyz * lr; |
| 536 | D.Next(); |
| 537 | } |
| 538 | if (Abs(dim) >= EPS_DIM) { |
| 539 | Ix /= dim; |
| 540 | Iy /= dim; |
| 541 | Iz /= dim; |
| 542 | g.SetCoord (Ix, Iy, Iz); |
| 543 | } |
| 544 | else { |
| 545 | dim =0.; |
| 546 | g.SetCoord (0., 0.,0.); |
| 547 | } |
| 548 | inertia = gp_Mat (gp_XYZ (Ixx, -Ixy, -Ixz), |
| 549 | gp_XYZ (-Ixy, Iyy, -Iyz), |
| 550 | gp_XYZ (-Ixz, -Iyz, Izz)); |
| 551 | } |
| 552 | |
| 553 | |
| 554 | |
| 555 | static void Compute(const Face& S, const gp_Pnt& loc, Standard_Real& dim, gp_Pnt& g, gp_Mat& inertia){ |
| 556 | Standard_Real Ix, Iy, Iz, Ixx, Iyy, Izz, Ixy, Ixz, Iyz; |
| 557 | dim = Ix = Iy = Iz = Ixx = Iyy = Izz = Ixy = Ixz = Iyz = 0.0; |
| 558 | |
| 559 | Standard_Real LowerU, UpperU, LowerV, UpperV; |
| 560 | S.Bounds (LowerU, UpperU, LowerV, UpperV); |
| 561 | Standard_Integer UOrder = Min(S.UIntegrationOrder (), |
| 562 | math::GaussPointsMax()); |
| 563 | Standard_Integer VOrder = Min(S.VIntegrationOrder (), |
| 564 | math::GaussPointsMax()); |
| 565 | gp_Pnt P; |
| 566 | gp_Vec VNor; |
| 567 | Standard_Real dsi, ds; |
| 568 | Standard_Real ur, um, u, vr, vm, v; |
| 569 | Standard_Real x, y, z; |
| 570 | Standard_Real Ixi, Iyi, Izi, Ixxi, Iyyi, Izzi, Ixyi, Ixzi, Iyzi; |
| 571 | Standard_Real xloc, yloc, zloc; |
| 572 | loc.Coord (xloc, yloc, zloc); |
| 573 | |
| 574 | Standard_Integer i, j; |
| 575 | math_Vector GaussPU (1, UOrder); //gauss points and weights |
| 576 | math_Vector GaussWU (1, UOrder); |
| 577 | math_Vector GaussPV (1, VOrder); |
| 578 | math_Vector GaussWV (1, VOrder); |
| 579 | |
| 580 | //Recuperation des points de Gauss dans le fichier GaussPoints. |
| 581 | math::GaussPoints (UOrder,GaussPU); |
| 582 | math::GaussWeights (UOrder,GaussWU); |
| 583 | math::GaussPoints (VOrder,GaussPV); |
| 584 | math::GaussWeights (VOrder,GaussWV); |
| 585 | |
| 586 | // Calcul des integrales aux points de gauss : |
| 587 | um = 0.5 * (UpperU + LowerU); |
| 588 | vm = 0.5 * (UpperV + LowerV); |
| 589 | ur = 0.5 * (UpperU - LowerU); |
| 590 | vr = 0.5 * (UpperV - LowerV); |
| 591 | |
| 592 | for (j = 1; j <= VOrder; j++) { |
| 593 | v = vm + vr * GaussPV (j); |
| 594 | dsi = Ixi = Iyi = Izi = Ixxi = Iyyi = Izzi = Ixyi = Ixzi = Iyzi = 0.0; |
| 595 | |
| 596 | for (i = 1; i <= UOrder; i++) { |
| 597 | u = um + ur * GaussPU (i); |
| 598 | S.Normal (u, v, P, VNor); |
| 599 | ds = VNor.Magnitude() * GaussWU (i); |
| 600 | P.Coord (x, y, z); |
| 601 | x -= xloc; |
| 602 | y -= yloc; |
| 603 | z -= zloc; |
| 604 | dsi += ds; |
| 605 | Ixi += x * ds; |
| 606 | Iyi += y * ds; |
| 607 | Izi += z * ds; |
| 608 | Ixyi += x * y * ds; |
| 609 | Iyzi += y * z * ds; |
| 610 | Ixzi += x * z * ds; |
| 611 | x *= x; |
| 612 | y *= y; |
| 613 | z *= z; |
| 614 | Ixxi += (y + z) * ds; |
| 615 | Iyyi += (x + z) * ds; |
| 616 | Izzi += (x + y) * ds; |
| 617 | } |
| 618 | dim += dsi * GaussWV (j); |
| 619 | Ix += Ixi * GaussWV (j); |
| 620 | Iy += Iyi * GaussWV (j); |
| 621 | Iz += Izi * GaussWV (j); |
| 622 | Ixx += Ixxi * GaussWV (j); |
| 623 | Iyy += Iyyi * GaussWV (j); |
| 624 | Izz += Izzi * GaussWV (j); |
| 625 | Ixy += Ixyi * GaussWV (j); |
| 626 | Iyz += Iyzi * GaussWV (j); |
| 627 | Ixz += Ixzi * GaussWV (j); |
| 628 | } |
| 629 | vr *= ur; |
| 630 | Ixx *= vr; |
| 631 | Iyy *= vr; |
| 632 | Izz *= vr; |
| 633 | Ixy *= vr; |
| 634 | Ixz *= vr; |
| 635 | Iyz *= vr; |
| 636 | if (Abs(dim) >= EPS_DIM) { |
| 637 | Ix /= dim; |
| 638 | Iy /= dim; |
| 639 | Iz /= dim; |
| 640 | dim *= vr; |
| 641 | g.SetCoord (Ix, Iy, Iz); |
| 642 | } |
| 643 | else { |
| 644 | dim =0.; |
| 645 | g.SetCoord (0.,0.,0.); |
| 646 | } |
| 647 | inertia = gp_Mat (gp_XYZ (Ixx, -Ixy, -Ixz), |
| 648 | gp_XYZ (-Ixy, Iyy, -Iyz), |
| 649 | gp_XYZ (-Ixz, -Iyz, Izz)); |
| 650 | } |
| 651 | |
| 652 | GProp_SGProps::GProp_SGProps(){} |
| 653 | |
| 654 | GProp_SGProps::GProp_SGProps (const Face& S, |
| 655 | const gp_Pnt& SLocation |
| 656 | ) |
| 657 | { |
| 658 | SetLocation(SLocation); |
| 659 | Perform(S); |
| 660 | } |
| 661 | |
| 662 | GProp_SGProps::GProp_SGProps (Face& S, |
| 663 | Domain& D, |
| 664 | const gp_Pnt& SLocation |
| 665 | ) |
| 666 | { |
| 667 | SetLocation(SLocation); |
| 668 | Perform(S,D); |
| 669 | } |
| 670 | |
| 671 | GProp_SGProps::GProp_SGProps(Face& S, const gp_Pnt& SLocation, const Standard_Real Eps){ |
| 672 | SetLocation(SLocation); |
| 673 | Perform(S, Eps); |
| 674 | } |
| 675 | |
| 676 | GProp_SGProps::GProp_SGProps(Face& S, Domain& D, const gp_Pnt& SLocation, const Standard_Real Eps){ |
| 677 | SetLocation(SLocation); |
| 678 | Perform(S, D, Eps); |
| 679 | } |
| 680 | |
| 681 | void GProp_SGProps::SetLocation(const gp_Pnt& SLocation){ |
| 682 | loc = SLocation; |
| 683 | } |
| 684 | |
| 685 | void GProp_SGProps::Perform(const Face& S){ |
| 686 | Compute(S,loc,dim,g,inertia); |
| 687 | myEpsilon = 1.0; |
| 688 | return; |
| 689 | } |
| 690 | |
| 691 | void GProp_SGProps::Perform(Face& S, Domain& D){ |
| 692 | Compute(S,D,loc,dim,g,inertia); |
| 693 | myEpsilon = 1.0; |
| 694 | return; |
| 695 | } |
| 696 | |
| 697 | Standard_Real GProp_SGProps::Perform(Face& S, const Standard_Real Eps){ |
| 698 | return myEpsilon = Compute(S,loc,dim,g,inertia,Eps); |
| 699 | } |
| 700 | |
| 701 | Standard_Real GProp_SGProps::Perform(Face& S, Domain& D, const Standard_Real Eps){ |
| 702 | return myEpsilon = Compute(S,D,loc,dim,g,inertia,Eps); |
| 703 | } |
| 704 | |
| 705 | |
| 706 | Standard_Real GProp_SGProps::GetEpsilon(){ |
| 707 | return myEpsilon; |
| 708 | } |