#include #include #include #include #include #include #include #include #include class HMath_Vector{ math_Vector *pvec; void operator=(const math_Vector&){} public: HMath_Vector(){ pvec = 0;} HMath_Vector(math_Vector* pv){ pvec = pv;} ~HMath_Vector(){ if(pvec != 0) delete pvec;} void operator=(math_Vector* pv){ if(pvec != pv && pvec != 0) delete pvec; pvec = pv;} Standard_Real& operator()(Standard_Integer i){ return (*pvec).operator()(i);} const Standard_Real& operator()(Standard_Integer i) const{ return (*pvec).operator()(i);} const math_Vector* operator->() const{ return pvec;} math_Vector* operator->(){ return pvec;} math_Vector* Init(Standard_Real v, Standard_Integer i = 0, Standard_Integer iEnd = 0){ if(pvec == 0) return pvec; if(iEnd - i == 0) pvec->Init(v); else for(; i <= iEnd; i++) pvec->operator()(i) = v; return pvec; } }; //Minimal value of interval's range for computation | minimal value of "dim" | ... static Standard_Real EPS_PARAM = Precision::Angular(), EPS_DIM = 1.E-30, ERROR_ALGEBR_RATIO = 2.0/3.0; //Maximum of GaussPoints on a subinterval and maximum of subintervals static Standard_Integer GPM = math::GaussPointsMax(), SUBS_POWER = 32, SM = SUBS_POWER*GPM + 1; static Standard_Boolean IS_MIN_DIM = 1; // if the value equal 0 error of algorithm calculted by static moments static math_Vector LGaussP0(1,GPM), LGaussW0(1,GPM), LGaussP1(1,RealToInt(Ceiling(ERROR_ALGEBR_RATIO*GPM))), LGaussW1(1,RealToInt(Ceiling(ERROR_ALGEBR_RATIO*GPM))); static HMath_Vector L1 = new math_Vector(1,SM), L2 = new math_Vector(1,SM), DimL = new math_Vector(1,SM), ErrL = new math_Vector(1,SM), ErrUL = new math_Vector(1,SM,0.0), IxL = new math_Vector(1,SM), IyL = new math_Vector(1,SM), IzL = new math_Vector(1,SM), IxxL = new math_Vector(1,SM), IyyL = new math_Vector(1,SM), IzzL = new math_Vector(1,SM), IxyL = new math_Vector(1,SM), IxzL = new math_Vector(1,SM), IyzL = new math_Vector(1,SM); static math_Vector* LGaussP[] = {&LGaussP0,&LGaussP1}; static math_Vector* LGaussW[] = {&LGaussW0,&LGaussW1}; static math_Vector UGaussP0(1,GPM), UGaussW0(1,GPM), UGaussP1(1,RealToInt(Ceiling(ERROR_ALGEBR_RATIO*GPM))), UGaussW1(1,RealToInt(Ceiling(ERROR_ALGEBR_RATIO*GPM))); static HMath_Vector U1 = new math_Vector(1,SM), U2 = new math_Vector(1,SM), DimU = new math_Vector(1,SM), ErrU = new math_Vector(1,SM,0.0), IxU = new math_Vector(1,SM), IyU = new math_Vector(1,SM), IzU = new math_Vector(1,SM), IxxU = new math_Vector(1,SM), IyyU = new math_Vector(1,SM), IzzU = new math_Vector(1,SM), IxyU = new math_Vector(1,SM), IxzU = new math_Vector(1,SM), IyzU = new math_Vector(1,SM); static math_Vector* UGaussP[] = {&UGaussP0,&UGaussP1}; static math_Vector* UGaussW[] = {&UGaussW0,&UGaussW1}; static Standard_Integer FillIntervalBounds(Standard_Real A, Standard_Real B, const TColStd_Array1OfReal& Knots, HMath_Vector& VA, HMath_Vector& VB) { Standard_Integer i = 1, iEnd = Knots.Upper(), j = 1, k = 1; VA(j++) = A; for(; i <= iEnd; i++){ Standard_Real kn = Knots(i); if(A < kn) if(kn < B) VA(j++) = VB(k++) = kn; else break; } VB(k) = B; return k; } static inline Standard_Integer MaxSubs(Standard_Integer n, Standard_Integer coeff = SUBS_POWER){ return n = IntegerLast()/coeff < n? IntegerLast(): n*coeff + 1; } static Standard_Integer LFillIntervalBounds(Standard_Real A, Standard_Real B, const TColStd_Array1OfReal& Knots, const Standard_Integer NumSubs) { Standard_Integer iEnd = Knots.Upper(), jEnd = L1->Upper(); // Modified by Sergey KHROMOV - Wed Mar 26 11:22:50 2003 iEnd = Max(iEnd, MaxSubs(iEnd-1,NumSubs)); if(iEnd - 1 > jEnd){ // iEnd = MaxSubs(iEnd-1,NumSubs); // Modified by Sergey KHROMOV - Wed Mar 26 11:22:51 2003 L1 = new math_Vector(1,iEnd); L2 = new math_Vector(1,iEnd); DimL = new math_Vector(1,iEnd); ErrL = new math_Vector(1,iEnd,0.0); ErrUL = new math_Vector(1,iEnd,0.0); IxL = new math_Vector(1,iEnd); IyL = new math_Vector(1,iEnd); IzL = new math_Vector(1,iEnd); IxxL = new math_Vector(1,iEnd); IyyL = new math_Vector(1,iEnd); IzzL = new math_Vector(1,iEnd); IxyL = new math_Vector(1,iEnd); IxzL = new math_Vector(1,iEnd); IyzL = new math_Vector(1,iEnd); } return FillIntervalBounds(A, B, Knots, L1, L2); } static Standard_Integer UFillIntervalBounds(Standard_Real A, Standard_Real B, const TColStd_Array1OfReal& Knots, const Standard_Integer NumSubs) { Standard_Integer iEnd = Knots.Upper(), jEnd = U1->Upper(); // Modified by Sergey KHROMOV - Wed Mar 26 11:22:50 2003 iEnd = Max(iEnd, MaxSubs(iEnd-1,NumSubs)); if(iEnd - 1 > jEnd){ // iEnd = MaxSubs(iEnd-1,NumSubs); // Modified by Sergey KHROMOV - Wed Mar 26 11:22:51 2003 U1 = new math_Vector(1,iEnd); U2 = new math_Vector(1,iEnd); DimU = new math_Vector(1,iEnd); ErrU = new math_Vector(1,iEnd,0.0); IxU = new math_Vector(1,iEnd); IyU = new math_Vector(1,iEnd); IzU = new math_Vector(1,iEnd); IxxU = new math_Vector(1,iEnd); IyyU = new math_Vector(1,iEnd); IzzU = new math_Vector(1,iEnd); IxyU = new math_Vector(1,iEnd); IxzU = new math_Vector(1,iEnd); IyzU = new math_Vector(1,iEnd); } return FillIntervalBounds(A, B, Knots, U1, U2); } static Standard_Real CCompute(Face& S, Domain& D, const Standard_Boolean ByPoint, const Standard_Real Coeff[], const gp_Pnt& loc, Standard_Real& Dim, gp_Pnt& g, gp_Mat& inertia, const Standard_Real EpsDim, const Standard_Boolean isErrorCalculation, const Standard_Boolean isVerifyComputation) { Standard_Boolean isNaturalRestriction = S.NaturalRestriction(); Standard_Integer NumSubs = SUBS_POWER; Standard_Boolean isMinDim = IS_MIN_DIM; Standard_Real Ix, Iy, Iz, Ixx, Iyy, Izz, Ixy, Ixz, Iyz; Dim = Ix = Iy = Iz = Ixx = Iyy = Izz = Ixy = Ixz = Iyz = 0.0; //boundary curve parametrization Standard_Real l1, l2, lm, lr, l; //Face parametrization in U and V direction Standard_Real BV1, BV2, v; Standard_Real BU1, BU2, u1, u2, um, ur, u; S.Bounds (BU1, BU2, BV1, BV2); u1 = BU1; //location point used to compute the inertia Standard_Real xloc, yloc, zloc; loc.Coord (xloc, yloc, zloc); //location point used to compute the inertiard (xloc, yloc, zloc); //Jacobien (x, y, z) -> (u, v) = ||n|| Standard_Real xn, yn, zn, s, ds, dDim; Standard_Real x, y, z, xi, px, py, pz, yi, zi, d1, d2, d3; //On the Face gp_Pnt Ps; gp_Vec VNor; //On the boundary curve u-v gp_Pnt2d Puv; gp_Vec2d Vuv; Standard_Real Dul; // Dul = Du / Dl Standard_Real CDim[2], CIx, CIy, CIz, CIxx[2], CIyy[2], CIzz[2], CIxy, CIxz, CIyz; Standard_Real LocDim[2], LocIx[2], LocIy[2], LocIz[2], LocIxx[2], LocIyy[2], LocIzz[2], LocIxy[2], LocIxz[2], LocIyz[2]; Standard_Integer iD = 0, NbLSubs, iLS, iLSubEnd, iGL, iGLEnd, NbLGaussP[2], LRange[2], iL, kL, kLEnd, IL, JL; Standard_Integer i, NbUSubs, iUS, iUSubEnd, iGU, iGUEnd, NbUGaussP[2], URange[2], iU, kU, kUEnd, IU, JU; Standard_Integer UMaxSubs, LMaxSubs; Standard_Real ErrorU, ErrorL, ErrorLMax = 0.0, Eps=0.0, EpsL=0.0, EpsU=0.0; iGLEnd = isErrorCalculation? 2: 1; for(i = 0; i < 2; i++) { LocDim[i] = 0.0; LocIx[i] = 0.0; LocIy[i] = 0.0; LocIz[i] = 0.0; LocIxx[i] = 0.0; LocIyy[i] = 0.0; LocIzz[i] = 0.0; LocIxy[i] = 0.0; LocIyz[i] = 0.0; LocIxz[i] = 0.0; } NbUGaussP[0] = S.SIntOrder(EpsDim); NbUGaussP[1] = RealToInt(Ceiling(ERROR_ALGEBR_RATIO*NbUGaussP[0])); math::GaussPoints(NbUGaussP[0],UGaussP0); math::GaussWeights(NbUGaussP[0],UGaussW0); math::GaussPoints(NbUGaussP[1],UGaussP1); math::GaussWeights(NbUGaussP[1],UGaussW1); NbUSubs = S.SUIntSubs(); TColStd_Array1OfReal UKnots(1,NbUSubs+1); S.UKnots(UKnots); while (isNaturalRestriction || D.More()) { if(isNaturalRestriction){ NbLGaussP[0] = Min(2*NbUGaussP[0],math::GaussPointsMax()); }else{ S.Load(D.Value()); ++iD; NbLGaussP[0] = S.LIntOrder(EpsDim); } NbLGaussP[1] = RealToInt(Ceiling(ERROR_ALGEBR_RATIO*NbLGaussP[0])); math::GaussPoints(NbLGaussP[0],LGaussP0); math::GaussWeights(NbLGaussP[0],LGaussW0); math::GaussPoints(NbLGaussP[1],LGaussP1); math::GaussWeights(NbLGaussP[1],LGaussW1); NbLSubs = isNaturalRestriction? S.SVIntSubs(): S.LIntSubs(); TColStd_Array1OfReal LKnots(1,NbLSubs+1); if(isNaturalRestriction){ S.VKnots(LKnots); l1 = BV1; l2 = BV2; }else{ S.LKnots(LKnots); l1 = S.FirstParameter(); l2 = S.LastParameter(); } ErrorL = 0.0; kLEnd = 1; JL = 0; //OCC503(apo): if(Abs(l2-l1) < EPS_PARAM) continue; if(Abs(l2-l1) > EPS_PARAM) { iLSubEnd = LFillIntervalBounds(l1, l2, LKnots, NumSubs); LMaxSubs = MaxSubs(iLSubEnd); //-- exception avoiding if(LMaxSubs > SM) LMaxSubs = SM; DimL.Init(0.0,1,LMaxSubs); ErrL.Init(0.0,1,LMaxSubs); ErrUL.Init(0.0,1,LMaxSubs); do{// while: L if(++JL > iLSubEnd){ LRange[0] = IL = ErrL->Max(); LRange[1] = JL; L1(JL) = (L1(IL) + L2(IL))/2.0; L2(JL) = L2(IL); L2(IL) = L1(JL); }else LRange[0] = IL = JL; if(JL == LMaxSubs || Abs(L2(JL) - L1(JL)) < EPS_PARAM) if(kLEnd == 1){ DimL(JL) = ErrL(JL) = IxL(JL) = IyL(JL) = IzL(JL) = IxxL(JL) = IyyL(JL) = IzzL(JL) = IxyL(JL) = IxzL(JL) = IyzL(JL) = 0.0; }else{ JL--; EpsL = ErrorL; Eps = EpsL/0.9; break; } else for(kL=0; kL < kLEnd; kL++){ iLS = LRange[kL]; lm = 0.5*(L2(iLS) + L1(iLS)); lr = 0.5*(L2(iLS) - L1(iLS)); CIx = CIy = CIz = CIxy = CIxz = CIyz = 0.0; for(iGL=0; iGL < iGLEnd; iGL++){// CDim[iGL] = CIxx[iGL] = CIyy[iGL] = CIzz[iGL] = 0.0; for(iL=1; iL<=NbLGaussP[iGL]; iL++){ l = lm + lr*(*LGaussP[iGL])(iL); if(isNaturalRestriction){ v = l; u2 = BU2; Dul = (*LGaussW[iGL])(iL); }else{ S.D12d (l, Puv, Vuv); Dul = Vuv.Y()*(*LGaussW[iGL])(iL); // Dul = Du / Dl if(Abs(Dul) < EPS_PARAM) continue; v = Puv.Y(); u2 = Puv.X(); //Check on cause out off bounds of value current parameter if(v < BV1) v = BV1; else if(v > BV2) v = BV2; if(u2 < BU1) u2 = BU1; else if(u2 > BU2) u2 = BU2; } ErrUL(iLS) = 0.0; kUEnd = 1; JU = 0; if(Abs(u2-u1) < EPS_PARAM) continue; iUSubEnd = UFillIntervalBounds(u1, u2, UKnots, NumSubs); UMaxSubs = MaxSubs(iUSubEnd); //-- exception avoiding if(UMaxSubs > SM) UMaxSubs = SM; DimU.Init(0.0,1,UMaxSubs); ErrU.Init(0.0,1,UMaxSubs); ErrorU = 0.0; do{//while: U if(++JU > iUSubEnd){ URange[0] = IU = ErrU->Max(); URange[1] = JU; U1(JU) = (U1(IU)+U2(IU))/2.0; U2(JU) = U2(IU); U2(IU) = U1(JU); }else URange[0] = IU = JU; if(JU == UMaxSubs || Abs(U2(JU) - U1(JU)) < EPS_PARAM) if(kUEnd == 1){ DimU(JU) = ErrU(JU) = IxU(JU) = IyU(JU) = IzU(JU) = IxxU(JU) = IyyU(JU) = IzzU(JU) = IxyU(JU) = IxzU(JU) = IyzU(JU) = 0.0; }else{ JU--; EpsU = ErrorU; Eps = EpsU*Abs((u2-u1)*Dul)/0.1; EpsL = 0.9*Eps; break; } else for(kU=0; kU < kUEnd; kU++){ iUS = URange[kU]; um = 0.5*(U2(iUS) + U1(iUS)); ur = 0.5*(U2(iUS) - U1(iUS)); iGUEnd = iGLEnd - iGL; for(iGU=0; iGU < iGUEnd; iGU++){// LocDim[iGU] = LocIxx[iGU] = LocIyy[iGU] = LocIzz[iGU] = LocIx[iGU] = LocIy[iGU] = LocIz[iGU] = LocIxy[iGU] = LocIxz[iGU] = LocIyz[iGU] = 0.0; for(iU=1; iU<=NbUGaussP[iGU]; iU++){ u = um + ur*(*UGaussP[iGU])(iU); S.Normal(u, v, Ps, VNor); VNor.Coord(xn, yn, zn); Ps.Coord(x, y, z); x -= xloc; y -= yloc; z -= zloc; xn *= (*UGaussW[iGU])(iU); yn *= (*UGaussW[iGU])(iU); zn *= (*UGaussW[iGU])(iU); if(ByPoint){ //volume of elementary cone dDim = (x*xn+y*yn+z*zn)/3.0; //coordinates of cone's center mass px = 0.75*x; py = 0.75*y; pz = 0.75*z; LocDim[iGU] += dDim; //if(iGU > 0) continue; LocIx[iGU] += px*dDim; LocIy[iGU] += py*dDim; LocIz[iGU] += pz*dDim; x -= Coeff[0]; y -= Coeff[1]; z -= Coeff[2]; dDim *= 3.0/5.0; LocIxy[iGU] -= x*y*dDim; LocIyz[iGU] -= y*z*dDim; LocIxz[iGU] -= x*z*dDim; xi = x*x; yi = y*y; zi = z*z; LocIxx[iGU] += (yi+zi)*dDim; LocIyy[iGU] += (xi+zi)*dDim; LocIzz[iGU] += (xi+yi)*dDim; }else{ // by plane s = xn*Coeff[0] + yn*Coeff[1] + zn*Coeff[2]; d1 = Coeff[0]*x + Coeff[1]*y + Coeff[2]*z - Coeff[3]; d2 = d1*d1; d3 = d1*d2/3.0; ds = s*d1; LocDim[iGU] += ds; //if(iGU > 0) continue; LocIx[iGU] += (x - Coeff[0]*d1/2.0) * ds; LocIy[iGU] += (y - Coeff[1]*d1/2.0) * ds; LocIz[iGU] += (z - Coeff[2]*d1/2.0) * ds; px = x-Coeff[0]*d1; py = y-Coeff[1]*d1; pz = z-Coeff[2]*d1; xi = px*px*d1 + px*Coeff[0]*d2 + Coeff[0]*Coeff[0]*d3; yi = py*py*d1 + py*Coeff[1]*d2 + Coeff[1]*Coeff[1]*d3; zi = pz*pz*d1 + pz*Coeff[2]*d2 + Coeff[2]*Coeff[2]*d3; LocIxx[iGU] += (yi+zi)*s; LocIyy[iGU] += (xi+zi)*s; LocIzz[iGU] += (xi+yi)*s; d2 /= 2.0; xi = py*pz*d1 + py*Coeff[2]*d2 + pz*Coeff[1]*d2 + Coeff[1]*Coeff[2]*d3; yi = px*pz*d1 + pz*Coeff[0]*d2 + px*Coeff[2]*d2 + Coeff[0]*Coeff[2]*d3; zi = px*py*d1 + px*Coeff[1]*d2 + py*Coeff[0]*d2 + Coeff[0]*Coeff[1]*d3; LocIxy[iGU] -= zi*s; LocIyz[iGU] -= xi*s; LocIxz[iGU] -= yi*s; } }//for: iU }//for: iGU DimU(iUS) = LocDim[0]*ur; IxxU(iUS) = LocIxx[0]*ur; IyyU(iUS) = LocIyy[0]*ur; IzzU(iUS) = LocIzz[0]*ur; if(iGL > 0) continue; LocDim[1] = Abs(LocDim[1]-LocDim[0]); LocIxx[1] = Abs(LocIxx[1]-LocIxx[0]); LocIyy[1] = Abs(LocIyy[1]-LocIyy[0]); LocIzz[1] = Abs(LocIzz[1]-LocIzz[0]); ErrU(iUS) = isMinDim? LocDim[1]*ur: (LocIxx[1] + LocIyy[1] + LocIzz[1])*ur; IxU(iUS) = LocIx[0]*ur; IyU(iUS) = LocIy[0]*ur; IzU(iUS) = LocIz[0]*ur; IxyU(iUS) = LocIxy[0]*ur; IxzU(iUS) = LocIxz[0]*ur; IyzU(iUS) = LocIyz[0]*ur; }//for: kU (iUS) if(JU == iUSubEnd) kUEnd = 2; if(kUEnd == 2) { Standard_Integer imax = ErrU->Max(); if(imax > 0) ErrorU = ErrU(imax); else ErrorU = 0.0; } }while((ErrorU - EpsU > 0.0 && EpsU != 0.0) || kUEnd == 1); for(i=1; i<=JU; i++) { CDim[iGL] += DimU(i)*Dul; CIxx[iGL] += IxxU(i)*Dul; CIyy[iGL] += IyyU(i)*Dul; CIzz[iGL] += IzzU(i)*Dul; } if(iGL > 0) continue; ErrUL(iLS) = ErrorU*Abs((u2-u1)*Dul); for(i=1; i<=JU; i++){ CIx += IxU(i)*Dul; CIy += IyU(i)*Dul; CIz += IzU(i)*Dul; //CIxx += IxxU(i)*Dul; CIyy += IyyU(i)*Dul; CIzz += IzzU(i)*Dul; CIxy += IxyU(i)*Dul; CIxz += IxzU(i)*Dul; CIyz += IyzU(i)*Dul; } }//for: iL }//for: iGL DimL(iLS) = CDim[0]*lr; IxxL(iLS) = CIxx[0]*lr; IyyL(iLS) = CIyy[0]*lr; IzzL(iLS) = CIzz[0]*lr; if(iGLEnd == 2) { //ErrL(iLS) = Abs(CDim[1]-CDim[0])*lr + ErrUL(iLS); CDim[1] = Abs(CDim[1]-CDim[0]); CIxx[1] = Abs(CIxx[1]-CIxx[0]); CIyy[1] = Abs(CIyy[1]-CIyy[0]); CIzz[1] = Abs(CIzz[1]-CIzz[0]); ErrorU = ErrUL(iLS); ErrL(iLS) = (isMinDim? CDim[1]: (CIxx[1] + CIyy[1] + CIzz[1]))*lr + ErrorU; } IxL(iLS) = CIx*lr; IyL(iLS) = CIy*lr; IzL(iLS) = CIz*lr; //IxxL(iLS) = CIxx*lr; IyyL(iLS) = CIyy*lr; IzzL(iLS) = CIzz*lr; IxyL(iLS) = CIxy*lr; IxzL(iLS) = CIxz*lr; IyzL(iLS) = CIyz*lr; }//for: (kL)iLS // Calculate/correct epsilon of computation by current value of Dim //That is need for not spend time for if(JL == iLSubEnd){ kLEnd = 2; Standard_Real DDim = 0.0, DIxx = 0.0, DIyy = 0.0, DIzz = 0.0; for(i=1; i<=JL; i++) { DDim += DimL(i); DIxx += IxxL(i); DIyy += IyyL(i); DIzz += IzzL(i); } DDim = isMinDim? Abs(DDim): Abs(DIxx) + Abs(DIyy) + Abs(DIzz); DDim = Abs(DDim*EpsDim); if(DDim > Eps) { Eps = DDim; EpsL = 0.9*Eps; } } if(kLEnd == 2) { Standard_Integer imax = ErrL->Max(); if(imax > 0) ErrorL = ErrL(imax); else ErrorL = 0.0; } }while((ErrorL - EpsL > 0.0 && isVerifyComputation) || kLEnd == 1); for(i=1; i<=JL; i++){ Dim += DimL(i); Ix += IxL(i); Iy += IyL(i); Iz += IzL(i); Ixx += IxxL(i); Iyy += IyyL(i); Izz += IzzL(i); Ixy += IxyL(i); Ixz += IxzL(i); Iyz += IyzL(i); } ErrorLMax = Max(ErrorLMax, ErrorL); } if(isNaturalRestriction) break; D.Next(); } if(Abs(Dim) >= EPS_DIM){ if(ByPoint){ Ix = Coeff[0] + Ix/Dim; Iy = Coeff[1] + Iy/Dim; Iz = Coeff[2] + Iz/Dim; }else{ Ix /= Dim; Iy /= Dim; Iz /= Dim; } g.SetCoord (Ix, Iy, Iz); }else{ Dim =0.; g.SetCoord(0.,0.,0.); } inertia.SetCols (gp_XYZ (Ixx, Ixy, Ixz), gp_XYZ (Ixy, Iyy, Iyz), gp_XYZ (Ixz, Iyz, Izz)); if(iGLEnd == 2) Eps = Dim != 0.0? ErrorLMax/(isMinDim? Abs(Dim): (Abs(Ixx) + Abs(Iyy) + Abs(Izz))): 0.0; else Eps = EpsDim; return Eps; } static Standard_Real Compute(Face& S, const Standard_Boolean ByPoint, const Standard_Real Coeff[], const gp_Pnt& loc, Standard_Real& Dim, gp_Pnt& g, gp_Mat& inertia, Standard_Real EpsDim) { Standard_Boolean isErrorCalculation = 0.0 > EpsDim || EpsDim < 0.001? 1: 0; Standard_Boolean isVerifyComputation = 0.0 < EpsDim && EpsDim < 0.001? 1: 0; EpsDim = Abs(EpsDim); Domain D; return CCompute(S,D,ByPoint,Coeff,loc,Dim,g,inertia,EpsDim,isErrorCalculation,isVerifyComputation); } static Standard_Real Compute(Face& S, Domain& D, const Standard_Boolean ByPoint, const Standard_Real Coeff[], const gp_Pnt& loc, Standard_Real& Dim, gp_Pnt& g, gp_Mat& inertia, Standard_Real EpsDim) { Standard_Boolean isErrorCalculation = 0.0 > EpsDim || EpsDim < 0.001? 1: 0; Standard_Boolean isVerifyComputation = 0.0 < EpsDim && EpsDim < 0.001? 1: 0; EpsDim = Abs(EpsDim); return CCompute(S,D,ByPoint,Coeff,loc,Dim,g,inertia,EpsDim,isErrorCalculation,isVerifyComputation); } static void Compute(const Face& S, const Standard_Boolean ByPoint, const Standard_Real Coeff[], const gp_Pnt& Loc, Standard_Real& Volu, gp_Pnt& G, gp_Mat& Inertia) { gp_Pnt P; gp_Vec VNor; Standard_Real dvi, dv; Standard_Real ur, um, u, vr, vm, v; Standard_Real x, y, z, xn, yn, zn, xi, yi, zi; // Standard_Real x, y, z, xn, yn, zn, xi, yi, zi, xyz; Standard_Real px,py,pz,s,d1,d2,d3; Standard_Real Ixi, Iyi, Izi, Ixxi, Iyyi, Izzi, Ixyi, Ixzi, Iyzi; Standard_Real xloc, yloc, zloc; Standard_Real Ix, Iy, Iz, Ixx, Iyy, Izz, Ixy, Ixz, Iyz; Volu = Ix = Iy = Iz = Ixx = Iyy = Izz = Ixy = Ixz = Iyz = 0.0; Loc.Coord (xloc, yloc, zloc); Standard_Real LowerU, UpperU, LowerV, UpperV; S.Bounds ( LowerU, UpperU, LowerV, UpperV); Standard_Integer UOrder = Min(S.UIntegrationOrder (), math::GaussPointsMax()); Standard_Integer VOrder = Min(S.VIntegrationOrder (), math::GaussPointsMax()); Standard_Integer i, j; math_Vector GaussPU (1, UOrder); //gauss points and weights math_Vector GaussWU (1, UOrder); math_Vector GaussPV (1, VOrder); math_Vector GaussWV (1, VOrder); math::GaussPoints (UOrder,GaussPU); math::GaussWeights (UOrder,GaussWU); math::GaussPoints (VOrder,GaussPV); math::GaussWeights (VOrder,GaussWV); um = 0.5 * (UpperU + LowerU); vm = 0.5 * (UpperV + LowerV); ur = 0.5 * (UpperU - LowerU); vr = 0.5 * (UpperV - LowerV); for (j = 1; j <= VOrder; j++) { v = vm + vr * GaussPV (j); dvi = Ixi = Iyi = Izi = Ixxi = Iyyi = Izzi = Ixyi = Ixzi = Iyzi = 0.0; for (i = 1; i <= UOrder; i++) { u = um + ur * GaussPU (i); S.Normal (u, v, P, VNor); VNor.Coord (xn, yn, zn); P.Coord (x, y, z); x -= xloc; y -= yloc; z -= zloc; xn *= GaussWU (i); yn *= GaussWU (i); zn *= GaussWU (i); if (ByPoint) { ///////////////////// /////////////////////// // OFV code // // Initial code // ///////////////////// /////////////////////// // modified by APO dv = (x*xn+y*yn+z*zn)/3.0; //xyz = x * y * z; dvi += dv; //Ixyi += zn * xyz; Ixi += 0.75*x*dv; //Iyzi += xn * xyz; Iyi += 0.75*y*dv; //Ixzi += yn * xyz; Izi += 0.75*z*dv; //xi = x * x * x * xn / 3.0; x -= Coeff[0]; //yi = y * y * y * yn / 3.0; y -= Coeff[1]; //zi = z * z * z * zn / 3.0; z -= Coeff[2]; //Ixxi += (yi + zi); dv *= 3.0/5.0; //Iyyi += (xi + zi); Ixyi -= x*y*dv; //Izzi += (xi + yi); Iyzi -= y*z*dv; //x -= Coeff[0]; Ixzi -= x*z*dv; //y -= Coeff[1]; xi = x*x; //z -= Coeff[2]; yi = y*y; //dv = x * xn + y * yn + z * zn; zi = z*z; //dvi += dv; Ixxi += (yi + zi)*dv; //Ixi += x * dv; Iyyi += (xi + zi)*dv; //Iyi += y * dv; Izzi += (xi + yi)*dv; //Izi += z * dv; } else { // by plane s = xn * Coeff[0] + yn * Coeff[1] + zn * Coeff[2]; d1 = Coeff[0] * x + Coeff[1] * y + Coeff[2] * z - Coeff[3]; d2 = d1 * d1; d3 = d1 * d2 / 3.0; dv = s * d1; dvi += dv; Ixi += (x - (Coeff[0] * d1 / 2.0)) * dv; Iyi += (y - (Coeff[1] * d1 / 2.0)) * dv; Izi += (z - (Coeff[2] * d1 / 2.0)) * dv; px = x - Coeff[0] * d1; py = y - Coeff[1] * d1; pz = z - Coeff[2] * d1; xi = px * px * d1 + px * Coeff[0]* d2 + Coeff[0] * Coeff[0] * d3; yi = py * py * d1 + py * Coeff[1] * d2 + Coeff[1] * Coeff[1] * d3; zi = pz * pz * d1 + pz * Coeff[2] * d2 + Coeff[2] * Coeff[2] * d3; Ixxi += (yi + zi) * s; Iyyi += (xi + zi) * s; Izzi += (xi + yi) * s; d2 /= 2.0; xi = (py * pz * d1) + (py * Coeff[2] * d2) + (pz * Coeff[1] * d2) + (Coeff[1] * Coeff[2] * d3); yi = (px * pz * d1) + (pz * Coeff[0] * d2) + (px * Coeff[2] * d2) + (Coeff[0] * Coeff[2] * d3); zi = (px * py * d1) + (px * Coeff[1] * d2) + (py * Coeff[0] * d2) + (Coeff[0] * Coeff[1] * d3); Ixyi -= zi * s; Iyzi -= xi * s; Ixzi -= yi * s; } } Volu += dvi * GaussWV (j); Ix += Ixi * GaussWV (j); Iy += Iyi * GaussWV (j); Iz += Izi * GaussWV (j); Ixx += Ixxi * GaussWV (j); Iyy += Iyyi * GaussWV (j); Izz += Izzi * GaussWV (j); Ixy += Ixyi * GaussWV (j); Ixz += Ixzi * GaussWV (j); Iyz += Iyzi * GaussWV (j); } vr *= ur; Ixx *= vr; Iyy *= vr; Izz *= vr; Ixy *= vr; Ixz *= vr; Iyz *= vr; if (Abs(Volu) >= EPS_DIM ) { if (ByPoint) { Ix = Coeff[0] + Ix/Volu; Iy = Coeff[1] + Iy/Volu; Iz = Coeff[2] + Iz/Volu; Volu *= vr; } else { //by plane Ix /= Volu; Iy /= Volu; Iz /= Volu; Volu *= vr; } G.SetCoord (Ix, Iy, Iz); } else { G.SetCoord(0.,0.,0.); Volu =0.; } Inertia.SetCols (gp_XYZ (Ixx, Ixy, Ixz), gp_XYZ (Ixy, Iyy, Iyz), gp_XYZ (Ixz, Iyz, Izz)); } // Last modified by OFV 5.2001: // 1). surface and edge integration order is equal now // 2). "by point" works now rathre correctly (it looks so...) static void Compute(Face& S, Domain& D, const Standard_Boolean ByPoint, const Standard_Real Coeff[], const gp_Pnt& Loc, Standard_Real& Volu, gp_Pnt& G, gp_Mat& Inertia) { Standard_Real x, y, z, xi, yi, zi, l1, l2, lm, lr, l, v1, v2, v, u1, u2, um, ur, u, ds, Dul, xloc, yloc, zloc; Standard_Real LocVolu, LocIx, LocIy, LocIz, LocIxx, LocIyy, LocIzz, LocIxy, LocIxz, LocIyz; Standard_Real CVolu, CIx, CIy, CIz, CIxx, CIyy, CIzz, CIxy, CIxz, CIyz, Ix, Iy, Iz, Ixx, Iyy, Izz, Ixy, Ixz, Iyz; Standard_Real xn, yn, zn, px, py, pz, s, d1, d2, d3, dSigma; Standard_Integer i, j, vio, sio, max, NbGaussgp_Pnts; gp_Pnt Ps; gp_Vec VNor; gp_Pnt2d Puv; gp_Vec2d Vuv; Loc.Coord (xloc, yloc, zloc); Volu = Ix = Iy = Iz = Ixx = Iyy = Izz = Ixy = Ixz = Iyz = 0.0; S.Bounds (u1, u2, v1, v2); Standard_Real _u2 = u2; //OCC104 vio = S.VIntegrationOrder (); while (D.More()) { S.Load(D.Value()); sio = S.IntegrationOrder (); max = Max(vio,sio); NbGaussgp_Pnts = Min(max,math::GaussPointsMax()); math_Vector GaussP (1, NbGaussgp_Pnts); math_Vector GaussW (1, NbGaussgp_Pnts); math::GaussPoints (NbGaussgp_Pnts,GaussP); math::GaussWeights (NbGaussgp_Pnts,GaussW); CVolu = CIx = CIy = CIz = CIxx = CIyy = CIzz = CIxy = CIxz = CIyz = 0.0; l1 = S.FirstParameter(); l2 = S.LastParameter(); lm = 0.5 * (l2 + l1); lr = 0.5 * (l2 - l1); for (i=1; i<=NbGaussgp_Pnts; i++) { l = lm + lr * GaussP(i); S.D12d (l, Puv, Vuv); v = Puv.Y(); u2 = Puv.X(); //OCC104 v = v < v1? v1: v; v = v > v2? v2: v; u2 = u2 < u1? u1: u2; u2 = u2 > _u2? _u2: u2; Dul = Vuv.Y() * GaussW(i); um = 0.5 * (u2 + u1); ur = 0.5 * (u2 - u1); LocVolu = LocIx = LocIy = LocIz = LocIxx = LocIyy = LocIzz = LocIxy = LocIxz = LocIyz = 0.0; for (j=1; j<=NbGaussgp_Pnts; j++) { u = um + ur * GaussP(j); S.Normal (u, v, Ps, VNor); VNor.Coord (xn, yn, zn); Ps.Coord (x, y, z); x -= xloc; y -= yloc; z -= zloc; xn = xn * Dul * GaussW(j); yn = yn * Dul * GaussW(j); zn = zn * Dul * GaussW(j); if(ByPoint) { dSigma = (x*xn+y*yn+z*zn)/3.0; LocVolu += dSigma; LocIx += 0.75*x*dSigma; LocIy += 0.75*y*dSigma; LocIz += 0.75*z*dSigma; x -= Coeff[0]; y -= Coeff[1]; z -= Coeff[2]; dSigma *= 3.0/5.0; LocIxy -= x*y*dSigma; LocIyz -= y*z*dSigma; LocIxz -= x*z*dSigma; xi = x*x; yi = y*y; zi = z*z; LocIxx += (yi + zi)*dSigma; LocIyy += (xi + zi)*dSigma; LocIzz += (xi + yi)*dSigma; } else { s = xn * Coeff[0] + yn * Coeff[1] + zn * Coeff[2]; d1 = Coeff[0] * x + Coeff[1] * y + Coeff[2] * z; d2 = d1 * d1; d3 = d1 * d2 / 3.0; ds = s * d1; LocVolu += ds; LocIx += (x - Coeff[0] * d1 / 2.0) * ds; LocIy += (y - Coeff[1] * d1 / 2.0) * ds; LocIz += (z - Coeff[2] * d1 / 2.0) * ds; px = x - Coeff[0] * d1; py = y - Coeff[1] * d1; pz = z - Coeff[2] * d1; xi = (px * px * d1) + (px * Coeff[0]* d2) + (Coeff[0] * Coeff[0] * d3); yi = (py * py * d1) + (py * Coeff[1] * d2) + (Coeff[1] * Coeff[1] * d3); zi = pz * pz * d1 + pz * Coeff[2] * d2 + (Coeff[2] * Coeff[2] * d3); LocIxx += (yi + zi) * s; LocIyy += (xi + zi) * s; LocIzz += (xi + yi) * s; d2 /= 2.0; xi = (py * pz * d1) + (py * Coeff[2] * d2) + (pz * Coeff[1] * d2) + (Coeff[1] * Coeff[2] * d3); yi = (px * pz * d1) + (pz * Coeff[0] * d2) + (px * Coeff[2] * d2) + (Coeff[0] * Coeff[2] * d3); zi = (px * py * d1) + (px * Coeff[1] * d2) + (py * Coeff[0] * d2) + (Coeff[0] * Coeff[1] * d3); LocIxy -= zi * s; LocIyz -= xi * s; LocIxz -= yi * s; } } CVolu += LocVolu * ur; CIx += LocIx * ur; CIy += LocIy * ur; CIz += LocIz * ur; CIxx += LocIxx * ur; CIyy += LocIyy * ur; CIzz += LocIzz * ur; CIxy += LocIxy * ur; CIxz += LocIxz * ur; CIyz += LocIyz * ur; } Volu += CVolu * lr; Ix += CIx * lr; Iy += CIy * lr; Iz += CIz * lr; Ixx += CIxx * lr; Iyy += CIyy * lr; Izz += CIzz * lr; Ixy += CIxy * lr; Ixz += CIxz * lr; Iyz += CIyz * lr; D.Next(); } if(Abs(Volu) >= EPS_DIM) { if(ByPoint) { Ix = Coeff[0] + Ix/Volu; Iy = Coeff[1] + Iy/Volu; Iz = Coeff[2] + Iz/Volu; } else { Ix /= Volu; Iy /= Volu; Iz /= Volu; } G.SetCoord (Ix, Iy, Iz); } else { Volu =0.; G.SetCoord(0.,0.,0.); } Inertia.SetCols (gp_XYZ (Ixx, Ixy, Ixz), gp_XYZ (Ixy, Iyy, Iyz), gp_XYZ (Ixz, Iyz, Izz)); } GProp_VGProps::GProp_VGProps(){} GProp_VGProps::GProp_VGProps(Face& S, const gp_Pnt& VLocation, const Standard_Real Eps){ SetLocation(VLocation); Perform(S,Eps); } GProp_VGProps::GProp_VGProps(Face& S, Domain& D, const gp_Pnt& VLocation, const Standard_Real Eps){ SetLocation(VLocation); Perform(S,D,Eps); } GProp_VGProps::GProp_VGProps(Face& S, Domain& D, const gp_Pnt& VLocation){ SetLocation(VLocation); Perform(S,D); } GProp_VGProps::GProp_VGProps(const Face& S, const gp_Pnt& VLocation){ SetLocation(VLocation); Perform(S); } GProp_VGProps::GProp_VGProps(Face& S, const gp_Pnt& O, const gp_Pnt& VLocation, const Standard_Real Eps){ SetLocation(VLocation); Perform(S,O,Eps); } GProp_VGProps::GProp_VGProps(Face& S, Domain& D, const gp_Pnt& O, const gp_Pnt& VLocation, const Standard_Real Eps){ SetLocation(VLocation); Perform(S,D,O,Eps); } GProp_VGProps::GProp_VGProps(const Face& S, const gp_Pnt& O, const gp_Pnt& VLocation){ SetLocation(VLocation); Perform(S,O); } GProp_VGProps::GProp_VGProps(Face& S, Domain& D, const gp_Pnt& O, const gp_Pnt& VLocation){ SetLocation(VLocation); Perform(S,D,O); } GProp_VGProps::GProp_VGProps(Face& S, const gp_Pln& Pl, const gp_Pnt& VLocation, const Standard_Real Eps){ SetLocation(VLocation); Perform(S,Pl,Eps); } GProp_VGProps::GProp_VGProps(Face& S, Domain& D, const gp_Pln& Pl, const gp_Pnt& VLocation, const Standard_Real Eps){ SetLocation(VLocation); Perform(S,D,Pl,Eps); } GProp_VGProps::GProp_VGProps(const Face& S, const gp_Pln& Pl, const gp_Pnt& VLocation){ SetLocation(VLocation); Perform(S,Pl); } GProp_VGProps::GProp_VGProps(Face& S, Domain& D, const gp_Pln& Pl, const gp_Pnt& VLocation){ SetLocation(VLocation); Perform(S,D,Pl); } void GProp_VGProps::SetLocation(const gp_Pnt& VLocation){ loc = VLocation; } Standard_Real GProp_VGProps::Perform(Face& S, const Standard_Real Eps){ Standard_Real Coeff[] = {0., 0., 0.}; return myEpsilon = Compute(S,Standard_True,Coeff,loc,dim,g,inertia,Eps); } Standard_Real GProp_VGProps::Perform(Face& S, Domain& D, const Standard_Real Eps){ Standard_Real Coeff[] = {0., 0., 0.}; return myEpsilon = Compute(S,D,Standard_True,Coeff,loc,dim,g,inertia,Eps); } void GProp_VGProps::Perform(const Face& S){ Standard_Real Coeff[] = {0., 0., 0.}; Compute(S,Standard_True,Coeff,loc,dim,g,inertia); myEpsilon = 1.0; return; } void GProp_VGProps::Perform(Face& S, Domain& D){ Standard_Real Coeff[] = {0., 0., 0.}; Compute(S,D,Standard_True,Coeff,loc,dim,g,inertia); myEpsilon = 1.0; return; } Standard_Real GProp_VGProps::Perform(Face& S, const gp_Pnt& O, const Standard_Real Eps){ Standard_Real xloc, yloc, zloc; loc.Coord(xloc, yloc, zloc); Standard_Real Coeff[3]; O.Coord (Coeff[0], Coeff[1], Coeff[2]); Coeff[0] -= xloc; Coeff[1] -= yloc; Coeff[2] -= zloc; return myEpsilon = Compute(S,Standard_True,Coeff,loc,dim,g,inertia,Eps); } Standard_Real GProp_VGProps::Perform(Face& S, Domain& D, const gp_Pnt& O, const Standard_Real Eps){ Standard_Real xloc, yloc, zloc; loc.Coord(xloc, yloc, zloc); Standard_Real Coeff[3]; O.Coord (Coeff[0], Coeff[1], Coeff[2]); Coeff[0] -= xloc; Coeff[1] -= yloc; Coeff[2] -= zloc; return myEpsilon = Compute(S,D,Standard_True,Coeff,loc,dim,g,inertia,Eps); } void GProp_VGProps::Perform(const Face& S, const gp_Pnt& O){ Standard_Real xloc, yloc, zloc; loc.Coord(xloc, yloc, zloc); Standard_Real Coeff[3]; O.Coord (Coeff[0], Coeff[1], Coeff[2]); Coeff[0] -= xloc; Coeff[1] -= yloc; Coeff[2] -= zloc; Compute(S,Standard_True,Coeff,loc,dim,g,inertia); myEpsilon = 1.0; return; } void GProp_VGProps::Perform(Face& S, Domain& D, const gp_Pnt& O){ Standard_Real xloc, yloc, zloc; loc.Coord(xloc, yloc, zloc); Standard_Real Coeff[3]; O.Coord (Coeff[0], Coeff[1], Coeff[2]); Coeff[0] -= xloc; Coeff[1] -= yloc; Coeff[2] -= zloc; Compute(S,D,Standard_True,Coeff,loc,dim,g,inertia); myEpsilon = 1.0; return; } Standard_Real GProp_VGProps::Perform(Face& S, const gp_Pln& Pl, const Standard_Real Eps){ Standard_Real xloc, yloc, zloc; loc.Coord (xloc, yloc, zloc); Standard_Real Coeff[4]; Pl.Coefficients (Coeff[0], Coeff[1],Coeff[2],Coeff[3]); Coeff[3] = Coeff[3] - Coeff[0]*xloc - Coeff[1]*yloc - Coeff[2]*zloc; return myEpsilon = Compute(S,Standard_False,Coeff,loc,dim,g,inertia,Eps); } Standard_Real GProp_VGProps::Perform(Face& S, Domain& D, const gp_Pln& Pl, const Standard_Real Eps){ Standard_Real xloc, yloc, zloc; loc.Coord (xloc, yloc, zloc); Standard_Real Coeff[4]; Pl.Coefficients (Coeff[0], Coeff[1],Coeff[2],Coeff[3]); Coeff[3] = Coeff[3] - Coeff[0]*xloc - Coeff[1]*yloc - Coeff[2]*zloc; return myEpsilon = Compute(S,D,Standard_False,Coeff,loc,dim,g,inertia,Eps); } void GProp_VGProps::Perform(const Face& S, const gp_Pln& Pl){ Standard_Real xloc, yloc, zloc; loc.Coord (xloc, yloc, zloc); Standard_Real Coeff[4]; Pl.Coefficients (Coeff[0], Coeff[1],Coeff[2],Coeff[3]); Coeff[3] = Coeff[3] - Coeff[0]*xloc - Coeff[1]*yloc - Coeff[2]*zloc; Compute(S,Standard_False,Coeff,loc,dim,g,inertia); myEpsilon = 1.0; return; } void GProp_VGProps::Perform(Face& S, Domain& D, const gp_Pln& Pl){ Standard_Real xloc, yloc, zloc; loc.Coord (xloc, yloc, zloc); Standard_Real Coeff[4]; Pl.Coefficients (Coeff[0], Coeff[1],Coeff[2],Coeff[3]); Coeff[3] = Coeff[3] - Coeff[0]*xloc - Coeff[1]*yloc - Coeff[2]*zloc; Compute(S,D,Standard_False,Coeff,loc,dim,g,inertia); myEpsilon = 1.0; return; } Standard_Real GProp_VGProps::GetEpsilon(){ return myEpsilon; }