dragonpilot - 基於 openpilot 的開源駕駛輔助系統
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.
 
 
 
 
 
 

111 lines
3.6 KiB

/*
* This file is part of ACADO Toolkit.
*
* ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
* Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
* Milan Vukov, Rien Quirynen, KU Leuven.
* Developed within the Optimization in Engineering Center (OPTEC)
* under supervision of Moritz Diehl. All rights reserved.
*
* ACADO Toolkit 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 3 of the License, or (at your option) any later version.
*
* ACADO Toolkit 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 ACADO Toolkit; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file include/acado/function/evaluation_point.ipp
* \author Boris Houska, Hans Joachim Ferreau
* \date 2010
*/
//
// PUBLIC MEMBER FUNCTIONS:
//
BEGIN_NAMESPACE_ACADO
inline returnValue EvaluationPoint::copy( const int *order, const DVector &rhs ){
uint i;
for( i = 0; i < rhs.getDim(); i++ )
z[order[i]] = rhs(i);
return SUCCESSFUL_RETURN;
}
inline DVector EvaluationPoint::backCopy( const int *order, const uint &dim ) const{
DVector tmp(dim);
uint i;
for( i = 0; i < dim; i++ )
tmp(i) = z[order[i]];
return tmp;
}
inline returnValue EvaluationPoint::setT ( const double &t ){ return copy( idx[0], DVector(1,&t) ); }
inline returnValue EvaluationPoint::setX ( const DVector &x ){ return copy( idx[1], x ); }
inline returnValue EvaluationPoint::setXA( const DVector &xa ){ return copy( idx[2], xa ); }
inline returnValue EvaluationPoint::setP ( const DVector &p ){ return copy( idx[3], p ); }
inline returnValue EvaluationPoint::setU ( const DVector &u ){ return copy( idx[4], u ); }
inline returnValue EvaluationPoint::setW ( const DVector &w ){ return copy( idx[5], w ); }
inline returnValue EvaluationPoint::setDX( const DVector &dx ){ return copy( idx[6], dx ); }
inline returnValue EvaluationPoint::setZ ( const uint &idx_,
const OCPiterate &iter ){
setT ( iter.getTime (idx_) );
setX ( iter.getX (idx_) );
setXA( iter.getXA (idx_) );
setP ( iter.getP (idx_) );
setU ( iter.getU (idx_) );
setW ( iter.getW (idx_) );
return SUCCESSFUL_RETURN;
}
inline returnValue EvaluationPoint::setZero( )
{
if ( z != 0 )
{
for( uint run1 = 0; run1 < N; run1++ )
z[run1] = 0.0;
}
return SUCCESSFUL_RETURN;
}
inline double EvaluationPoint::getT () const{ return z[*idx[0]] ; }
inline DVector EvaluationPoint::getX () const{ return backCopy( idx[1], nx ); }
inline DVector EvaluationPoint::getXA() const{ return backCopy( idx[2], na ); }
inline DVector EvaluationPoint::getP () const{ return backCopy( idx[3], np ); }
inline DVector EvaluationPoint::getU () const{ return backCopy( idx[4], nu ); }
inline DVector EvaluationPoint::getW () const{ return backCopy( idx[5], nw ); }
inline DVector EvaluationPoint::getDX() const{ return backCopy( idx[6], nd ); }
inline double* EvaluationPoint::getEvaluationPointer() const{ return z; }
CLOSE_NAMESPACE_ACADO
// end of file.