external_packages/qpOASES-3.0beta/src/Indexlist.cpp

Go to the documentation of this file.

/*
 *      This file is part of qpOASES.
 *
 *      qpOASES -- An Implementation of the Online Active Set Strategy.
 *      Copyright (C) 2007-2011 by Hans Joachim Ferreau, Andreas Potschka,
 *      Christian Kirches et al. All rights reserved.
 *
 *      qpOASES is free software; you can redistribute it and/or
 *      modify it under the terms of the GNU Lesser General Public
 *      License as published by the Free Software Foundation; either
 *      version 2.1 of the License, or (at your option) any later version.
 *
 *      qpOASES is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *      See the GNU Lesser General Public License for more details.
 *
 *      You should have received a copy of the GNU Lesser General Public
 *      License along with qpOASES; if not, write to the Free Software
 *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */


#include <qpOASES/Indexlist.hpp>


BEGIN_NAMESPACE_QPOASES


/*****************************************************************************
 *  P U B L I C                                                              *
 *****************************************************************************/


/*
 *      I n d e x l i s t
 */
Indexlist::Indexlist( )
{
        number   = 0;
        iSort = 0;

        init( );
}


/*
 *      I n d e x l i s t
 */
Indexlist::Indexlist( int n )
{
        number   = 0;
        iSort = 0;

        init( n );
}


/*
 *      I n d e x l i s t
 */
Indexlist::Indexlist( const Indexlist& rhs )
{
        copy( rhs );
}


/*
 *      ~ I n d e x l i s t
 */
Indexlist::~Indexlist( )
{
        clear( );
}


/*
 *      o p e r a t o r =
 */
Indexlist& Indexlist::operator=( const Indexlist& rhs )
{
        if ( this != &rhs )
        {
                clear( );
                copy( rhs );
        }

        return *this;
}



/*
 *      i n i t
 */
returnValue Indexlist::init(    int n
                                                                )
{
        if ( n < 0 )
                return THROWERROR( RET_INVALID_ARGUMENTS );

        clear( );

        length = 0;
        physicallength = n;

        if ( n > 0 )
        {
                number   = new int[n];
                iSort = new int[n];
        }

        return SUCCESSFUL_RETURN;
}


/*
 *      g e t N u m b e r A r r a y
 */
returnValue Indexlist::getNumberArray( int** const numberarray ) const
{
        if (numberarray == 0)
                return THROWERROR( RET_INVALID_ARGUMENTS );

        *numberarray = number;

        return SUCCESSFUL_RETURN;
}


/*
 *      g e t I n d e x
 */
int Indexlist::getIndex( int givennumber ) const
{
        int index = findInsert(givennumber);
        return number[iSort[index]] == givennumber ? iSort[index] : -1;
}


/*
 *      a d d N u m b e r
 */
returnValue Indexlist::addNumber( int addnumber )
{
        if ( length >= physicallength )
                return THROWERROR( RET_INDEXLIST_EXCEEDS_MAX_LENGTH );

        int i, j;
        number[length] = addnumber;
        j = findInsert(addnumber);
        for (i = length; i > j+1; i--)
                iSort[i] = iSort[i-1];
        iSort[j+1] = length;
        ++length;

        return SUCCESSFUL_RETURN;
}


/*
 *      r e m o v e N u m b e r
 */
returnValue Indexlist::removeNumber( int removenumber )
{
        int i;
        int idx = findInsert( removenumber );
        int iSidx = iSort[idx];

        /* nothing to be done if number is not contained in index set */
        if ( number[iSidx] != removenumber )
                return SUCCESSFUL_RETURN;

        /* update sorted indices iSort first */
        for (i = 0; i < length; i++)
                if (iSort[i] > iSidx) iSort[i]--;
        for (i = idx+1; i < length; i++)
                iSort[i-1] = iSort[i];

        /* remove from numbers list */
        for( i=iSidx; i<length-1; ++i )
                number[i] = number[i+1];
        number[length-1] = -1;

        --length;

        return SUCCESSFUL_RETURN;
}


/*
 *      s w a p N u m b e r s
 */
returnValue Indexlist::swapNumbers( int number1, int number2 )
{
        int index1 = findInsert( number1 );
        int index2 = findInsert( number2 );

        /* consistency check */
        if ( ( number[iSort[index1]] != number1 ) || ( number[iSort[index2]] != number2 ) )
                return THROWERROR( RET_INDEXLIST_CORRUPTED );

        int tmp;
        /* swap numbers */
        tmp = number[iSort[index1]];
        number[iSort[index1]] = number[iSort[index2]];
        number[iSort[index2]] = tmp;
        /* swap sorting indices */
        tmp = iSort[index1];
        iSort[index1] = iSort[index2];
        iSort[index2] = tmp;

        return SUCCESSFUL_RETURN;
}



/*****************************************************************************
 *  P R O T E C T E D                                                        *
 *****************************************************************************/

/*
 *      c l e a r
 */
returnValue Indexlist::clear( )
{
        if ( iSort != 0 )
        {
                delete[] iSort;
                iSort = 0;
        }

        if ( number != 0 )
        {
                delete[] number;
                number = 0;
        }

        return SUCCESSFUL_RETURN;
}


/*
 *      c o p y
 */
returnValue Indexlist::copy(    const Indexlist& rhs
                                                                )
{
        int i;

        length = rhs.length;
        physicallength = rhs.physicallength;

        if ( rhs.number != 0 )
        {
                number = new int[physicallength];
                for( i=0; i<physicallength; ++i )
                        number[i] = rhs.number[i];
                iSort = new int[physicallength];
                for( i=0; i<physicallength; ++i )
                        iSort[i] = rhs.iSort[i];
        }
        else
        {
                number = 0;
                iSort = 0;
        }

        return SUCCESSFUL_RETURN;
}

int Indexlist::findInsert(int i) const
{
        /* quick check if index can be appended */
        if (length == 0 || i < number[iSort[0]]) return -1;
        if (i >= number[iSort[length-1]]) return length-1;

        /* otherwise, perform bisection search */
        int fst = 0, lst = length-1, mid;

        while (fst < lst - 1)
        {
                mid = (fst + lst) / 2;
                if (i >= number[iSort[mid]]) fst = mid;
                else lst = mid;
        }

        return fst;
}

END_NAMESPACE_QPOASES


/*
 *      end of file
 */