You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
342 lines
6.2 KiB
342 lines
6.2 KiB
/*
|
|
* This file is part of qpOASES.
|
|
*
|
|
* qpOASES -- An Implementation of the Online Active Set Strategy.
|
|
* Copyright (C) 2007-2008 by Hans Joachim Ferreau 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
|
|
*
|
|
*/
|
|
|
|
|
|
/**
|
|
* \file SRC/Indexlist.cpp
|
|
* \author Hans Joachim Ferreau
|
|
* \version 1.3embedded
|
|
* \date 2007-2008
|
|
*
|
|
* Implementation of the Indexlist class designed to manage index lists of
|
|
* constraints and bounds within a QProblem_SubjectTo.
|
|
*/
|
|
|
|
|
|
#include <Indexlist.hpp>
|
|
|
|
|
|
/*****************************************************************************
|
|
* P U B L I C *
|
|
*****************************************************************************/
|
|
|
|
|
|
/*
|
|
* I n d e x l i s t
|
|
*/
|
|
Indexlist::Indexlist( ) : length( 0 ),
|
|
first( -1 ),
|
|
last( -1 ),
|
|
lastusedindex( -1 ),
|
|
physicallength( INDEXLISTFACTOR*(NVMAX+NCMAX) )
|
|
{
|
|
int i;
|
|
|
|
for( i=0; i<physicallength; ++i )
|
|
{
|
|
number[i] = -1;
|
|
next[i] = -1;
|
|
previous[i] = -1;
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
* I n d e x l i s t
|
|
*/
|
|
Indexlist::Indexlist( const Indexlist& rhs ) : length( rhs.length ),
|
|
first( rhs.first ),
|
|
last( rhs.last ),
|
|
lastusedindex( rhs.lastusedindex ),
|
|
physicallength( rhs.physicallength )
|
|
{
|
|
int i;
|
|
|
|
for( i=0; i<physicallength; ++i )
|
|
{
|
|
number[i] = rhs.number[i];
|
|
next[i] = rhs.next[i];
|
|
previous[i] = rhs.previous[i];
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
* ~ I n d e x l i s t
|
|
*/
|
|
Indexlist::~Indexlist( )
|
|
{
|
|
}
|
|
|
|
|
|
/*
|
|
* o p e r a t o r =
|
|
*/
|
|
Indexlist& Indexlist::operator=( const Indexlist& rhs )
|
|
{
|
|
int i;
|
|
|
|
if ( this != &rhs )
|
|
{
|
|
length = rhs.length;
|
|
first = rhs.first;
|
|
last = rhs.last;
|
|
lastusedindex = rhs.lastusedindex;
|
|
physicallength = rhs.physicallength;
|
|
|
|
for( i=0; i<physicallength; ++i )
|
|
{
|
|
number[i] = rhs.number[i];
|
|
next[i] = rhs.next[i];
|
|
previous[i] = rhs.previous[i];
|
|
}
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
|
|
/*
|
|
* i n i t
|
|
*/
|
|
returnValue Indexlist::init( )
|
|
{
|
|
int i;
|
|
|
|
length = 0;
|
|
first = -1;
|
|
last = -1;
|
|
lastusedindex = -1;
|
|
physicallength = INDEXLISTFACTOR*(NVMAX+NCMAX);
|
|
|
|
for( i=0; i<physicallength; ++i )
|
|
{
|
|
number[i] = -1;
|
|
next[i] = -1;
|
|
previous[i] = -1;
|
|
}
|
|
|
|
return SUCCESSFUL_RETURN;
|
|
}
|
|
|
|
|
|
/*
|
|
* g e t N u m b e r A r r a y
|
|
*/
|
|
returnValue Indexlist::getNumberArray( int* const numberarray ) const
|
|
{
|
|
int i;
|
|
int n = first;
|
|
|
|
/* Run trough indexlist and store numbers in numberarray. */
|
|
for( i=0; i<length; ++i )
|
|
{
|
|
if ( ( n >= 0 ) && ( number[n] >= 0 ) )
|
|
numberarray[i] = number[n];
|
|
else
|
|
return THROWERROR( RET_INDEXLIST_CORRUPTED );
|
|
|
|
n = next[n];
|
|
}
|
|
|
|
return SUCCESSFUL_RETURN;
|
|
}
|
|
|
|
|
|
/*
|
|
* g e t I n d e x
|
|
*/
|
|
int Indexlist::getIndex( int givennumber ) const
|
|
{
|
|
int i;
|
|
int n = first;
|
|
int index = -1; /* return -1 by default */
|
|
|
|
/* Run trough indexlist until number is found, if so return it index. */
|
|
for ( i=0; i<length; ++i )
|
|
{
|
|
if ( number[n] == givennumber )
|
|
{
|
|
index = i;
|
|
break;
|
|
}
|
|
|
|
n = next[n];
|
|
}
|
|
|
|
return index;
|
|
}
|
|
|
|
|
|
/*
|
|
* g e t P h y s i c a l I n d e x
|
|
*/
|
|
int Indexlist::getPhysicalIndex( int givennumber ) const
|
|
{
|
|
int i;
|
|
int n = first;
|
|
int index = -1; /* return -1 by default */
|
|
|
|
/* Run trough indexlist until number is found, if so return it physicalindex. */
|
|
for ( i=0; i<length; ++i )
|
|
{
|
|
if ( number[n] == givennumber )
|
|
{
|
|
index = n;
|
|
break;
|
|
}
|
|
|
|
n = next[n];
|
|
}
|
|
|
|
return index;
|
|
}
|
|
|
|
|
|
/*
|
|
* a d d N u m b e r
|
|
*/
|
|
returnValue Indexlist::addNumber( int addnumber )
|
|
{
|
|
int i;
|
|
|
|
if ( lastusedindex+1 < physicallength )
|
|
{
|
|
/* If there is enough storage, add number to indexlist. */
|
|
++lastusedindex;
|
|
number[lastusedindex] = addnumber;
|
|
next[lastusedindex] = 0;
|
|
|
|
if ( length == 0 )
|
|
{
|
|
first = lastusedindex;
|
|
previous[lastusedindex] = 0;
|
|
}
|
|
else
|
|
{
|
|
next[last] = lastusedindex;
|
|
previous[lastusedindex] = last;
|
|
}
|
|
|
|
last = lastusedindex;
|
|
++length;
|
|
|
|
return SUCCESSFUL_RETURN;
|
|
}
|
|
else
|
|
{
|
|
/* Rearrangement of index list necessary! */
|
|
if ( length == physicallength )
|
|
return THROWERROR( RET_INDEXLIST_EXCEEDS_MAX_LENGTH );
|
|
else
|
|
{
|
|
int numberArray[NVMAX+NCMAX];
|
|
getNumberArray( numberArray );
|
|
|
|
/* copy existing elements */
|
|
for ( i=0; i<length; ++i )
|
|
{
|
|
number[i] = numberArray[i];
|
|
next[i] = i+1;
|
|
previous[i] = i-1;
|
|
}
|
|
|
|
/* add new number at end of list */
|
|
number[length] = addnumber;
|
|
next[length] = -1;
|
|
previous[length] = length-1;
|
|
|
|
/* and set remaining entries to empty */
|
|
for ( i=length+1; i<physicallength; ++i )
|
|
{
|
|
number[i] = -1;
|
|
next[i] = -1;
|
|
previous[i] = -1;
|
|
}
|
|
|
|
first = 0;
|
|
last = length;
|
|
lastusedindex = length;
|
|
++length;
|
|
|
|
return THROWWARNING( RET_INDEXLIST_MUST_BE_REORDERD );
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
* r e m o v e N u m b e r
|
|
*/
|
|
returnValue Indexlist::removeNumber( int removenumber )
|
|
{
|
|
int i = getPhysicalIndex( removenumber );
|
|
|
|
/* nothing to be done if number is not contained in index set */
|
|
if ( i < 0 )
|
|
return SUCCESSFUL_RETURN;
|
|
|
|
int p = previous[i];
|
|
int n = next[i];
|
|
|
|
if ( i == last )
|
|
last = p;
|
|
else
|
|
previous[n] = p;
|
|
|
|
if ( i == first )
|
|
first = n;
|
|
else
|
|
next[p] = n;
|
|
|
|
number[i] = -1;
|
|
next[i] = -1;
|
|
previous[i] = -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 = getPhysicalIndex( number1 );
|
|
int index2 = getPhysicalIndex( number2 );
|
|
|
|
/* consistency check */
|
|
if ( ( index1 < 0 ) || ( index2 < 0 ) )
|
|
return THROWERROR( RET_INDEXLIST_CORRUPTED );
|
|
|
|
int tmp = number[index1];
|
|
number[index1] = number[index2];
|
|
number[index2] = tmp;
|
|
|
|
return SUCCESSFUL_RETURN;
|
|
}
|
|
|
|
|
|
/*
|
|
* end of file
|
|
*/
|
|
|