cinosnurbscurve.h

Go to the documentation of this file.

//******************************************************************************
//******************************************************************************
#ifndef INC_CINOSNURBSCURVE_H
#define INC_CINOSNURBSCURVE_H
//------------------------------------------------------------------------------
// defines
//------------------------------------------------------------------------------
//
#define DF_INOS_NURBSCURVE_UNDIFINED_SEG_INDEX 0xFFFFFFFF
 
// --- general -----------------------------------------------------------------
//
typedef double _vector[DF_INOS_NURBSCURVE_MAX_DEGREE+1];
typedef double _matrix[DF_INOS_NURBSCURVE_MAX_DEGREE+1][DF_INOS_NURBSCURVE_MAX_DEGREE+1];
//
//------------------------------------------------------------------------------
// includes
//------------------------------------------------------------------------------
//
// system
#include <inos.h>
#include <inosstdtypes.h>
#include <cinosnurbspoint.h>
#include <cinosspline.h>
 
//
// C++
//
// project
//
//------------------------------------------------------------------------------
// class definition
//------------------------------------------------------------------------------
//
// Binomial coefficient up to 5
static constexpr double BinCoeff[6 * 6] =  {
    1.0, 0.0,  0.0, 0.0, 0.0, 0.0,
    1.0, 1.0,  0.0, 0.0, 0.0, 0.0,
    1.0, 2.0,  1.0, 0.0, 0.0, 0.0,
    1.0, 3.0,  3.0, 1.0, 0.0, 0.0,
    1.0, 4.0,  6.0, 5.0, 1.0, 0.0,
    1.0, 5.0, 10.0, 10.0, 5.0, 1.0
};
 
template <int N> class CINOSNurbsCurve
{
    //--- user interface ---------------------------------------------------
 
    // public member functions
    public :
 
        void AddPoint(CINOSNurbsPoint<N>& aPoint)
        {
            // check point weight
            if (aPoint.w != 0.0) {
                // remove all zero flag
                m_uFlags &= ~eFlgZeroWeight;
                // custom weight ?
                if (aPoint.w != 1.0) {
                    // yes
                    m_uFlags |= eFlgCustomWeight;
                } // end if
            }
            // remove curve valid flag
            m_uFlags &= ~eFlgCurveValid;
            // add point to vector
            P.push_back(aPoint);
        }
 
        void AddPoint(TINOSVector<N>& aPoint)
        {
            CINOSNurbsPoint<N> p;
            // copy coordinates
            for (uint32 i=0; i<N; i++) {
                p[i] = aPoint[i];
            }
            // finally add it
            AddPoint(p);
        }
 
        void AddKnot(double adKnot, uint32 auMultiplicity)
        {
            // add knot values
            for (uint32 i=0; i<auMultiplicity; i++){
                // add knot
                U.push_back(adKnot);
            }
            // remove curve valid flag
            m_uFlags &= ~eFlgCurveValid;
        }
 
        void SetPoints(uint32 auPoints)
        {
            // resize vector
            P.reserve(auPoints);
        }
 
        uint32 GetPoints()
        {
            // return vector size
            return (uint32)P.size();
        }
 
        void SetInterpolationPoints(uint32 auInterpolationPoints)
        {
            // Ensure we have at least 2 points (start and end).
            m_uInterpolationPoints = auInterpolationPoints < 2 ? 2 : auInterpolationPoints;
        }
 
        uint32 Check()
        {
            // remove curve valid flag
            m_uFlags &= ~eFlgCurveValid;
 
            // check degree
            if ((D < 1) || (D > DF_INOS_NURBSCURVE_MAX_DEGREE)) {
                // invalid degree selected
                return INOS_MOVEPATH_ERROR_NURBS_DEGREE_INVALID;
            } // end if
 
            // we need at least degree+1 control points
            if (P.size() < (D+1)) {
                // not enough control points, delevate degree if possible
                if (P.size() > 1) {
                    // adjust degree
                    D = (int)(P.size() - 1);
                } // end if
                else {
                    // not possible
                    return INOS_MOVEPATH_ERROR_NURBS_NOT_ENOUGH_POINTS;
                } // end else
            } // end if
 
            // at least one control point weith needs to be != 0
            if (m_uFlags & eFlgZeroWeight) {
                // all control point weights are zero
                return INOS_MOVEPATH_ERROR_NURBS_ALL_WEIGHTS_ZERO;
            } // end if
 
            // knots defined ?
            if (0 == U.size()) {
                // no -> assume uniform, clamped nodes
                // first node : value = 0, multiplicity = degree+1 (e.g. 0,0,0,0)
                // mid nodes  : value = 1/(points-degree)
                // last  node : value = 1, multiplicity = degree+1 (e.g. 1,1,1,1)
                AddKnot(0.0, D+1);
                double dk = 1.0/double(P.size()-D);
                for (uint32 i=1; i<(P.size()-D); i++) {
                    AddKnot(double(i) * dk, 1);
                } // end for
                AddKnot(1.0, D+1);
            }
 
            // we need points+degree+1 knots
            if (U.size() != (P.size()+D+1)) {
                // wrong number of knots
                return INOS_MOVEPATH_ERROR_NURBS_NUMBER_OF_KNOTS_INVALID;
            } // end if
 
            // curve is flag
            m_uFlags |= eFlgCurveValid;
 
            // ok
            return INOS_OK;
        }
 
        static constexpr double d43 = 4.0/3.0;
        static constexpr double d13 = 1.0/3.0;
 
        double GetLength()
        {
            return m_dLength;
        }
 
        uint32 Prepare()
        {
            // Curve Vaild
            if((m_uFlags & eFlgCurveValid) == 0) {
                // Not valid? Check again.
                uint32 uError = Check();
                if(uError) {
                    return uError;
                }
            }
 
            // Reset position UofP cache channel for Run.
            m_sPreviousCache[0].m_uPSpan = 0;
            m_sPreviousCache[0].m_uSplineIndex = 1;
            m_sPreviousCache[0].m_uUSpan = D;
            m_sPreviousCache[0].m_uSegment = 0;
            m_sPreviousCache[0].m_dDistanceSegBeg = -1.0;
            m_sPreviousCache[0].m_dDistanceSegEnd = -1.0;
            m_sPreviousCache[0].m_dP = 0.0;
 
            // Already prepared?
            if(m_uFlags & eFlgPrepared) {
                // Already prepared
                return 0;
            }
 
            // Number of points
            uint32 uNoP = (uint32)P.size();
 
            // Splines that interpolate P -> U function
            m_UofPs.resize(uNoP - 1, nullptr);
 
 
            // Check for only 2 points and no length
            if(uNoP == 2 && P[0] == P[1]) {
                // No length
                m_dLength = 0.0;
 
                // Make dummy spline
                m_UofPs[0] = new CINOSSpline(2);
 
                // Get shortcut
                auto Spline = m_UofPs[0];
 
                // Set first U of P point
                Spline->SetPoint(0, 0.0, 0.0);
                // Set second/last U of P point
                Spline->SetPoint(1, 1.0, 0.0);
 
                Spline->Calc(false);
                // curve is prepared
                m_uFlags |= eFlgPrepared;
                // return result
                return 0;
            }
 
            // Three points needed for length calculation
            TINOSVector<N> point1;
            TINOSVector<N> point2;
            TINOSVector<N> point3;
 
            // Temporary lengths
            double l1 = 0.0;
            double l2 = 0.0;
            double ldiff = 0.0;
 
            // Total length
            double L = 0.0;
 
            // Current u value
            double u = 0.0;
            // U value of current segment begining
            double uSeg = 0.0;
 
            // Delta values
            const double dUSeg = U.back() / double(uNoP - 1);
            const double du = dUSeg / double(m_uInterpolationPoints-1);
            const double du_2 =du * 0.5;
            const double du_edge =du * 0.0001;
 
            // Get first point
            GetPositionU(0.0, point1);
            for(uint32 uIxP = 0; uIxP < uNoP - 1; uIxP++) {
                // Create new spline
                m_UofPs[uIxP] = new CINOSSpline(m_uInterpolationPoints);
 
                // Get shortcut
                auto Spline = m_UofPs[uIxP];
 
                // Set first U of P point
                Spline->SetPoint(0, L, u);
 
                // Recalculate uSeg and u to minimize numerical errors.
                uSeg = u = dUSeg * (double)uIxP;
 
                // get second point for edge 1st derivative calculation
                GetPositionU(u+du_edge, point2);
                ldiff =  (point2 - point1).GetMaskedLength(m_uLengthMask) ;
 
                // Check length non-zero
                if(ldiff == 0.0) {
                    // length is zero -> remove curve valid flag
                    m_uFlags &= ~eFlgCurveValid;
                    // return error
                    return INOS_MOVEPATH_ERROR_NURBS_RELEVANT_LENGTH_ZERO;
                }
                // 1st derivative at begining of the spline
                double vStart = du_edge / ldiff;
 
                // calc length of the current points
                // the algorithm can be found here :
                // https://math.stackexchange.com/questions/777830/simpsons-rule-like-method-for-arc-length-approximation
                // this version is doing the calculation iteratively
                for (uint32 uIx =1; uIx <m_uInterpolationPoints; uIx ++){
                    // Get two more points for length calculation
                    GetPositionU(u+du_2, point2);
                    GetPositionU(u+du, point3);
 
                    // Calculate the lengths
                    l1 = (point2 - point1).GetMaskedLength(m_uLengthMask)
                            + (point3 - point2).GetMaskedLength(m_uLengthMask);
                    l2 = (point3 - point1).GetMaskedLength(m_uLengthMask);
 
                    //  Calculate adjusted length
                    ldiff = d43 * l1 - d13 * l2;
 
                    // Check lengths non-zero
                    if(ldiff == 0.0) {
                        // length is zero -> remove curve valid flag
                        m_uFlags &= ~eFlgCurveValid;
                        // return error
                        return INOS_MOVEPATH_ERROR_NURBS_RELEVANT_LENGTH_ZERO;
                    }
 
                    // Add adjusted length
                    L = L + ldiff;
 
                    // Calculate the new u
                    u = uSeg + uIx * du;
 
                    // Move the last point to the new first point
                    point1 = point3;
 
                    // Set the new U of P point
                    Spline->SetPoint(uIx, L, u);
                }
 
                // Get a point just before the last point to calculate edge 1st derivative
                GetPositionU(u-du_edge, point2);
                l1  = (point3 - point2).GetMaskedLength(m_uLengthMask) ;
 
                // Check length non-zero
                if(l1 == 0.0) {
                    // length is zero -> remove curve valid flag
                    m_uFlags &= ~eFlgCurveValid;
                    // return error
                    return INOS_MOVEPATH_ERROR_NURBS_RELEVANT_LENGTH_ZERO;
                }
 
                // Calculate the 1st derivative
                double vSEnd = du_edge / l1;
 
                // Finally calculate the spline for this segment of the nurbs.
                Spline->Calc(true, CINOSSpline::eBt1stDeriv, vStart,
                        CINOSSpline::eBt1stDeriv, vSEnd, false);
            }
 
            // Store the calculated length
            m_dLength = L;
 
            // curve is prepared
            m_uFlags |= eFlgPrepared;
            // return result
            return 0;
        }
 
        virtual void GetPosition(double adP, TINOSVector<N>& ovPos)
        {
            // project P to U (use channel 0)
            double u = GetLocalU(adP, 0);
            // calc position at U
            GetLocalPositionU(u, ovPos);
        }
 
        virtual void GetPosition(double adP, TINOSVector<N>& ovPos, uint32& auSegment)
        {
            // project P to U (use channel 0)
            double u = GetLocalU(adP, 0);
            // calc position at U
            GetPositionU(u, ovPos);
            // get actual segment (use channel 0)
            auSegment = GetSegment(0, adP, ovPos);
        }
 
        virtual void GetPositionFirst(TINOSVector<N>& ovPos)
        {
            // project P to u
            GetPositionU(U.front(), ovPos);
        }
 
        virtual void GetPositionLast(TINOSVector<N>& ovPos)
        {
            // project P to u
            GetPositionU(U.back(), ovPos);
        }
 
        void GetLocalPositionU(double adU, TINOSVector<N>& ovPos)
        {
            if(adU<=U[D]) {
                m_sPreviousCache[0].m_uUSpan = D;
            }
            else if(adU>=U[P.size()]) {
                m_sPreviousCache[0].m_uUSpan = (uint32)(P.size()-1);
            }
            else if(m_sPreviousCache[0].m_uPSpan == DF_INOS_NURBSCURVE_UNDIFINED_SEG_INDEX) {
                m_sPreviousCache[0].m_uUSpan = FindSpan(adU);
            }
            else if(adU >= U[m_sPreviousCache[0].m_uUSpan + 1]) {
                do {
                    m_sPreviousCache[0].m_uUSpan++;
                }
                while(adU >= U[m_sPreviousCache[0].m_uUSpan + 1]);
            }
            else if(adU < U[m_sPreviousCache[0].m_uUSpan]) {
                do {
                    m_sPreviousCache[0].m_uUSpan--;
                }
                while(adU < U[m_sPreviousCache[0].m_uUSpan]);
            }
 
            GetPositionU(adU, ovPos, m_sPreviousCache[0].m_uUSpan);
        }
 
        inline void GetPositionU(double adU, TINOSVector<N>& ovPos)
        {
            GetPositionU(adU, ovPos, FindSpan(adU));
        }
 
        virtual void GetPositionU(double adU, TINOSVector<N>& ovPos, uint32 auUSpan)
        {
            // locals
            _vector Nb;
            BasisFuns(adU, auUSpan, Nb);
            ovPos.Set(0.0);
            // custom weights ?
            if (!(m_uFlags & eFlgCustomWeight)) {
                // no
                for(uint32 i=0;i<=D;i++) {
                    ovPos = ovPos + (Nb[i] * P[auUSpan-D+i]);
                } // end for
            } // end if
            else {
                // yes
                double w = 0.0;
                for(uint32 i=0;i<=D;i++) {
                    uint32 ii = auUSpan-D+i;
                    double ww = Nb[i] * P[ii].w;
                    w += ww;
                    ovPos = ovPos + (ww * P[ii]);
                }
                if(w != 0.0) {
                    ovPos = ovPos * (1.0/w);
                }
            } // end else
        }
 
        virtual void GetDerivative(uint32 auLevel, double adP, TINOSVector<N>& ovDer)
        {
            // project P to U (use channel auLevel)
            double u = GetLocalU(adP, auLevel);
            // calc derivative at U
            GetDerivativeU(auLevel, u, ovDer);
        }
 
        virtual void GetDerivativeBgn(uint32 auLevel, TINOSVector<N>& ovDer)
        {
            // call base
            GetDerivativeU(auLevel, U.front(), ovDer);
        }
 
        virtual void GetDerivativeEnd(uint32 auLevel, TINOSVector<N>& ovDer)
        {
            // call base
            GetDerivativeU(auLevel, U.back(), ovDer);
        }
 
 
        typedef TINOSVector<N> _array_vector[DF_INOS_NURBSCURVE_MAX_DEGREE+1];
        virtual void GetDerivativeU(uint32 auLevel, double adU, TINOSVector<N>& ovDer)
        {
            // ensure correct parameter
            if (adU > U.back()) adU = U.back();
            else if (adU < U.front()) adU = U.front();
 
            // calc derivatives
            _array_vector ders;
            CurveDerivs(adU, auLevel, ders);
            // check level
            if (auLevel == 1) {
                // norm to direction vector
                double l = ders[1].GetMaskedLength(m_uLengthMask);
                if(l == 0.0) {
                    // no movement, derivative is zero
                    ovDer = 0.0;
                }
                else {
                    ovDer = 1.0/l * ders[1];
                }
            } // end if
            else if (auLevel == 2) {
                double dotProd = 0.0;
                double div = 0.0;
                for(int i = 0; i < N; i++) {
                    if (!(m_uLengthMask & (1<<i))){
                        dotProd += ders[1][i] * ders[2][i];
                        div += ders[1][i] * ders[1][i];
                    }
                }
                if(div == 0.0) {
                    // no movement, derivative is zero
                    ovDer = 0.0;
                    return;
                }
                dotProd /= div;
                for(int i = 0; i < N; i++) {
                    if (!(m_uLengthMask & (1<<i))){
                        ovDer[i] = ders[2][i] - dotProd * ders[1][i];
                    }
                    else {
                        ovDer[i] = ders[2][i] ;
                    }
                }
 
                double l = ovDer.GetMaskedLength(m_uLengthMask);
 
                // norm to curvature vector
                div = ders[1].GetMaskedLength(m_uLengthMask);
 
                if(l > 0.0) {
                    div = div * div * div * l;
 
 
                    // Cross product calculation
                    double dNom;
                    uint8 (&ixs)[N] = m_uLengthDims;
 
                    if(m_uNoLengthDims == 2) {
                        dNom =  ders[1][ixs[0]] * ders[2][ixs[1]] - ders[1][ixs[1]] * ders[2][ixs[0]];
                    }
                    else if (m_uNoLengthDims == 3 && (N > 2)) { // (N > 2) only checked to please the compiler
                        TINOSVector<N> crossProd;
                        crossProd[0] =  ders[1][ixs[1]] * ders[2][ixs[2]] - ders[1][ixs[2]] * ders[2][ixs[1]];
                        crossProd[1] =ders[1][ixs[2]] * ders[2][ixs[0]] - ders[1][ixs[0]] * ders[2][ixs[2]];
                        crossProd[2] =  ders[1][ixs[0]] * ders[2][ixs[1]] - ders[1][ixs[1]] * ders[2][ixs[0]];
 
                        dNom = crossProd.GetLength();
                    }
                    else {
                        // not supported
                        ovDer = 0.0;
                        return;
                    }
                    ovDer =  (dNom / div) * ovDer;
 
                }
                else {
                    double acc = ders[2].GetMaskedLength(m_uLengthMask)
                            / (div * div * div);
                    double nom = 1 / (div * div);
                    for(int i = 0; i < N; i++) {
                        if (m_uLengthMask & (1<<i)){
                            ovDer[i] = nom * ovDer[i]
                            + acc * ders[1][i];
                        }
                    }
                }
            } // end if
            else {
                // not supported
                // for 3rd derivative use approximation in GetD3max
                ovDer = 0.0;
            } // end else
        }
 
        virtual void GetD2max(TINOSVector<N>& ovD2Max)
        {
            // locals
            double u = U.front();
            ovD2Max = 0.0;
            // search max. second
            double d = (U.back() - u)/double(m_uInterpolationPoints);
            while (u<U.back()){
                // calc D2
                TINOSVector<N> derivatives;
                GetDerivativeU(2, u, derivatives);
                for (uint32 i=0; i<N; i++){
                    // calc normalised D2
                    double dD2 = fabs(derivatives[i]);
                    // check max
                    if (dD2 > ovD2Max[i]){
                        // set new max
                        ovD2Max[i] = dD2;
                    } // end if
                } // end for
                u += d;
            } // end if
        }
 
        virtual void GetD3max(TINOSVector<N>& ovD3Max)
        {
            //handled in CINOSMovePathInterpolatorNurbs
            ovD3Max = 0;
        } // end GetD3max
 
        double GetGlobalU(double adP)
        {
            if(adP <= 0.0) {
                return 0.0;
            }
            if(adP >= m_dLength) {
                return U.back();
            }
            // locals
            uint32 i;
            uint32 l = 0;
            uint32 u = m_UofPs.size();
            // binary search
            do {
                i = (l+u)>>1;
                // p in upper part ?
                if (adP > m_UofPs[i]->GetMaxX()) {
                    l = i;
                }
                else if (adP < m_UofPs[i]->GetMinX()){
                    // no
                    u = i;
                }
                else {
                    break;
                }
            } while (l != u);
 
            uint32 uIndex = DF_INOS_SPLINE_BINARY_SEARCH_INDEX;
            return m_UofPs[i]->GetY(adP, uIndex);
        }
 
        double GetLocalU(double adP, uint32 auChannel)
        {
            if(adP <= 0.0) {
                m_sPreviousCache[auChannel].m_uPSpan = 0;
                m_sPreviousCache[auChannel].m_uSplineIndex = 1;
                return 0.0;
            }
            if(adP >= m_dLength) {
                m_sPreviousCache[auChannel].m_uPSpan = (uint32)(m_UofPs.size() - 1);
                m_sPreviousCache[auChannel].m_uSplineIndex = m_uInterpolationPoints - 1;
                return U.back();
            }
            if(m_sPreviousCache[auChannel].m_uPSpan == DF_INOS_NURBSCURVE_UNDIFINED_SEG_INDEX) {
                // locals
                uint32 i;
                uint32 l = 0;
                uint32 u = (uint32)m_UofPs.size();
                // binary search
                do {
                    i = (l+u)>>1;
                    // p in upper part ?
                    if (adP > m_UofPs[i]->GetMaxX()) {
                        l = i;
                    }
                    else if (adP < m_UofPs[i]->GetMinX()){
                        // no
                        u = i;
                    }
                    else {
                        break;
                    }
                } while (l != u);
                m_sPreviousCache[auChannel].m_uPSpan = i;
            }
            else if(adP >= m_UofPs[m_sPreviousCache[auChannel].m_uPSpan]->GetMaxX()) {
                do {
                    m_sPreviousCache[auChannel].m_uPSpan++;
                }
                while(adP >= m_UofPs[m_sPreviousCache[auChannel].m_uPSpan]->GetMaxX());
 
                m_sPreviousCache[auChannel].m_uSplineIndex = 1;
            }
            else if(adP <  m_UofPs[m_sPreviousCache[auChannel].m_uPSpan]->GetMinX()) {
                do {
                    m_sPreviousCache[auChannel].m_uPSpan--;
                }
                while(adP < m_UofPs[m_sPreviousCache[auChannel].m_uPSpan]->GetMinX());
 
                m_sPreviousCache[auChannel].m_uSplineIndex = m_uInterpolationPoints - 1;
            }
 
            return m_UofPs[m_sPreviousCache[auChannel].m_uPSpan]->
                    GetY(adP, m_sPreviousCache[auChannel].m_uSplineIndex);
        }
 
    //--- internals --------------------------------------------------------
 
    friend class CINOSMovePath;
 
    // constructor / destructor
    public :
        CINOSNurbsCurve (uint32 auDegree = DF_INOS_NURBSCURVE_MAX_DEGREE,
            uint32 auInterpolationPoints = DF_INOS_NURBSCURVE_INTERPOLATION_POINTS,
            uint32 auLengthMask = 0xFFFFFFF8)
            : D(auDegree), m_uLengthMask(auLengthMask),
              m_uInterpolationPoints(auInterpolationPoints < 2 ? 2 : auInterpolationPoints),
              m_UofPs(0)
 
        {
            m_uNoLengthDims= 0;
 
            for(uint32 uD = 0; uD<N; uD++) {
                if((m_uLengthMask&(1<<uD)) == 0) {
                    m_uLengthDims[m_uNoLengthDims] = uD;
                    m_uNoLengthDims++;
                }
            }
 
            // init calculation channels
            memset(&m_sPreviousCache, 0, sizeof(m_sPreviousCache));
            for (uint32 i=0; i<eCnsMaxChannels; i++) {
                m_sPreviousCache[i].m_uPSpan = DF_INOS_NURBSCURVE_UNDIFINED_SEG_INDEX;
                m_sPreviousCache[i].m_uSplineIndex = DF_INOS_SPLINE_BINARY_SEARCH_INDEX;
                m_sPreviousCache[i].m_uSegment = 0;
                m_sPreviousCache[i].m_dDistanceSegBeg = -1.0;
                m_sPreviousCache[i].m_dDistanceSegEnd = -1.0;
                m_sPreviousCache[i].m_dP = 0.0;
            }
        }
 
        virtual ~CINOSNurbsCurve ()
        {
            for(auto spline : m_UofPs) {
                if(spline != nullptr) {
                    delete spline;
                }
            }
        }
 
    // protected methods
    protected :
 
        uint32 FindSpan(double u) const
        {
            if(u>=U[P.size()])
                return (uint32)(P.size()-1) ;
            if(u<=U[D])
                return D ;
            int low  = D;
            int high = (int)(P.size()+1) ;
            int mid = (low+high)/2 ;
            while(u<U[mid] || u>= U[mid+1]){
                if(u<U[mid])
                  high = mid ;
                else
                  low = mid ;
                mid = (low+high)/2 ;
            }
            return mid ;
        }
 
        void BasisFuns(double u, uint32 i, _vector& Nb) const
        {
            double left[2*(DF_INOS_NURBSCURVE_MAX_DEGREE+1)];
            double* right = &left[D+1] ;
            double temp,saved ;
            Nb[0] = 1.0 ;
            for(uint32 j=1; j<= D ; j++){
                left[j] = u-U[i+1-j] ;
                right[j] = U[i+j]-u ;
                saved = 0.0 ;
                for(uint32 r=0 ; r<j; r++){
                    temp = Nb[r]/(right[r+1]+left[j-r]) ;
                    Nb[r] = saved+right[r+1] * temp ;
                    saved = left[j-r] * temp ;
                }
                Nb[j] = saved ;
            }
        }
 
        void DersBasisFuns(uint32 n, double u, uint32 span, _matrix& ders) const
        {
            double left[2*(DF_INOS_NURBSCURVE_MAX_DEGREE+1)] ;
            double* right = &left[D+1] ;
            _matrix ndu;
            double saved,temp ;
            int j,r ;
            ndu[0][0] = 1.0 ;
            for(int j=1; j<=(int)D ;j++){
                left[j] = u-U[span+1-j] ;
                right[j] = U[span+j]-u ;
                saved = 0.0 ;
                for(r=0;r<j ; r++){
                    // Lower triangle
                    ndu[j][r] = right[r+1]+left[j-r] ;
                    temp = ndu[r][j-1]/ndu[j][r] ;
                    // Upper triangle
                    ndu[r][j] = saved+right[r+1] * temp ;
                    saved = left[j-r] * temp ;
                }
                ndu[j][j] = saved ;
            }
 
            for(int j=(int)D;j>=0;--j)
                ders[0][j] = ndu[j][D];
 
            // Compute the derivatives
            _matrix a;
            for (int r=0;r<=(int)D;r++) {
                int s1,s2 ;
                s1 = 0 ; s2 = 1 ; // alternate rows in array a
                a[0][0] = 1.0 ;
                // Compute the kth derivative
                for (int k=1;k<=(int)n;k++) {
                    double d ;
                    int rk,pk,j1,j2 ;
                    d = 0.0 ;
                    rk = r-k ; pk = D-k ;
                    if (r>=k) {
                        a[s2][0] = a[s1][0]/ndu[pk+1][rk];
                        d = a[s2][0]*ndu[rk][pk] ;
                    }
                    if(rk>=-1){
                        j1 = 1 ;
                    }
                    else {
                        j1 = -rk ;
                    }
                    if(r-1 <= pk){
                        j2 = k-1 ;
                    }
                    else {
                        j2 = D-r ;
                    }
                    for (j=j1;j<=j2;j++){
                        a[s2][j] = (a[s1][j]-a[s1][j-1])/ndu[pk+1][rk+j];
                        d += a[s2][j]*ndu[rk+j][pk];
                    }
                    if (r<=pk) {
                        a[s2][k] = -a[s1][k-1]/ndu[pk+1][r];
                        d += a[s2][k]*ndu[r][pk] ;
                    }
                    ders[k][r] = d ;
                    j = s1 ; s1 = s2 ; s2 = j ; // Switch rows
                }
            }
            // Multiply through by the correct factors
            r = D ;
            for(int k=1;k<=(int)n;k++){
                for(int j=(int)D;j>=0;--j)
                    ders[k][j] *= r ;
                r *= D-k ;
            }
        }
 
 
        void CurveDerivs(double u, uint32 d, _array_vector& ck)
        {
            // locals
            uint32 du = d;
            if (D<du) du = D;
            for (uint32 k=D+1; k<=d; k++) ck[k] = 0.0;
            uint32 span = FindSpan(u) ;
            _matrix nders;
            DersBasisFuns(du,u,span,nders) ;
 
            if(m_uFlags & eFlgCustomWeight) {
                // Get the weighted derivatives (Aders) and weight derivatives (wders)
                // needed for algorithm below
                _array_vector Aders;
                TINOSVector<N> wders;
 
                for(int k=du;k>=0;--k){
                    Aders[k] = 0.0 ;
                    for(int j=0; j<=(int)D; j++){
                        Aders[k] = Aders[k] + nders[k][j]*P[span-D+j] * P[span-D+j].w;
                        wders[k] = wders[k] + nders[k][j]*P[span-D+j].w ;
                    } // end for
                } // end for
 
                double wders0_inv = 1/wders[0];
 
                for(uint32 k= 0; k<=d; k++) {
                    TINOSVector<N> v = Aders[k];
                    for(uint32 i=1; i<=k; i++)
                        v = v - BinCoeff[k * 6 + i]*wders[i]*ck[k-i];
                    ck[k] = v*wders0_inv;
                }
            }
            else {
                for(int k=du;k>=0;--k){
                    ck[k] = 0.0 ;
                    for(int j=0; j<=(int)D; j++){
                        ck[k] = ck[k] + nders[k][j]*P[span-D+j] ;
                    } // end for
                } // end for
            }
        }
 
        uint32 GetSegment (uint32 auChannel, double adP, TINOSVector<N>& ovPos)
        {
            // locals
            auto& cnl = m_sPreviousCache[auChannel];
 
            // first call ?
            if (cnl.m_dDistanceSegEnd<0.0){
                // yes -> calc distance
                cnl.m_dDistanceSegEnd = (P[1]-ovPos).GetMaskedLength2(m_uLengthMask);
                cnl.m_dDistanceSegBeg = (P[0]-ovPos).GetMaskedLength2(m_uLengthMask);
                cnl.m_uSegment = 0;
            }
            // moving forward?
            else if(adP > cnl.m_dP) {
                // avoid overflows
                if (cnl.m_uSegment < P.size()-2) {
                    // calc distance to segment end
                    double dDistanceOld = cnl.m_dDistanceSegEnd;
                    cnl.m_dDistanceSegEnd =
                            (P[cnl.m_uSegment+1]-ovPos).GetMaskedLength2(m_uLengthMask);
                    // segment change ?
                    if (dDistanceOld<cnl.m_dDistanceSegEnd){
                        // yes
                        cnl.m_uSegment++;
                        // Last end is the new begin
                        cnl.m_dDistanceSegBeg = cnl.m_dDistanceSegEnd;
                        // recalc distance to next segment
                        cnl.m_dDistanceSegEnd =
                                (P[cnl.m_uSegment+1]-ovPos).GetMaskedLength2(m_uLengthMask);
                    } // end if
                    else {
                        // update begin in case of turn around.
                        cnl.m_dDistanceSegBeg =
                                (P[cnl.m_uSegment]-ovPos).GetMaskedLength2(m_uLengthMask);
                    } // end if
                }
                else {
                    // update begin in case of turn around.
                    cnl.m_dDistanceSegBeg =
                            (P[cnl.m_uSegment]-ovPos).GetMaskedLength2(m_uLengthMask);
                } // end if
            }
            // moving backward?
            else if(adP < cnl.m_dP) {
                // avoid undeflows
                if (cnl.m_uSegment > 0) {
                    // calc distance to segment end
                    double dDistanceOld = cnl.m_dDistanceSegBeg;
                    cnl.m_dDistanceSegBeg =
                            (P[cnl.m_uSegment]-ovPos).GetMaskedLength2(m_uLengthMask);
                    // segment change ?
                    if (dDistanceOld<cnl.m_dDistanceSegEnd){
                        // yes
                        cnl.m_uSegment--;
                        // Last begin is the new end
                        cnl.m_dDistanceSegEnd = cnl.m_dDistanceSegBeg;
                        // recalc distance to next segment
                        cnl.m_dDistanceSegBeg = (P[cnl.m_uSegment]-ovPos).GetLength2(m_uLengthMask);
                    } // end if
                    else {
                        // update end in case of turn around.
                        cnl.m_dDistanceSegEnd = (P[cnl.m_uSegment+1]-ovPos).GetLength2(m_uLengthMask);
                    } // end if
                }
                else {
                    // update end in case of turn around.
                    cnl.m_dDistanceSegEnd = (P[cnl.m_uSegment+1]-ovPos).GetLength2(m_uLengthMask);
                } // end if
            }
 
 
            cnl.m_dP = adP;
            // return result
            return cnl.m_uSegment;
        }
 
    // protected members
    protected:
 
        // curve flags
        enum {
            eFlgPrepared = 0x00000001,      //<! curve is prepared
            eFlgCustomWeight = 0x00000002,  //<! control points with custom weights
            eFlgZeroWeight = 0x00000004,    //<! all control points have zero weight
            eFlgCurveValid = 0x00000008,    //<! curve is valid
        };
        enum {
            eCnsMaxChannels = 4,
        };
 
        uint32 m_uFlags = eFlgZeroWeight;
        uint32 D = 1;
        const uint32 m_uLengthMask = 0xFFFFFFF8;
        uint32 m_uInterpolationPoints = DF_INOS_NURBSCURVE_INTERPOLATION_POINTS;
        double m_dLength = std::numeric_limits<double>::quiet_NaN();
        inos_std::vector<CINOSNurbsPoint<N>> P;
        inos_std::vector<double> U;
 
        uint8 m_uLengthDims[N];
        uint8 m_uNoLengthDims;
 
 
        // Previous values cache for local get function
        struct SPreviousCache {
            uint32 m_uPSpan;
            uint32 m_uSplineIndex;
            uint32 m_uUSpan;
            uint32 m_uSegment;
            double m_dDistanceSegBeg;
            double m_dDistanceSegEnd;
            double m_dP;
 
 
        } m_sPreviousCache[eCnsMaxChannels];
 
        inos_std::vector<CINOSSpline*> m_UofPs;
 
    // dynamic object handling
    DECLARE_DYNAMIC_N(CINOSNurbsCurve, N);
};
 
IMPLEMENT_DYNAMIC_N(CINOSNurbsCurve, N);
 
//------------------------------------------------------------------------------
// end of file
//------------------------------------------------------------------------------
 
#endif  // INC_CINOSNURBSCURVE_H