diff --git a/CurveAux.cpp b/CurveAux.cpp index e9ff464..465a118 100644 --- a/CurveAux.cpp +++ b/CurveAux.cpp @@ -31,7 +31,7 @@ using namespace std ; static bool FindSpan( double dU, int nDeg, const DBLVECTOR& vKnots, int& nSpan) ; -static bool CalcBasisFunc( double dU, int nDeg, const DBLVECTOR& vKnots, DBLVECTOR& vBasis) ; +static bool CalcBasisFunc( double dU, int nSpan, int nDeg, const DBLVECTOR& vKnots, DBLVECTOR& vBasis) ; //---------------------------------------------------------------------------- bool @@ -1143,13 +1143,13 @@ InterpolatePointSetWithBezier( const PNTVECTOR& vPnt, double dTol) // pag 364 del Piegl // scelgo il parametro associato ad ogni punto in modo che rispecchi la distanza tra i punti - int nPoints = vPnt.size() ; + int nPoints = int( vPnt.size()) ; if ( nPoints < 4) return nullptr ; int nDeg = 3 ; DBLVECTOR vLen ; double dLenTot = 0 ; - for ( int i = 0 ; i < int( nPoints -1) ; ++i) { + for ( int i = 0 ; i < nPoints - 1 ; ++i) { double dLen = Dist(vPnt[i], vPnt[i+1]) ; vLen.push_back( dLen) ; dLenTot += dLen ; @@ -1158,7 +1158,7 @@ InterpolatePointSetWithBezier( const PNTVECTOR& vPnt, double dTol) vPntParam.resize( nPoints) ; vPntParam[0] = 0 ; vPntParam.back() = 1 ; - for ( int i = 1 ; i < int( nPoints - 1) ; ++i) + for ( int i = 1 ; i < nPoints - 1 ; ++i) vPntParam[i] = vPntParam[i-1] + vLen[i-1] / dLenTot ; DBLVECTOR vKnots ; @@ -1170,7 +1170,7 @@ InterpolatePointSetWithBezier( const PNTVECTOR& vPnt, double dTol) for ( int i = nDeg ; i < nPoints - 1 ; ++i) { double dKnot = 0 ; - for ( int j = i ; j < i + nDeg ; ++j) + for ( int j = i + 1 ; j < i + nDeg + 1 ; ++j) dKnot += vPntParam[j - nDeg] ; dKnot /= nDeg ; vKnots[i] = dKnot ; @@ -1179,21 +1179,26 @@ InterpolatePointSetWithBezier( const PNTVECTOR& vPnt, double dTol) Eigen::MatrixXd mA( nPoints, nPoints) ; mA.fill( 0) ; for ( int i = 0 ; i < nPoints ; ++i) { - int nSpan = 0 ; FindSpan(vPntParam[i], nDeg, vKnots, nSpan) ; if ( i == 0) mA.row(0).col(0) << 1 ; else if ( i == nPoints - 1) - mA.row(i).col(nPoints - 1) << 1 ; + mA.row(i).col(nPoints - 1) << 1 ; else { + int nSpan = 0 ; FindSpan(vPntParam[i], nDeg, vKnots, nSpan) ; DBLVECTOR vBasis ; vBasis.resize( nDeg + 1) ; - CalcBasisFunc( vPntParam[i], nDeg, vKnots, vBasis) ; - for( int j = nSpan - nDeg ; j < nSpan ; ++j) - mA.row(i).col(j) << vBasis[j - nSpan + nDeg] ; + CalcBasisFunc( vPntParam[i], nSpan, nDeg, vKnots, vBasis) ; + for( int j = nSpan - nDeg + 1 ; j <= nSpan + 1 ; ++j) + mA.row(i).col(j) << vBasis[j - nSpan + nDeg - 1] ; } } int nDim = 3 ; Eigen::MatrixXd mb ; mb.resize( nPoints, nDim) ; + for ( int i = 0 ; i < int( vPnt.size()) ; ++i) { + mb.row(i).col(0) << vPnt[i].x ; + mb.row(i).col(1) << vPnt[i].y ; + mb.row(i).col(2) << vPnt[i].z ; + } Eigen::MatrixXd mX = mA.fullPivLu().solve(mb) ; PNTVECTOR vPntCtrl ; @@ -1207,7 +1212,10 @@ InterpolatePointSetWithBezier( const PNTVECTOR& vPnt, double dTol) pCrvInt.Set( NurbsToBezierCurve( cNurbs)) ; - return Release( pCrvInt) ; + if( ! IsNull(pCrvInt) && pCrvInt->IsValid()) + return Release( pCrvInt) ; + else + return nullptr ; } //---------------------------------------------------------------------------- @@ -1217,7 +1225,7 @@ FindSpan( double dU, int nDeg, const DBLVECTOR& vKnots, int& nSpan) if ( dU < 0) return false ; else if ( dU < EPS_ZERO) { - nSpan = nDeg ; + nSpan = nDeg - 1 ; return true ; } // trovo a quale span appartiene il parametro dU @@ -1226,16 +1234,16 @@ FindSpan( double dU, int nDeg, const DBLVECTOR& vKnots, int& nSpan) nSpan = nKnots - 1 ; return true ; } - int nLow = nDeg ; - int nHigh = nKnots ; + int nLow = nDeg - 1 ; + int nHigh = nKnots - 1 ; int nMid = ( nLow + nHigh) / 2 ; - while ( dU < vKnots[nMid] || dU > vKnots[nMid + 1] ) { + while ( dU < vKnots[nMid] || dU >= vKnots[nMid + 1] ) { if ( dU < vKnots[nMid]) nHigh = nMid ; else nLow = nMid ; nMid = ( nLow + nHigh) / 2 ; - if( nMid == nDeg) + if( nMid == nDeg - 1) break ; } nSpan = nMid ; @@ -1244,15 +1252,12 @@ FindSpan( double dU, int nDeg, const DBLVECTOR& vKnots, int& nSpan) //---------------------------------------------------------------------------- static bool -CalcBasisFunc( double dU, int nDeg, const DBLVECTOR& vKnots, DBLVECTOR& vBasis) +CalcBasisFunc( double dU, int nSpan, int nDeg, const DBLVECTOR& vKnots, DBLVECTOR& vBasis) { // mi aspetto che il vettore vBasis sia di lunghezza nDeg + 1 if ( vBasis.size() != nDeg + 1) return false ; - int nSpan = 0 ; - FindSpan( dU, nDeg, vKnots, nSpan) ; - vBasis[0] = 1 ; DBLVECTOR vLeft ; vLeft.resize( nDeg + 1) ; DBLVECTOR vRight ; vRight.resize( nDeg + 1) ; @@ -1261,7 +1266,10 @@ CalcBasisFunc( double dU, int nDeg, const DBLVECTOR& vKnots, DBLVECTOR& vBasis) vRight[j] = vKnots[nSpan + j] - dU ; double dSaved = 0 ; for ( int r = 0 ; r < j ; ++r) { - double dTemp = vBasis[r] / ( vRight[r+1] + vLeft[j-r]) ; + double dSum = vRight[r+1] + vLeft[j-r] ; + double dTemp = 0 ; + if( dSum > EPS_SMALL) + dTemp = vBasis[r] / dSum ; vBasis[r] = dSaved + vRight[r+1] * dTemp ; dSaved = vLeft[j-r] * dTemp ; }