<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: AccessibleParaManager.cxx,v $
 *
 *  $Revision: 1.15 $
 *
 *  last change: $Author: hr $ $Date: 2007-06-27 16:38:39 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library 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 this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_svx.hxx"

//------------------------------------------------------------------------
//
// Global header
//
//------------------------------------------------------------------------

#ifndef _COM_SUN_STAR_UNO_ANY_HXX_
#include <com/sun/star/uno/Any.hxx>
#endif

#ifndef _COM_SUN_STAR_UNO_REFERENCE_HXX_
#include <com/sun/star/uno/Reference.hxx>
#endif

#ifndef _CPPUHELPER_WEAKREF_HXX_
#include <cppuhelper/weakref.hxx>
#endif

#ifndef _COM_SUN_STAR_ACCESSIBILITY_XACCESSIBLE_HPP_
#include <com/sun/star/accessibility/XAccessible.hpp>
#endif

#ifndef _COM_SUN_STAR_ACCESSIBILITY_ACCESSIBLESTATETYPE_HPP_
#include <com/sun/star/accessibility/AccessibleStateType.hpp>
#endif

//------------------------------------------------------------------------
//
// Project-local header
//
//------------------------------------------------------------------------

#include "unoedhlp.hxx"
#include "unopracc.hxx"
#include <svx/unoedsrc.hxx>
#include "AccessibleParaManager.hxx"
#include "AccessibleEditableTextPara.hxx"

using namespace ::com::sun::star;
using namespace ::com::sun::star::accessibility;

namespace accessibility
{
    AccessibleParaManager::AccessibleParaManager() :
        maChildren(1),
        maEEOffset( 0, 0 ),
        mnFocusedChild( -1 ),
        mbActive( sal_False )
    {
    }

    AccessibleParaManager::~AccessibleParaManager()
    {
        // owner is responsible for possible child defuncs
    }

    void AccessibleParaManager::SetAdditionalChildStates( const VectorOfStates& rChildStates )
    {
        maChildStates = rChildStates;
    }

    const AccessibleParaManager::VectorOfStates& AccessibleParaManager::GetAdditionalChildStates() const
    {
        return maChildStates;
    }

    void AccessibleParaManager::SetNum( sal_Int32 nNumParas )
    {
        if( (size_t)nNumParas < maChildren.size() )
            Release( nNumParas, maChildren.size() );

        maChildren.resize( nNumParas );

        if( mnFocusedChild >= nNumParas )
            mnFocusedChild = -1;
    }

    sal_uInt32 AccessibleParaManager::GetNum() const
    {
        return maChildren.size();
    }

    AccessibleParaManager::VectorOfChildren::iterator AccessibleParaManager::begin()
    {
        return maChildren.begin();
    }

    AccessibleParaManager::VectorOfChildren::iterator AccessibleParaManager::end()
    {
        return maChildren.end();
    }

    AccessibleParaManager::VectorOfChildren::const_iterator AccessibleParaManager::begin() const
    {
        return maChildren.begin();
    }

    AccessibleParaManager::VectorOfChildren::const_iterator AccessibleParaManager::end() const
    {
        return maChildren.end();
    }

    void AccessibleParaManager::Release( sal_uInt32 nPara )
    {
        DBG_ASSERT( maChildren.size() > nPara, "AccessibleParaManager::Release: invalid index" );

        if( maChildren.size() > nPara )
        {
            ShutdownPara( GetChild( nPara ) );

            // clear reference and rect
            maChildren[ nPara ] = WeakChild();
        }
    }

    void AccessibleParaManager::FireEvent( sal_uInt32 nPara,
                                           const sal_Int16 nEventId,
                                           const uno::Any& rNewValue,
                                           const uno::Any& rOldValue ) const
    {
        DBG_ASSERT( maChildren.size() > nPara, "AccessibleParaManager::FireEvent: invalid index" );

        if( maChildren.size() > nPara )
        {
            WeakPara::HardRefType maChild( GetChild( nPara ).first.get() );
            if( maChild.is() )
                maChild->FireEvent( nEventId, rNewValue, rOldValue );
        }
    }

    sal_Bool AccessibleParaManager::IsReferencable( WeakPara::HardRefType aChild )
    {
        return aChild.is();
    }

    sal_Bool AccessibleParaManager::IsReferencable( sal_uInt32 nChild ) const
    {
        DBG_ASSERT( maChildren.size() > nChild, "AccessibleParaManager::IsReferencable: invalid index" );

        if( maChildren.size() > nChild )
        {
            // retrieve hard reference from weak one
            return IsReferencable( GetChild( nChild ).first.get() );
        }
        else
        {
            return sal_False;
        }
    }

    AccessibleParaManager::WeakChild AccessibleParaManager::GetChild( sal_uInt32 nParagraphIndex ) const
    {
        DBG_ASSERT( maChildren.size() > nParagraphIndex, "AccessibleParaManager::GetChild: invalid index" );

        if( maChildren.size() > nParagraphIndex )
        {
            return maChildren[ nParagraphIndex ];
        }
        else
        {
            return WeakChild();
        }
    }

    AccessibleParaManager::Child AccessibleParaManager::CreateChild( sal_Int32                              nChild,
                                                                     const uno::Reference< XAccessible >&   xFrontEnd,
                                                                     SvxEditSourceAdapter&                  rEditSource,
                                                                     sal_uInt32                             nParagraphIndex )
    {
        DBG_ASSERT( maChildren.size() > nParagraphIndex, "AccessibleParaManager::CreateChild: invalid index" );

        if( maChildren.size() > nParagraphIndex )
        {
            // retrieve hard reference from weak one
            WeakPara::HardRefType aChild( GetChild( nParagraphIndex ).first.get() );

            if( !IsReferencable( nParagraphIndex ) )
            {
                // there is no hard reference available, create object then
                // --> OD 2006-01-11 #i27138#
                AccessibleEditableTextPara* pChild = new AccessibleEditableTextPara( xFrontEnd, this );
                // <--
                uno::Reference< XAccessible > xChild( static_cast< ::cppu::OWeakObject* > (pChild), uno::UNO_QUERY );

                if( !xChild.is() )
                    throw uno::RuntimeException(::rtl::OUString(RTL_CONSTASCII_USTRINGPARAM("Child creation failed")), xFrontEnd);

                aChild = WeakPara::HardRefType( xChild, pChild );

                InitChild( *aChild, rEditSource, nChild, nParagraphIndex );

                maChildren[ nParagraphIndex ] = WeakChild( aChild, pChild->getBounds() );
            }

            return Child( aChild.getRef(), GetChild( nParagraphIndex ).second );
        }
        else
        {
            return Child();
        }
    }

    void AccessibleParaManager::SetEEOffset( const Point& rOffset )
    {
        maEEOffset = rOffset;

        MemFunAdapter< const Point& > aAdapter( &::accessibility::AccessibleEditableTextPara::SetEEOffset, rOffset );
        ::std::for_each( begin(), end(), aAdapter );
    }

    void AccessibleParaManager::SetActive( sal_Bool bActive )
    {
        mbActive = bActive;

        if( bActive )
        {
            SetState( AccessibleStateType::ACTIVE );
            SetState( AccessibleStateType::EDITABLE );
        }
        else
        {
            UnSetState( AccessibleStateType::ACTIVE );
            UnSetState( AccessibleStateType::EDITABLE );
        }
    }

    void AccessibleParaManager::SetFocus( sal_Int32 nChild )
    {
        if( mnFocusedChild != -1 )
            UnSetState( mnFocusedChild, AccessibleStateType::FOCUSED );

        mnFocusedChild = nChild;

        if( mnFocusedChild != -1 )
            SetState( mnFocusedChild, AccessibleStateType::FOCUSED );
    }

    void AccessibleParaManager::InitChild( AccessibleEditableTextPara&  rChild,
                                           SvxEditSourceAdapter&        rEditSource,
                                           sal_Int32                    nChild,
                                           sal_uInt32                   nParagraphIndex ) const
    {
        rChild.SetEditSource( &rEditSource );
        rChild.SetIndexInParent( nChild );
        rChild.SetParagraphIndex( nParagraphIndex );

        rChild.SetEEOffset( maEEOffset );

        if( mbActive )
        {
            rChild.SetState( AccessibleStateType::ACTIVE );
            rChild.SetState( AccessibleStateType::EDITABLE );
        }

        if( mnFocusedChild == static_cast<sal_Int32>(nParagraphIndex) )
            rChild.SetState( AccessibleStateType::FOCUSED );

        // add states passed from outside
        for( VectorOfStates::const_iterator aIt = maChildStates.begin(), aEnd = maChildStates.end(); aIt != aEnd; ++aIt )
            rChild.SetState( *aIt );
    }

    void AccessibleParaManager::SetState( sal_Int32 nChild, const sal_Int16 nStateId )
    {
        MemFunAdapter< const sal_Int16 > aFunc( &AccessibleEditableTextPara::SetState,
                                                nStateId );
        aFunc( GetChild(nChild) );
    }

    void AccessibleParaManager::SetState( const sal_Int16 nStateId )
    {
        ::std::for_each( begin(), end(),
                         MemFunAdapter< const sal_Int16 >( &AccessibleEditableTextPara::SetState,
                                                           nStateId ) );
    }

    void AccessibleParaManager::UnSetState( sal_Int32 nChild, const sal_Int16 nStateId )
    {
        MemFunAdapter< const sal_Int16 > aFunc( &AccessibleEditableTextPara::UnSetState,
                                                nStateId );
        aFunc( GetChild(nChild) );
    }

    void AccessibleParaManager::UnSetState( const sal_Int16 nStateId )
    {
        ::std::for_each( begin(), end(),
                         MemFunAdapter< const sal_Int16 >( &AccessibleEditableTextPara::UnSetState,
                                                           nStateId ) );
    }

    void AccessibleParaManager::SetEditSource( SvxEditSourceAdapter* pEditSource )
    {
        MemFunAdapter< SvxEditSourceAdapter* > aAdapter( &::accessibility::AccessibleEditableTextPara::SetEditSource, pEditSource );
        ::std::for_each( begin(), end(), aAdapter );
    }

    // not generic yet, no arguments...
    class AccessibleParaManager_DisposeChildren : public ::std::unary_function< ::accessibility::AccessibleEditableTextPara&, void >
    {
    public:
        AccessibleParaManager_DisposeChildren() {}
        void operator()( ::accessibility::AccessibleEditableTextPara& rPara )
        {
            rPara.Dispose();
        }
    };

    void AccessibleParaManager::Dispose()
    {
        AccessibleParaManager_DisposeChildren aFunctor;

        ::std::for_each( begin(), end(),
                         WeakChildAdapter< AccessibleParaManager_DisposeChildren > (aFunctor) );
    }

    // not generic yet, too many method arguments...
    class StateChangeEvent : public ::std::unary_function< ::accessibility::AccessibleEditableTextPara&, void >
    {
    public:
        typedef void return_type;
        StateChangeEvent( const sal_Int16 nEventId,
                          const uno::Any& rNewValue,
                          const uno::Any& rOldValue ) :
            mnEventId( nEventId ),
            mrNewValue( rNewValue ),
            mrOldValue( rOldValue ) {}
        void operator()( ::accessibility::AccessibleEditableTextPara& rPara )
        {
            rPara.FireEvent( mnEventId, mrNewValue, mrOldValue );
        }

    private:
        const sal_Int16 mnEventId;
        const uno::Any& mrNewValue;
        const uno::Any& mrOldValue;
    };

    void AccessibleParaManager::FireEvent( sal_uInt32 nStartPara,
                                           sal_uInt32 nEndPara,
                                           const sal_Int16 nEventId,
                                           const uno::Any& rNewValue,
                                           const uno::Any& rOldValue ) const
    {
        DBG_ASSERT( maChildren.size() > nStartPara &&
                    maChildren.size() >= nEndPara , "AccessibleParaManager::FireEvent: invalid index" );

        if( maChildren.size() > nStartPara &&
            maChildren.size() >= nEndPara )
        {
            VectorOfChildren::const_iterator front = maChildren.begin();
            VectorOfChildren::const_iterator back = front;

            ::std::advance( front, nStartPara );
            ::std::advance( back, nEndPara );

            StateChangeEvent aFunctor( nEventId, rNewValue, rOldValue );

            ::std::for_each( front, back, AccessibleParaManager::WeakChildAdapter< StateChangeEvent >( aFunctor ) );
        }
    }

    class ReleaseChild : public ::std::unary_function< const AccessibleParaManager::WeakChild&, AccessibleParaManager::WeakChild >
    {
    public:
        AccessibleParaManager::WeakChild operator()( const AccessibleParaManager::WeakChild& rPara )
        {
            AccessibleParaManager::ShutdownPara( rPara );

            // clear reference
            return AccessibleParaManager::WeakChild();
        }
    };

    void AccessibleParaManager::Release( sal_uInt32 nStartPara, sal_uInt32 nEndPara )
    {
        DBG_ASSERT( maChildren.size() > nStartPara &&
                    maChildren.size() >= nEndPara, "AccessibleParaManager::Release: invalid index" );

        if( maChildren.size() > nStartPara &&
            maChildren.size() >= nEndPara )
        {
            VectorOfChildren::iterator front = maChildren.begin();
            VectorOfChildren::iterator back = front;

            ::std::advance( front, nStartPara );
            ::std::advance( back, nEndPara );

            ::std::transform( front, back, front, ReleaseChild() );
        }
    }

    void AccessibleParaManager::ShutdownPara( const WeakChild& rChild )
    {
        WeakPara::HardRefType aChild( rChild.first.get() );

        if( IsReferencable( aChild ) )
            aChild->SetEditSource( NULL );
    }

}

//------------------------------------------------------------------------
<commit_msg>INTEGRATION: CWS changefileheader (1.15.368); FILE MERGED 2008/04/01 15:50:08 thb 1.15.368.3: #i85898# Stripping all external header guards 2008/04/01 12:47:58 thb 1.15.368.2: #i85898# Stripping all external header guards 2008/03/31 14:19:11 rt 1.15.368.1: #i87441# Change license header to LPGL v3.<commit_after>/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2008 by Sun Microsystems, Inc.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * $RCSfile: AccessibleParaManager.cxx,v $
 * $Revision: 1.16 $
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org 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 version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_svx.hxx"

//------------------------------------------------------------------------
//
// Global header
//
//------------------------------------------------------------------------
#include <com/sun/star/uno/Any.hxx>
#include <com/sun/star/uno/Reference.hxx>
#include <cppuhelper/weakref.hxx>
#include <com/sun/star/accessibility/XAccessible.hpp>
#include <com/sun/star/accessibility/AccessibleStateType.hpp>

//------------------------------------------------------------------------
//
// Project-local header
//
//------------------------------------------------------------------------

#include "unoedhlp.hxx"
#include "unopracc.hxx"
#include <svx/unoedsrc.hxx>
#include "AccessibleParaManager.hxx"
#include "AccessibleEditableTextPara.hxx"

using namespace ::com::sun::star;
using namespace ::com::sun::star::accessibility;

namespace accessibility
{
    AccessibleParaManager::AccessibleParaManager() :
        maChildren(1),
        maEEOffset( 0, 0 ),
        mnFocusedChild( -1 ),
        mbActive( sal_False )
    {
    }

    AccessibleParaManager::~AccessibleParaManager()
    {
        // owner is responsible for possible child defuncs
    }

    void AccessibleParaManager::SetAdditionalChildStates( const VectorOfStates& rChildStates )
    {
        maChildStates = rChildStates;
    }

    const AccessibleParaManager::VectorOfStates& AccessibleParaManager::GetAdditionalChildStates() const
    {
        return maChildStates;
    }

    void AccessibleParaManager::SetNum( sal_Int32 nNumParas )
    {
        if( (size_t)nNumParas < maChildren.size() )
            Release( nNumParas, maChildren.size() );

        maChildren.resize( nNumParas );

        if( mnFocusedChild >= nNumParas )
            mnFocusedChild = -1;
    }

    sal_uInt32 AccessibleParaManager::GetNum() const
    {
        return maChildren.size();
    }

    AccessibleParaManager::VectorOfChildren::iterator AccessibleParaManager::begin()
    {
        return maChildren.begin();
    }

    AccessibleParaManager::VectorOfChildren::iterator AccessibleParaManager::end()
    {
        return maChildren.end();
    }

    AccessibleParaManager::VectorOfChildren::const_iterator AccessibleParaManager::begin() const
    {
        return maChildren.begin();
    }

    AccessibleParaManager::VectorOfChildren::const_iterator AccessibleParaManager::end() const
    {
        return maChildren.end();
    }

    void AccessibleParaManager::Release( sal_uInt32 nPara )
    {
        DBG_ASSERT( maChildren.size() > nPara, "AccessibleParaManager::Release: invalid index" );

        if( maChildren.size() > nPara )
        {
            ShutdownPara( GetChild( nPara ) );

            // clear reference and rect
            maChildren[ nPara ] = WeakChild();
        }
    }

    void AccessibleParaManager::FireEvent( sal_uInt32 nPara,
                                           const sal_Int16 nEventId,
                                           const uno::Any& rNewValue,
                                           const uno::Any& rOldValue ) const
    {
        DBG_ASSERT( maChildren.size() > nPara, "AccessibleParaManager::FireEvent: invalid index" );

        if( maChildren.size() > nPara )
        {
            WeakPara::HardRefType maChild( GetChild( nPara ).first.get() );
            if( maChild.is() )
                maChild->FireEvent( nEventId, rNewValue, rOldValue );
        }
    }

    sal_Bool AccessibleParaManager::IsReferencable( WeakPara::HardRefType aChild )
    {
        return aChild.is();
    }

    sal_Bool AccessibleParaManager::IsReferencable( sal_uInt32 nChild ) const
    {
        DBG_ASSERT( maChildren.size() > nChild, "AccessibleParaManager::IsReferencable: invalid index" );

        if( maChildren.size() > nChild )
        {
            // retrieve hard reference from weak one
            return IsReferencable( GetChild( nChild ).first.get() );
        }
        else
        {
            return sal_False;
        }
    }

    AccessibleParaManager::WeakChild AccessibleParaManager::GetChild( sal_uInt32 nParagraphIndex ) const
    {
        DBG_ASSERT( maChildren.size() > nParagraphIndex, "AccessibleParaManager::GetChild: invalid index" );

        if( maChildren.size() > nParagraphIndex )
        {
            return maChildren[ nParagraphIndex ];
        }
        else
        {
            return WeakChild();
        }
    }

    AccessibleParaManager::Child AccessibleParaManager::CreateChild( sal_Int32                              nChild,
                                                                     const uno::Reference< XAccessible >&   xFrontEnd,
                                                                     SvxEditSourceAdapter&                  rEditSource,
                                                                     sal_uInt32                             nParagraphIndex )
    {
        DBG_ASSERT( maChildren.size() > nParagraphIndex, "AccessibleParaManager::CreateChild: invalid index" );

        if( maChildren.size() > nParagraphIndex )
        {
            // retrieve hard reference from weak one
            WeakPara::HardRefType aChild( GetChild( nParagraphIndex ).first.get() );

            if( !IsReferencable( nParagraphIndex ) )
            {
                // there is no hard reference available, create object then
                // --> OD 2006-01-11 #i27138#
                AccessibleEditableTextPara* pChild = new AccessibleEditableTextPara( xFrontEnd, this );
                // <--
                uno::Reference< XAccessible > xChild( static_cast< ::cppu::OWeakObject* > (pChild), uno::UNO_QUERY );

                if( !xChild.is() )
                    throw uno::RuntimeException(::rtl::OUString(RTL_CONSTASCII_USTRINGPARAM("Child creation failed")), xFrontEnd);

                aChild = WeakPara::HardRefType( xChild, pChild );

                InitChild( *aChild, rEditSource, nChild, nParagraphIndex );

                maChildren[ nParagraphIndex ] = WeakChild( aChild, pChild->getBounds() );
            }

            return Child( aChild.getRef(), GetChild( nParagraphIndex ).second );
        }
        else
        {
            return Child();
        }
    }

    void AccessibleParaManager::SetEEOffset( const Point& rOffset )
    {
        maEEOffset = rOffset;

        MemFunAdapter< const Point& > aAdapter( &::accessibility::AccessibleEditableTextPara::SetEEOffset, rOffset );
        ::std::for_each( begin(), end(), aAdapter );
    }

    void AccessibleParaManager::SetActive( sal_Bool bActive )
    {
        mbActive = bActive;

        if( bActive )
        {
            SetState( AccessibleStateType::ACTIVE );
            SetState( AccessibleStateType::EDITABLE );
        }
        else
        {
            UnSetState( AccessibleStateType::ACTIVE );
            UnSetState( AccessibleStateType::EDITABLE );
        }
    }

    void AccessibleParaManager::SetFocus( sal_Int32 nChild )
    {
        if( mnFocusedChild != -1 )
            UnSetState( mnFocusedChild, AccessibleStateType::FOCUSED );

        mnFocusedChild = nChild;

        if( mnFocusedChild != -1 )
            SetState( mnFocusedChild, AccessibleStateType::FOCUSED );
    }

    void AccessibleParaManager::InitChild( AccessibleEditableTextPara&  rChild,
                                           SvxEditSourceAdapter&        rEditSource,
                                           sal_Int32                    nChild,
                                           sal_uInt32                   nParagraphIndex ) const
    {
        rChild.SetEditSource( &rEditSource );
        rChild.SetIndexInParent( nChild );
        rChild.SetParagraphIndex( nParagraphIndex );

        rChild.SetEEOffset( maEEOffset );

        if( mbActive )
        {
            rChild.SetState( AccessibleStateType::ACTIVE );
            rChild.SetState( AccessibleStateType::EDITABLE );
        }

        if( mnFocusedChild == static_cast<sal_Int32>(nParagraphIndex) )
            rChild.SetState( AccessibleStateType::FOCUSED );

        // add states passed from outside
        for( VectorOfStates::const_iterator aIt = maChildStates.begin(), aEnd = maChildStates.end(); aIt != aEnd; ++aIt )
            rChild.SetState( *aIt );
    }

    void AccessibleParaManager::SetState( sal_Int32 nChild, const sal_Int16 nStateId )
    {
        MemFunAdapter< const sal_Int16 > aFunc( &AccessibleEditableTextPara::SetState,
                                                nStateId );
        aFunc( GetChild(nChild) );
    }

    void AccessibleParaManager::SetState( const sal_Int16 nStateId )
    {
        ::std::for_each( begin(), end(),
                         MemFunAdapter< const sal_Int16 >( &AccessibleEditableTextPara::SetState,
                                                           nStateId ) );
    }

    void AccessibleParaManager::UnSetState( sal_Int32 nChild, const sal_Int16 nStateId )
    {
        MemFunAdapter< const sal_Int16 > aFunc( &AccessibleEditableTextPara::UnSetState,
                                                nStateId );
        aFunc( GetChild(nChild) );
    }

    void AccessibleParaManager::UnSetState( const sal_Int16 nStateId )
    {
        ::std::for_each( begin(), end(),
                         MemFunAdapter< const sal_Int16 >( &AccessibleEditableTextPara::UnSetState,
                                                           nStateId ) );
    }

    void AccessibleParaManager::SetEditSource( SvxEditSourceAdapter* pEditSource )
    {
        MemFunAdapter< SvxEditSourceAdapter* > aAdapter( &::accessibility::AccessibleEditableTextPara::SetEditSource, pEditSource );
        ::std::for_each( begin(), end(), aAdapter );
    }

    // not generic yet, no arguments...
    class AccessibleParaManager_DisposeChildren : public ::std::unary_function< ::accessibility::AccessibleEditableTextPara&, void >
    {
    public:
        AccessibleParaManager_DisposeChildren() {}
        void operator()( ::accessibility::AccessibleEditableTextPara& rPara )
        {
            rPara.Dispose();
        }
    };

    void AccessibleParaManager::Dispose()
    {
        AccessibleParaManager_DisposeChildren aFunctor;

        ::std::for_each( begin(), end(),
                         WeakChildAdapter< AccessibleParaManager_DisposeChildren > (aFunctor) );
    }

    // not generic yet, too many method arguments...
    class StateChangeEvent : public ::std::unary_function< ::accessibility::AccessibleEditableTextPara&, void >
    {
    public:
        typedef void return_type;
        StateChangeEvent( const sal_Int16 nEventId,
                          const uno::Any& rNewValue,
                          const uno::Any& rOldValue ) :
            mnEventId( nEventId ),
            mrNewValue( rNewValue ),
            mrOldValue( rOldValue ) {}
        void operator()( ::accessibility::AccessibleEditableTextPara& rPara )
        {
            rPara.FireEvent( mnEventId, mrNewValue, mrOldValue );
        }

    private:
        const sal_Int16 mnEventId;
        const uno::Any& mrNewValue;
        const uno::Any& mrOldValue;
    };

    void AccessibleParaManager::FireEvent( sal_uInt32 nStartPara,
                                           sal_uInt32 nEndPara,
                                           const sal_Int16 nEventId,
                                           const uno::Any& rNewValue,
                                           const uno::Any& rOldValue ) const
    {
        DBG_ASSERT( maChildren.size() > nStartPara &&
                    maChildren.size() >= nEndPara , "AccessibleParaManager::FireEvent: invalid index" );

        if( maChildren.size() > nStartPara &&
            maChildren.size() >= nEndPara )
        {
            VectorOfChildren::const_iterator front = maChildren.begin();
            VectorOfChildren::const_iterator back = front;

            ::std::advance( front, nStartPara );
            ::std::advance( back, nEndPara );

            StateChangeEvent aFunctor( nEventId, rNewValue, rOldValue );

            ::std::for_each( front, back, AccessibleParaManager::WeakChildAdapter< StateChangeEvent >( aFunctor ) );
        }
    }

    class ReleaseChild : public ::std::unary_function< const AccessibleParaManager::WeakChild&, AccessibleParaManager::WeakChild >
    {
    public:
        AccessibleParaManager::WeakChild operator()( const AccessibleParaManager::WeakChild& rPara )
        {
            AccessibleParaManager::ShutdownPara( rPara );

            // clear reference
            return AccessibleParaManager::WeakChild();
        }
    };

    void AccessibleParaManager::Release( sal_uInt32 nStartPara, sal_uInt32 nEndPara )
    {
        DBG_ASSERT( maChildren.size() > nStartPara &&
                    maChildren.size() >= nEndPara, "AccessibleParaManager::Release: invalid index" );

        if( maChildren.size() > nStartPara &&
            maChildren.size() >= nEndPara )
        {
            VectorOfChildren::iterator front = maChildren.begin();
            VectorOfChildren::iterator back = front;

            ::std::advance( front, nStartPara );
            ::std::advance( back, nEndPara );

            ::std::transform( front, back, front, ReleaseChild() );
        }
    }

    void AccessibleParaManager::ShutdownPara( const WeakChild& rChild )
    {
        WeakPara::HardRefType aChild( rChild.first.get() );

        if( IsReferencable( aChild ) )
            aChild->SetEditSource( NULL );
    }

}

//------------------------------------------------------------------------
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
 *
 * Licensed under either the GNU Lesser General Public License v3.0 :
 * https://www.gnu.org/licenses/lgpl-3.0.html
 * or the GNU Lesser General Public License v2.1 :
 * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 * at your option.
 *
 * Originally developed for Prioritized Optimal Control (2014)
 * Refactored in 2018.
 * Design inspired by
 * - ACADO toolbox (http://acado.github.io)
 * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
 */

#include <iDynTree/OptimizationProblem.h>
#include <iDynTree/Optimizers/IpoptInterface.h>
#include <iDynTree/Core/TestUtils.h>
#include <iDynTree/Core/Utils.h>
#include <iDynTree/Core/VectorDynSize.h>
#include <iDynTree/Core/MatrixDynSize.h>
#include <vector>
#include <memory>

class TestProblem : public iDynTree::optimization::OptimizationProblem {
    double m_a1, m_b1, m_c1;
    double m_a2, m_b2, m_c2;
    double m_plusInfinity, m_minusInfinity;
    iDynTree::VectorDynSize m_variables, m_expectedVariables;

public:
    TestProblem()
    :m_plusInfinity(1e19)
    ,m_minusInfinity(-1e19)
    {}

    virtual ~TestProblem() override {}

    virtual bool prepare() override {
        m_a1 = iDynTree::getRandomDouble(0.01, 10.0);
        m_b1 = iDynTree::getRandomDouble(-10.0, 10.0);
        m_c1 = iDynTree::getRandomDouble(-10.0, 10.0);
        m_a2 = iDynTree::getRandomDouble(0.01, 10.0);
        m_b2 = iDynTree::getRandomDouble(-10.0, 10.0);
        m_c2 = iDynTree::getRandomDouble(-10.0, 10.0);

        return true;
    }

    virtual void reset() override {}

    virtual unsigned int numberOfVariables() override {
        return 2;
    }

    virtual unsigned int numberOfConstraints() override {
        return 2;
    }

    virtual bool getGuess(iDynTree::VectorDynSize &guess) override {
        guess.resize(2);
        guess.zero();
        return true;
    }

    virtual bool getConstraintsBounds(iDynTree::VectorDynSize& constraintsLowerBounds, iDynTree::VectorDynSize& constraintsUpperBounds) override {
        constraintsLowerBounds.resize(2);
        constraintsUpperBounds.resize(2);
        constraintsLowerBounds(0) = m_c1;
        constraintsLowerBounds(1) = m_c2;
        constraintsUpperBounds(0) = m_plusInfinity;
        constraintsUpperBounds(1) = m_plusInfinity;
        return true;
    }

    virtual bool getVariablesUpperBound(iDynTree::VectorDynSize& variablesUpperBound) override {
        return false;
    } //return false if not upper bounded

    virtual bool getVariablesLowerBound(iDynTree::VectorDynSize& variablesLowerBound) override {
        return false;
    } //return false if not lower bounded

    virtual bool getConstraintsJacobianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) override {
        nonZeroElementRows.resize(2);
        nonZeroElementColumns.resize(2);

        nonZeroElementRows[0] = 0;
        nonZeroElementRows[1] = 1;

        nonZeroElementColumns = nonZeroElementRows;

        return true;
    }

    virtual bool getHessianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) override {
        nonZeroElementRows.resize(2);
        nonZeroElementColumns.resize(2);

        nonZeroElementRows[0] = 0;
        nonZeroElementRows[1] = 1;

        nonZeroElementColumns = nonZeroElementRows;

        return true;
    } //costs and constraints together

    virtual bool setVariables(const iDynTree::VectorDynSize& variables) override {
        ASSERT_IS_TRUE(variables.size()==2);
        m_variables = variables;
        return true;
    }

    virtual bool evaluateCostFunction(double& costValue) override {
        costValue = m_a1 * m_variables(0) * m_variables(0) + m_b1 * m_variables(0) + m_a2 * m_variables(1) * m_variables(1) + m_b2 * m_variables(1);
        return true;
    }

    virtual bool evaluateCostGradient(iDynTree::VectorDynSize& gradient) override {
        gradient.resize(numberOfVariables());
        gradient(0) = 2 * m_a1 * m_variables(0) + m_b1;
        gradient(1) = 2 * m_a2 * m_variables(1) + m_b2;
        return true;
    }

    virtual bool evaluateCostHessian(iDynTree::MatrixDynSize& hessian) override {
        hessian.resize(2,2);
        hessian.zero();
        hessian(0,0) = 2*m_a1;
        hessian(1,1) = 2*m_a2;
        return true;
    }

    virtual bool evaluateConstraints(iDynTree::VectorDynSize& constraints) override {
        constraints.resize(2);
        constraints = m_variables;
        return true;
    }

    virtual bool evaluateConstraintsJacobian(iDynTree::MatrixDynSize& jacobian) override {
        jacobian.resize(2,2);
        jacobian(0,0) = 1.0;
        jacobian(1,1) = 1.0;
        return true;
    } //using dense matrices, but the sparsity pattern is still obtained

    virtual bool evaluateConstraintsHessian(const iDynTree::VectorDynSize& constraintsMultipliers, iDynTree::MatrixDynSize& hessian) override {
        hessian.resize(2,2);
        hessian.zero();
        return true;
    } //using dense matrices, but the sparsity pattern is still obtained

    bool setPlusInfinity(double plusInfinity) {
        if (plusInfinity < 0)
            return false;

        m_plusInfinity = plusInfinity;
        return true;
    }

    bool setMinusInfinity(double minusInfinity) {
        if (minusInfinity > 0)
            return false;

        m_minusInfinity = minusInfinity;
        return true;
    }

    double expectedMinimum() {
        double expected;
        if (m_c1 < (-m_b1/(2*m_a1)))
            expected = - m_b1 * m_b1 / (4*m_a1);
        else
            expected = m_a1*m_c1*m_c1 + m_b1*m_c1;

        if (m_c2 < (-m_b2/(2*m_a2)))
            expected += - m_b2 * m_b2 / (4*m_a2);
        else
            expected += m_a2*m_c2*m_c2 + m_b2*m_c2;

        return expected;
    }

    const iDynTree::VectorDynSize& expectedVariables() {
        m_expectedVariables.resize(2);
        if (m_c1 < (-m_b1/(2*m_a1)))
            m_expectedVariables(0) = - m_b1 / (2*m_a1);
        else
            m_expectedVariables(0) = m_c1;

        if (m_c2 < (-m_b2/(2*m_a2)))
            m_expectedVariables(1) = - m_b2 / (2*m_a2);
        else
            m_expectedVariables(1) = m_c2;

        return m_expectedVariables;
    }
};

int main(){
    iDynTree::optimization::IpoptInterface ipoptSolver;
    std::shared_ptr<TestProblem> problem(new TestProblem);
    iDynTree::VectorDynSize guess(2), dummy1, dummy2, dummy3;
    guess.zero();

    ASSERT_IS_TRUE(ipoptSolver.setIpoptOption("nlp_lower_bound_inf", -1.0e20));
    ASSERT_IS_TRUE(ipoptSolver.setIpoptOption("print_level", 0));

    ASSERT_IS_TRUE(problem->setMinusInfinity(ipoptSolver.minusInfinity()));
    ASSERT_IS_TRUE(problem->setPlusInfinity(ipoptSolver.plusInfinity()));
    ASSERT_IS_TRUE(ipoptSolver.setProblem(problem));
    for (int i = 0; i < 5; ++i){
        ASSERT_IS_TRUE(ipoptSolver.solve());
        double optimalCost;
        ASSERT_IS_TRUE(ipoptSolver.getOptimalCost(optimalCost));
        ASSERT_EQUAL_DOUBLE_TOL(optimalCost, problem->expectedMinimum(), 1e-5);
        iDynTree::VectorDynSize solution;
        ASSERT_IS_TRUE(ipoptSolver.getPrimalVariables(solution));
        ASSERT_EQUAL_VECTOR_TOL(solution, problem->expectedVariables(), 1e-5);
        ASSERT_IS_TRUE(ipoptSolver.getDualVariables(dummy1, dummy2, dummy3));
    }
    return EXIT_SUCCESS;
}
<commit_msg>IpoptInterfaceTest: increase test tolerance<commit_after>/*
 * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
 *
 * Licensed under either the GNU Lesser General Public License v3.0 :
 * https://www.gnu.org/licenses/lgpl-3.0.html
 * or the GNU Lesser General Public License v2.1 :
 * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 * at your option.
 *
 * Originally developed for Prioritized Optimal Control (2014)
 * Refactored in 2018.
 * Design inspired by
 * - ACADO toolbox (http://acado.github.io)
 * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
 */

#include <iDynTree/OptimizationProblem.h>
#include <iDynTree/Optimizers/IpoptInterface.h>
#include <iDynTree/Core/TestUtils.h>
#include <iDynTree/Core/Utils.h>
#include <iDynTree/Core/VectorDynSize.h>
#include <iDynTree/Core/MatrixDynSize.h>
#include <vector>
#include <memory>

class TestProblem : public iDynTree::optimization::OptimizationProblem {
    double m_a1, m_b1, m_c1;
    double m_a2, m_b2, m_c2;
    double m_plusInfinity, m_minusInfinity;
    iDynTree::VectorDynSize m_variables, m_expectedVariables;

public:
    TestProblem()
    :m_plusInfinity(1e19)
    ,m_minusInfinity(-1e19)
    {}

    virtual ~TestProblem() override {}

    virtual bool prepare() override {
        m_a1 = iDynTree::getRandomDouble(0.01, 10.0);
        m_b1 = iDynTree::getRandomDouble(-10.0, 10.0);
        m_c1 = iDynTree::getRandomDouble(-10.0, 10.0);
        m_a2 = iDynTree::getRandomDouble(0.01, 10.0);
        m_b2 = iDynTree::getRandomDouble(-10.0, 10.0);
        m_c2 = iDynTree::getRandomDouble(-10.0, 10.0);

        return true;
    }

    virtual void reset() override {}

    virtual unsigned int numberOfVariables() override {
        return 2;
    }

    virtual unsigned int numberOfConstraints() override {
        return 2;
    }

    virtual bool getGuess(iDynTree::VectorDynSize &guess) override {
        guess.resize(2);
        guess.zero();
        return true;
    }

    virtual bool getConstraintsBounds(iDynTree::VectorDynSize& constraintsLowerBounds, iDynTree::VectorDynSize& constraintsUpperBounds) override {
        constraintsLowerBounds.resize(2);
        constraintsUpperBounds.resize(2);
        constraintsLowerBounds(0) = m_c1;
        constraintsLowerBounds(1) = m_c2;
        constraintsUpperBounds(0) = m_plusInfinity;
        constraintsUpperBounds(1) = m_plusInfinity;
        return true;
    }

    virtual bool getVariablesUpperBound(iDynTree::VectorDynSize& variablesUpperBound) override {
        return false;
    } //return false if not upper bounded

    virtual bool getVariablesLowerBound(iDynTree::VectorDynSize& variablesLowerBound) override {
        return false;
    } //return false if not lower bounded

    virtual bool getConstraintsJacobianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) override {
        nonZeroElementRows.resize(2);
        nonZeroElementColumns.resize(2);

        nonZeroElementRows[0] = 0;
        nonZeroElementRows[1] = 1;

        nonZeroElementColumns = nonZeroElementRows;

        return true;
    }

    virtual bool getHessianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) override {
        nonZeroElementRows.resize(2);
        nonZeroElementColumns.resize(2);

        nonZeroElementRows[0] = 0;
        nonZeroElementRows[1] = 1;

        nonZeroElementColumns = nonZeroElementRows;

        return true;
    } //costs and constraints together

    virtual bool setVariables(const iDynTree::VectorDynSize& variables) override {
        ASSERT_IS_TRUE(variables.size()==2);
        m_variables = variables;
        return true;
    }

    virtual bool evaluateCostFunction(double& costValue) override {
        costValue = m_a1 * m_variables(0) * m_variables(0) + m_b1 * m_variables(0) + m_a2 * m_variables(1) * m_variables(1) + m_b2 * m_variables(1);
        return true;
    }

    virtual bool evaluateCostGradient(iDynTree::VectorDynSize& gradient) override {
        gradient.resize(numberOfVariables());
        gradient(0) = 2 * m_a1 * m_variables(0) + m_b1;
        gradient(1) = 2 * m_a2 * m_variables(1) + m_b2;
        return true;
    }

    virtual bool evaluateCostHessian(iDynTree::MatrixDynSize& hessian) override {
        hessian.resize(2,2);
        hessian.zero();
        hessian(0,0) = 2*m_a1;
        hessian(1,1) = 2*m_a2;
        return true;
    }

    virtual bool evaluateConstraints(iDynTree::VectorDynSize& constraints) override {
        constraints.resize(2);
        constraints = m_variables;
        return true;
    }

    virtual bool evaluateConstraintsJacobian(iDynTree::MatrixDynSize& jacobian) override {
        jacobian.resize(2,2);
        jacobian(0,0) = 1.0;
        jacobian(1,1) = 1.0;
        return true;
    } //using dense matrices, but the sparsity pattern is still obtained

    virtual bool evaluateConstraintsHessian(const iDynTree::VectorDynSize& constraintsMultipliers, iDynTree::MatrixDynSize& hessian) override {
        hessian.resize(2,2);
        hessian.zero();
        return true;
    } //using dense matrices, but the sparsity pattern is still obtained

    bool setPlusInfinity(double plusInfinity) {
        if (plusInfinity < 0)
            return false;

        m_plusInfinity = plusInfinity;
        return true;
    }

    bool setMinusInfinity(double minusInfinity) {
        if (minusInfinity > 0)
            return false;

        m_minusInfinity = minusInfinity;
        return true;
    }

    double expectedMinimum() {
        double expected;
        if (m_c1 < (-m_b1/(2*m_a1)))
            expected = - m_b1 * m_b1 / (4*m_a1);
        else
            expected = m_a1*m_c1*m_c1 + m_b1*m_c1;

        if (m_c2 < (-m_b2/(2*m_a2)))
            expected += - m_b2 * m_b2 / (4*m_a2);
        else
            expected += m_a2*m_c2*m_c2 + m_b2*m_c2;

        return expected;
    }

    const iDynTree::VectorDynSize& expectedVariables() {
        m_expectedVariables.resize(2);
        if (m_c1 < (-m_b1/(2*m_a1)))
            m_expectedVariables(0) = - m_b1 / (2*m_a1);
        else
            m_expectedVariables(0) = m_c1;

        if (m_c2 < (-m_b2/(2*m_a2)))
            m_expectedVariables(1) = - m_b2 / (2*m_a2);
        else
            m_expectedVariables(1) = m_c2;

        return m_expectedVariables;
    }
};

int main(){
    iDynTree::optimization::IpoptInterface ipoptSolver;
    std::shared_ptr<TestProblem> problem(new TestProblem);
    iDynTree::VectorDynSize guess(2), dummy1, dummy2, dummy3;
    guess.zero();

    ASSERT_IS_TRUE(ipoptSolver.setIpoptOption("nlp_lower_bound_inf", -1.0e20));
    ASSERT_IS_TRUE(ipoptSolver.setIpoptOption("print_level", 0));

    ASSERT_IS_TRUE(problem->setMinusInfinity(ipoptSolver.minusInfinity()));
    ASSERT_IS_TRUE(problem->setPlusInfinity(ipoptSolver.plusInfinity()));
    ASSERT_IS_TRUE(ipoptSolver.setProblem(problem));
    double testTolerance = 5e-5;
    for (int i = 0; i < 5; ++i){
        ASSERT_IS_TRUE(ipoptSolver.solve());
        double optimalCost;
        ASSERT_IS_TRUE(ipoptSolver.getOptimalCost(optimalCost));
        ASSERT_EQUAL_DOUBLE_TOL(optimalCost, problem->expectedMinimum(), testTolerance);
        iDynTree::VectorDynSize solution;
        ASSERT_IS_TRUE(ipoptSolver.getPrimalVariables(solution));
        ASSERT_EQUAL_VECTOR_TOL(solution, problem->expectedVariables(), testTolerance);
        ASSERT_IS_TRUE(ipoptSolver.getDualVariables(dummy1, dummy2, dummy3));
    }
    return EXIT_SUCCESS;
}
<|endoftext|>


<commit_before>// Copyright 2012-2016 CNRS-UM LIRMM, CNRS-AIST JRL
//
// This file is part of Tasks.
//
// Tasks 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.
//
// Tasks 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 Tasks.  If not, see <http://www.gnu.org/licenses/>.

// associated header
#include "LSSOLQPSolver.h"

// includes
// Tasks
#include "GenQPUtils.h"
#include "Tasks/QPSolver.h"

namespace tasks
{

namespace qp
{

LSSOLQPSolver::LSSOLQPSolver():
	lssol_(),
	A_(),AL_(),AU_(),
	AFull_(),
	XL_(),XU_(),
	XLFull_(),XUFull_(),
	Q_(),C_(),
	QFull_(),CFull_(),
	XFull_(),
	nrALines_(0)
{
	lssol_.warm(true);
	lssol_.feasibilityTol(1e-6);
}

void LSSOLQPSolver::updateSize(int nrVars, int nrEq, int nrInEq, int nrGenInEq)
{
	int maxALines = nrEq + nrInEq + nrGenInEq;
	AFull_.resize(maxALines, nrVars);
	AL_.resize(maxALines);
	AU_.resize(maxALines);

	XLFull_.resize(nrVars);
	XUFull_.resize(nrVars);

	QFull_.resize(nrVars, nrVars);
	CFull_.resize(nrVars);

	if(dependencies_.size())
	{
		int nrReducedVars = nrVars - static_cast<int>(dependencies_.size());

		A_.resize(maxALines, nrReducedVars);
		Q_.resize(nrReducedVars, nrReducedVars);
		C_.resize(nrReducedVars);

		XL_.resize(nrReducedVars);
		XU_.resize(nrReducedVars);
		XFull_.resize(nrVars);

		lssol_.problem(nrReducedVars, maxALines);
	}
	else
	{
		lssol_.problem(nrVars, maxALines);
	}
}

void LSSOLQPSolver::updateMatrix(
	const std::vector<Task*>& tasks,
	const std::vector<Equality*>& eqConstr,
	const std::vector<Inequality*>& inEqConstr,
	const std::vector<GenInequality*>& genInEqConstr,
	const std::vector<Bound*>& boundConstr)
{
	AFull_.setZero();
	AL_.setZero();
	AU_.setZero();
	XLFull_.fill(-std::numeric_limits<double>::infinity());
	XUFull_.fill(std::numeric_limits<double>::infinity());
	QFull_.setZero();
	CFull_.setZero();

	const int nrVars = int(QFull_.rows());

	nrALines_ = 0;
	nrALines_ = fillEq(eqConstr, nrVars, nrALines_, AFull_, AL_, AU_);
	nrALines_ = fillInEq(inEqConstr, nrVars, nrALines_, AFull_, AL_, AU_);
	nrALines_ = fillGenInEq(genInEqConstr, nrVars, nrALines_, AFull_, AL_, AU_);

	fillBound(boundConstr, XLFull_, XUFull_);
	fillQC(tasks, nrVars, QFull_, CFull_);

	if(dependencies_.size())
	{
		reduceA(AFull_, A_, fullToReduced_, reducedToFull_, dependencies_);
		reduceBound(XLFull_, XL_, XUFull_, XU_, fullToReduced_, reducedToFull_, dependencies_);
		reduceQC(QFull_, CFull_, Q_, C_, fullToReduced_, reducedToFull_, dependencies_);
	}
}

bool LSSOLQPSolver::solve()
{
	bool success = false;
	if(dependencies_.size())
	{
		success = lssol_.solve(Q_, C_,
			A_.block(0, 0, nrALines_, int(A_.cols())), int(A_.rows()),
			AL_.segment(0, nrALines_), AU_.segment(0, nrALines_), XL_, XU_);
		expandResult(lssol_.result(), XFull_,
									reducedToFull_,
									dependencies_);
	}
	else
	{
		success = lssol_.solve(QFull_, CFull_,
			AFull_.block(0, 0, nrALines_, int(AFull_.cols())), int(AFull_.rows()),
			AL_.segment(0, nrALines_), AU_.segment(0, nrALines_), XLFull_, XUFull_);
	}
	return success;
}

const Eigen::VectorXd& LSSOLQPSolver::result() const
{
	if(dependencies_.size())
	{
		return XFull_;
	}
	else
	{
		return lssol_.result();
	}
}

std::ostream& LSSOLQPSolver::errorMsg(
	const std::vector<rbd::MultiBody>& mbs,
	const std::vector<Task*>& /* tasks */,
	const std::vector<Equality*>& eqConstr,
	const std::vector<Inequality*>& inEqConstr,
	const std::vector<GenInequality*>& genInEqConstr,
	const std::vector<Bound*>& boundConstr,
	std::ostream& out) const
{
	const int nrVars = int(Q_.rows());

	out << "lssol output: " << lssol_.fail() << std::endl;
	const Eigen::VectorXi& istate = lssol_.istate();
	// check bound constraint
	for(int i = 0; i < nrVars; ++i)
	{
		if(istate(i) < 0)
		{
			for(Bound* b: boundConstr)
			{
				int start = b->beginVar();
				int end = start + int(b->Lower().rows());
				if(i >= start && i < end)
				{
					int line = i - start;
					out << b->nameBound() << " violated at line: " << line << std::endl;
					out << b->descBound(mbs, line) << std::endl;
					out << XL_(i) << " <= " << lssol_.result()(i) << " <= " << XU_(i) << std::endl;
					break;
				}
			}
		}
	}

	// check inequality constraint
	for(int i = 0; i < nrALines_; ++i)
	{
		int iInIstate = i + nrVars;
		if(istate(iInIstate) < 0)
		{
			int start = 0;
			int end = 0;

			constrErrorMsg(mbs, lssol_.result(), i, eqConstr, start, end, out);
			constrErrorMsg(mbs, lssol_.result(), i, inEqConstr, start, end, out);
			constrErrorMsg(mbs, lssol_.result(), i, genInEqConstr, start, end, out);
			out << std::endl;
		}
	}
	return out;
}

} // namespace qp

} // namespace tasks
<commit_msg>add more information to the printed error code for LSSOL backend (#32)<commit_after>// Copyright 2012-2016 CNRS-UM LIRMM, CNRS-AIST JRL
//
// This file is part of Tasks.
//
// Tasks 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.
//
// Tasks 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 Tasks.  If not, see <http://www.gnu.org/licenses/>.

// associated header
#include "LSSOLQPSolver.h"

// includes
// Tasks
#include "GenQPUtils.h"
#include "Tasks/QPSolver.h"

namespace tasks
{

namespace qp
{

LSSOLQPSolver::LSSOLQPSolver():
	lssol_(),
	A_(),AL_(),AU_(),
	AFull_(),
	XL_(),XU_(),
	XLFull_(),XUFull_(),
	Q_(),C_(),
	QFull_(),CFull_(),
	XFull_(),
	nrALines_(0)
{
	lssol_.warm(true);
	lssol_.feasibilityTol(1e-6);
}

void LSSOLQPSolver::updateSize(int nrVars, int nrEq, int nrInEq, int nrGenInEq)
{
	int maxALines = nrEq + nrInEq + nrGenInEq;
	AFull_.resize(maxALines, nrVars);
	AL_.resize(maxALines);
	AU_.resize(maxALines);

	XLFull_.resize(nrVars);
	XUFull_.resize(nrVars);

	QFull_.resize(nrVars, nrVars);
	CFull_.resize(nrVars);

	if(dependencies_.size())
	{
		int nrReducedVars = nrVars - static_cast<int>(dependencies_.size());

		A_.resize(maxALines, nrReducedVars);
		Q_.resize(nrReducedVars, nrReducedVars);
		C_.resize(nrReducedVars);

		XL_.resize(nrReducedVars);
		XU_.resize(nrReducedVars);
		XFull_.resize(nrVars);

		lssol_.problem(nrReducedVars, maxALines);
	}
	else
	{
		lssol_.problem(nrVars, maxALines);
	}
}

void LSSOLQPSolver::updateMatrix(
	const std::vector<Task*>& tasks,
	const std::vector<Equality*>& eqConstr,
	const std::vector<Inequality*>& inEqConstr,
	const std::vector<GenInequality*>& genInEqConstr,
	const std::vector<Bound*>& boundConstr)
{
	AFull_.setZero();
	AL_.setZero();
	AU_.setZero();
	XLFull_.fill(-std::numeric_limits<double>::infinity());
	XUFull_.fill(std::numeric_limits<double>::infinity());
	QFull_.setZero();
	CFull_.setZero();

	const int nrVars = int(QFull_.rows());

	nrALines_ = 0;
	nrALines_ = fillEq(eqConstr, nrVars, nrALines_, AFull_, AL_, AU_);
	nrALines_ = fillInEq(inEqConstr, nrVars, nrALines_, AFull_, AL_, AU_);
	nrALines_ = fillGenInEq(genInEqConstr, nrVars, nrALines_, AFull_, AL_, AU_);

	fillBound(boundConstr, XLFull_, XUFull_);
	fillQC(tasks, nrVars, QFull_, CFull_);

	if(dependencies_.size())
	{
		reduceA(AFull_, A_, fullToReduced_, reducedToFull_, dependencies_);
		reduceBound(XLFull_, XL_, XUFull_, XU_, fullToReduced_, reducedToFull_, dependencies_);
		reduceQC(QFull_, CFull_, Q_, C_, fullToReduced_, reducedToFull_, dependencies_);
	}
}

bool LSSOLQPSolver::solve()
{
	bool success = false;
	if(dependencies_.size())
	{
		success = lssol_.solve(Q_, C_,
			A_.block(0, 0, nrALines_, int(A_.cols())), int(A_.rows()),
			AL_.segment(0, nrALines_), AU_.segment(0, nrALines_), XL_, XU_);
		expandResult(lssol_.result(), XFull_,
									reducedToFull_,
									dependencies_);
	}
	else
	{
		success = lssol_.solve(QFull_, CFull_,
			AFull_.block(0, 0, nrALines_, int(AFull_.cols())), int(AFull_.rows()),
			AL_.segment(0, nrALines_), AU_.segment(0, nrALines_), XLFull_, XUFull_);
	}
	return success;
}

const Eigen::VectorXd& LSSOLQPSolver::result() const
{
	if(dependencies_.size())
	{
		return XFull_;
	}
	else
	{
		return lssol_.result();
	}
}

std::ostream& LSSOLQPSolver::errorMsg(
	const std::vector<rbd::MultiBody>& mbs,
	const std::vector<Task*>& /* tasks */,
	const std::vector<Equality*>& eqConstr,
	const std::vector<Inequality*>& inEqConstr,
	const std::vector<GenInequality*>& genInEqConstr,
	const std::vector<Bound*>& boundConstr,
	std::ostream& out) const
{
	const int nrVars = int(Q_.rows());

	out << "lssol output (" << lssol_.fail() << "): ";
	out << std::endl;
	lssol_.inform(out);

	const Eigen::VectorXi& istate = lssol_.istate();
	// check bound constraint
	for(int i = 0; i < nrVars; ++i)
	{
		if(istate(i) < 0)
		{
			for(Bound* b: boundConstr)
			{
				int start = b->beginVar();
				int end = start + int(b->Lower().rows());
				if(i >= start && i < end)
				{
					int line = i - start;
					out << b->nameBound() << " violated at line: " << line << std::endl;
					out << b->descBound(mbs, line) << std::endl;
					out << XL_(i) << " <= " << lssol_.result()(i) << " <= " << XU_(i) << std::endl;
					break;
				}
			}
		}
	}

	// check inequality constraint
	for(int i = 0; i < nrALines_; ++i)
	{
		int iInIstate = i + nrVars;
		if(istate(iInIstate) < 0)
		{
			int start = 0;
			int end = 0;

			constrErrorMsg(mbs, lssol_.result(), i, eqConstr, start, end, out);
			constrErrorMsg(mbs, lssol_.result(), i, inEqConstr, start, end, out);
			constrErrorMsg(mbs, lssol_.result(), i, genInEqConstr, start, end, out);
			out << std::endl;
		}
	}
	return out;
}

} // namespace qp

} // namespace tasks
<|endoftext|>


<commit_before>/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// associated header
#include "LSSOLQPSolver.h"

// includes
// Tasks
#include "GenQPUtils.h"
#include "Tasks/QPSolver.h"

namespace tasks
{

namespace qp
{

LSSOLQPSolver::LSSOLQPSolver()
: lssol_(), A_(), AL_(), AU_(), AFull_(), XL_(), XU_(), XLFull_(), XUFull_(), Q_(), C_(), QFull_(), CFull_(), XFull_(),
  nrALines_(0)
{
  lssol_.warm(true);
  lssol_.feasibilityTol(1e-6);
  lssol_.persistance(true);
  lssol_.forceMinSize(false);
  lssol_.feasibilityMaxIter(4 * lssol_.feasibilityMaxIter());
  lssol_.optimalityMaxIter(4 * lssol_.optimalityMaxIter());
}

void LSSOLQPSolver::updateSize(int nrVars, int nrEq, int nrInEq, int nrGenInEq)
{
  int maxALines = nrEq + nrInEq + nrGenInEq;
  AFull_.resize(maxALines, nrVars);
  AL_.resize(maxALines);
  AU_.resize(maxALines);

  XLFull_.resize(nrVars);
  XUFull_.resize(nrVars);

  QFull_.resize(nrVars, nrVars);
  CFull_.resize(nrVars);

  if(dependencies_.size())
  {
    int nrReducedVars = nrVars - static_cast<int>(dependencies_.size());

    A_.resize(maxALines, nrReducedVars);
    Q_.resize(nrReducedVars, nrReducedVars);
    C_.resize(nrReducedVars);

    XL_.resize(nrReducedVars);
    XU_.resize(nrReducedVars);
    XFull_.resize(nrVars);

    lssol_.resize(nrReducedVars, maxALines, Eigen::lssol::QP2);
  }
  else
  {
    lssol_.resize(nrVars, maxALines, Eigen::lssol::QP2);
  }
}

void LSSOLQPSolver::updateMatrix(const std::vector<Task *> & tasks,
                                 const std::vector<Equality *> & eqConstr,
                                 const std::vector<Inequality *> & inEqConstr,
                                 const std::vector<GenInequality *> & genInEqConstr,
                                 const std::vector<Bound *> & boundConstr)
{
  AFull_.setZero();
  AL_.setZero();
  AU_.setZero();
  XLFull_.fill(-std::numeric_limits<double>::infinity());
  XUFull_.fill(std::numeric_limits<double>::infinity());
  QFull_.setZero();
  CFull_.setZero();

  const int nrVars = int(QFull_.rows());

  nrALines_ = 0;
  nrALines_ = fillEq(eqConstr, nrVars, nrALines_, AFull_, AL_, AU_);
  nrALines_ = fillInEq(inEqConstr, nrVars, nrALines_, AFull_, AL_, AU_);
  nrALines_ = fillGenInEq(genInEqConstr, nrVars, nrALines_, AFull_, AL_, AU_);

  fillBound(boundConstr, XLFull_, XUFull_);
  fillQC(tasks, nrVars, QFull_, CFull_);

  if(dependencies_.size())
  {
    reduceA(AFull_, A_, fullToReduced_, reducedToFull_, dependencies_);
    reduceBound(XLFull_, XL_, XUFull_, XU_, fullToReduced_, reducedToFull_, dependencies_);
    reduceQC(QFull_, CFull_, Q_, C_, fullToReduced_, reducedToFull_, dependencies_);
  }
}

bool LSSOLQPSolver::solve()
{
  bool success = false;
  if(dependencies_.size())
  {
    success = lssol_.solve(XL_, XU_, Q_, C_, A_.block(0, 0, nrALines_, A_.cols()), AL_.segment(0, nrALines_),
                           AU_.segment(0, nrALines_));
    expandResult(lssol_.result(), XFull_, reducedToFull_, dependencies_);
  }
  else
  {
    success = lssol_.solve(XLFull_, XUFull_, QFull_, CFull_, AFull_.block(0, 0, nrALines_, AFull_.cols()),
                           AL_.segment(0, nrALines_), AU_.segment(0, nrALines_));
  }
  return success;
}

const Eigen::VectorXd & LSSOLQPSolver::result() const
{
  if(dependencies_.size())
  {
    return XFull_;
  }
  else
  {
    return lssol_.result();
  }
}

std::ostream & LSSOLQPSolver::errorMsg(const std::vector<rbd::MultiBody> & mbs,
                                       const std::vector<Task *> & /* tasks */,
                                       const std::vector<Equality *> & eqConstr,
                                       const std::vector<Inequality *> & inEqConstr,
                                       const std::vector<GenInequality *> & genInEqConstr,
                                       const std::vector<Bound *> & boundConstr,
                                       std::ostream & out) const
{
  const int nrVars = int(Q_.rows());

  out << "lssol output (" << lssol_.inform() << "): ";
  out << std::endl;
  lssol_.inform(out);

  const Eigen::VectorXi & istate = lssol_.istate();
  // check bound constraint
  for(int i = 0; i < nrVars; ++i)
  {
    if(istate(i) < 0)
    {
      for(Bound * b : boundConstr)
      {
        int start = b->beginVar();
        int end = start + int(b->Lower().rows());
        if(i >= start && i < end)
        {
          int line = i - start;
          out << b->nameBound() << " violated at line: " << line << std::endl;
          out << b->descBound(mbs, line) << std::endl;
          out << XL_(i) << " <= " << lssol_.result()(i) << " <= " << XU_(i) << std::endl;
          break;
        }
      }
    }
  }

  // check inequality constraint
  for(int i = 0; i < nrALines_; ++i)
  {
    int iInIstate = i + nrVars;
    if(istate(iInIstate) < 0)
    {
      int start = 0;
      int end = 0;

      constrErrorMsg(mbs, lssol_.result(), i, eqConstr, start, end, out);
      constrErrorMsg(mbs, lssol_.result(), i, inEqConstr, start, end, out);
      constrErrorMsg(mbs, lssol_.result(), i, genInEqConstr, start, end, out);
      out << std::endl;
    }
  }
  return out;
}

std::string LSSOLQPSolver::name() const
{
  return "LSSOL";
}

} // namespace qp

} // namespace tasks
<commit_msg>Follow https://gite.lirmm.fr/multi-contact/eigen-lssol/merge_requests/10<commit_after>/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// associated header
#include "LSSOLQPSolver.h"

// includes
// Tasks
#include "GenQPUtils.h"
#include "Tasks/QPSolver.h"

namespace tasks
{

namespace qp
{

LSSOLQPSolver::LSSOLQPSolver()
: lssol_(), A_(), AL_(), AU_(), AFull_(), XL_(), XU_(), XLFull_(), XUFull_(), Q_(), C_(), QFull_(), CFull_(), XFull_(),
  nrALines_(0)
{
  lssol_.warm(true);
  lssol_.feasibilityTol(1e-6);
  lssol_.persistence(true);
  lssol_.forceMinSize(false);
  lssol_.feasibilityMaxIter(4 * lssol_.feasibilityMaxIter());
  lssol_.optimalityMaxIter(4 * lssol_.optimalityMaxIter());
}

void LSSOLQPSolver::updateSize(int nrVars, int nrEq, int nrInEq, int nrGenInEq)
{
  int maxALines = nrEq + nrInEq + nrGenInEq;
  AFull_.resize(maxALines, nrVars);
  AL_.resize(maxALines);
  AU_.resize(maxALines);

  XLFull_.resize(nrVars);
  XUFull_.resize(nrVars);

  QFull_.resize(nrVars, nrVars);
  CFull_.resize(nrVars);

  if(dependencies_.size())
  {
    int nrReducedVars = nrVars - static_cast<int>(dependencies_.size());

    A_.resize(maxALines, nrReducedVars);
    Q_.resize(nrReducedVars, nrReducedVars);
    C_.resize(nrReducedVars);

    XL_.resize(nrReducedVars);
    XU_.resize(nrReducedVars);
    XFull_.resize(nrVars);

    lssol_.resize(nrReducedVars, maxALines, Eigen::lssol::QP2);
  }
  else
  {
    lssol_.resize(nrVars, maxALines, Eigen::lssol::QP2);
  }
}

void LSSOLQPSolver::updateMatrix(const std::vector<Task *> & tasks,
                                 const std::vector<Equality *> & eqConstr,
                                 const std::vector<Inequality *> & inEqConstr,
                                 const std::vector<GenInequality *> & genInEqConstr,
                                 const std::vector<Bound *> & boundConstr)
{
  AFull_.setZero();
  AL_.setZero();
  AU_.setZero();
  XLFull_.fill(-std::numeric_limits<double>::infinity());
  XUFull_.fill(std::numeric_limits<double>::infinity());
  QFull_.setZero();
  CFull_.setZero();

  const int nrVars = int(QFull_.rows());

  nrALines_ = 0;
  nrALines_ = fillEq(eqConstr, nrVars, nrALines_, AFull_, AL_, AU_);
  nrALines_ = fillInEq(inEqConstr, nrVars, nrALines_, AFull_, AL_, AU_);
  nrALines_ = fillGenInEq(genInEqConstr, nrVars, nrALines_, AFull_, AL_, AU_);

  fillBound(boundConstr, XLFull_, XUFull_);
  fillQC(tasks, nrVars, QFull_, CFull_);

  if(dependencies_.size())
  {
    reduceA(AFull_, A_, fullToReduced_, reducedToFull_, dependencies_);
    reduceBound(XLFull_, XL_, XUFull_, XU_, fullToReduced_, reducedToFull_, dependencies_);
    reduceQC(QFull_, CFull_, Q_, C_, fullToReduced_, reducedToFull_, dependencies_);
  }
}

bool LSSOLQPSolver::solve()
{
  bool success = false;
  if(dependencies_.size())
  {
    success = lssol_.solve(XL_, XU_, Q_, C_, A_.block(0, 0, nrALines_, A_.cols()), AL_.segment(0, nrALines_),
                           AU_.segment(0, nrALines_));
    expandResult(lssol_.result(), XFull_, reducedToFull_, dependencies_);
  }
  else
  {
    success = lssol_.solve(XLFull_, XUFull_, QFull_, CFull_, AFull_.block(0, 0, nrALines_, AFull_.cols()),
                           AL_.segment(0, nrALines_), AU_.segment(0, nrALines_));
  }
  return success;
}

const Eigen::VectorXd & LSSOLQPSolver::result() const
{
  if(dependencies_.size())
  {
    return XFull_;
  }
  else
  {
    return lssol_.result();
  }
}

std::ostream & LSSOLQPSolver::errorMsg(const std::vector<rbd::MultiBody> & mbs,
                                       const std::vector<Task *> & /* tasks */,
                                       const std::vector<Equality *> & eqConstr,
                                       const std::vector<Inequality *> & inEqConstr,
                                       const std::vector<GenInequality *> & genInEqConstr,
                                       const std::vector<Bound *> & boundConstr,
                                       std::ostream & out) const
{
  const int nrVars = int(Q_.rows());

  out << "lssol output (" << lssol_.inform() << "): ";
  out << std::endl;
  lssol_.inform(out);

  const Eigen::VectorXi & istate = lssol_.istate();
  // check bound constraint
  for(int i = 0; i < nrVars; ++i)
  {
    if(istate(i) < 0)
    {
      for(Bound * b : boundConstr)
      {
        int start = b->beginVar();
        int end = start + int(b->Lower().rows());
        if(i >= start && i < end)
        {
          int line = i - start;
          out << b->nameBound() << " violated at line: " << line << std::endl;
          out << b->descBound(mbs, line) << std::endl;
          out << XL_(i) << " <= " << lssol_.result()(i) << " <= " << XU_(i) << std::endl;
          break;
        }
      }
    }
  }

  // check inequality constraint
  for(int i = 0; i < nrALines_; ++i)
  {
    int iInIstate = i + nrVars;
    if(istate(iInIstate) < 0)
    {
      int start = 0;
      int end = 0;

      constrErrorMsg(mbs, lssol_.result(), i, eqConstr, start, end, out);
      constrErrorMsg(mbs, lssol_.result(), i, inEqConstr, start, end, out);
      constrErrorMsg(mbs, lssol_.result(), i, genInEqConstr, start, end, out);
      out << std::endl;
    }
  }
  return out;
}

std::string LSSOLQPSolver::name() const
{
  return "LSSOL";
}

} // namespace qp

} // namespace tasks
<|endoftext|>


<commit_before>#ifndef _BINARY_SEARCH_TREE_HPP_
#define _BINARY_SEARCH_TREE_HPP_

template<class T>
class BST : public BinaryTree<T>
{

};

#endif<commit_msg>Binary Search Tree definition<commit_after>#ifndef _BINARY_SEARCH_TREE_HPP_
#define _BINARY_SEARCH_TREE_HPP_

template<class T>
class BST : public BinaryTree<T>
{
    void insert(T);
    bool search(T) const;
    void del(T);
};

#endif
<|endoftext|>


<commit_before>#include "msp430lib/sys/v1/MSP430_System.hpp"
#include <msp430.h>

namespace {
  volatile uint32_t milliseconds_since_start = 0;
}

#pragma vector=TIMER0_A0_VECTOR
__interrupt void systickTimerInterrupt(void)
{
  milliseconds_since_start++;
}

#define USE_ASM_SPINNING    1
//--------------------------------------------------------------------------------------------------
#if(USE_ASM_SPINNING == 1)
#if defined(__GNUC__) && defined(__MSP430__)
// MSPGCC
#define ARG32_MSB    "R15"
#define ARG32_LSB    "R14"

#elif defined(__TI_COMPILER_VERSION__)
// TI's Code Composer Studio
#define ARG32_MSB    "R13"
#define ARG32_LSB    "R12"

#else
#error "Compiler not supported yet."
#endif

static void asmspin(uint32_t count){
  // count SHOULD always be located at R13:R12 when compiling with CCS
  // count SHOULD always be located at R15:R14 when compiling with MSPGCC

  // Loop until count == 0 (8 cycles/loop)
  __asm__("  nop             ");        // 1 // Loop1:
  __asm__("  nop             ");        // 1
  __asm__("  nop             ");        // 1
  __asm__("  nop             ");        // 1 // Loop2:
  __asm__("  dec.w " ARG32_LSB);        // 1
  __asm__("  sbc.w " ARG32_MSB);        // 1
  __asm__("  jnz $-12        ");        // 2 // jnz Loop1
  __asm__("  tst.w " ARG32_LSB);        // 1
  __asm__("  jnz $-10        ");        // 2 // jnz Loop2
}
#endif

//--------------------------------------------------------------------------------------------------

namespace msp430lib {
namespace v1 {

auto MSP430_System::delay_us(register uint16_t us) const -> void
{
  while (us--)
  {
    __delay_cycles( F_CPU / 1000000UL );
  }
}

void MSP430_System::init()
{
  // Setup the Systick Timer
  milliseconds_since_start = 0;

  // TASSEL2 - SMCLK
  // ID3 - DIVIDER 8
  TA0CTL = TASSEL_2;
  TA0CCTL0 = CCIE;
  TA0CCR0  = F_CPU / 1000 / 2; /* Interrupt only once per period */
  TA0CTL |= MC_3;
}

auto MSP430_System::millisecondsSinceStart() const -> uint32_t
{
  return milliseconds_since_start;
}

auto MSP430_System::delay_ms(register uint16_t ms) const -> void
{
  uint32_t wait_until = millisecondsSinceStart() + ms;
  while (millisecondsSinceStart() < wait_until)
  {
    // TODO: POWER DOWN MODE?
  }
}
}
}

<commit_msg>SysTick Timer Interrupt exits LPM1 now<commit_after>#include "msp430lib/sys/v1/MSP430_System.hpp"
#include <msp430.h>

namespace {
  volatile uint32_t milliseconds_since_start = 0;
}

#pragma vector=TIMER0_A0_VECTOR
__interrupt void systickTimerInterrupt(void)
{
  milliseconds_since_start++;
  LPM1_EXIT;
}

#define USE_ASM_SPINNING    1
//--------------------------------------------------------------------------------------------------
#if(USE_ASM_SPINNING == 1)
#if defined(__GNUC__) && defined(__MSP430__)
// MSPGCC
#define ARG32_MSB    "R15"
#define ARG32_LSB    "R14"

#elif defined(__TI_COMPILER_VERSION__)
// TI's Code Composer Studio
#define ARG32_MSB    "R13"
#define ARG32_LSB    "R12"

#else
#error "Compiler not supported yet."
#endif

static void asmspin(uint32_t count){
  // count SHOULD always be located at R13:R12 when compiling with CCS
  // count SHOULD always be located at R15:R14 when compiling with MSPGCC

  // Loop until count == 0 (8 cycles/loop)
  __asm__("  nop             ");        // 1 // Loop1:
  __asm__("  nop             ");        // 1
  __asm__("  nop             ");        // 1
  __asm__("  nop             ");        // 1 // Loop2:
  __asm__("  dec.w " ARG32_LSB);        // 1
  __asm__("  sbc.w " ARG32_MSB);        // 1
  __asm__("  jnz $-12        ");        // 2 // jnz Loop1
  __asm__("  tst.w " ARG32_LSB);        // 1
  __asm__("  jnz $-10        ");        // 2 // jnz Loop2
}
#endif

//--------------------------------------------------------------------------------------------------

namespace msp430lib {
namespace v1 {

auto MSP430_System::delay_us(register uint16_t us) const -> void
{
  while (us--)
  {
    __delay_cycles( F_CPU / 1000000UL );
  }
}

void MSP430_System::init()
{
  // Setup the Systick Timer
  milliseconds_since_start = 0;

  // TASSEL2 - SMCLK
  // ID3 - DIVIDER 8
  TA0CTL = TASSEL_2;
  TA0CCTL0 = CCIE;
  TA0CCR0  = F_CPU / 1000 / 2; /* Interrupt only once per period */
  TA0CTL |= MC_3;
}

auto MSP430_System::millisecondsSinceStart() const -> uint32_t
{
  return milliseconds_since_start;
}

auto MSP430_System::delay_ms(register uint16_t ms) const -> void
{
  uint32_t wait_until = millisecondsSinceStart() + ms;
  while (millisecondsSinceStart() < wait_until)
  {
    // TODO: POWER DOWN MODE?
  }
}
}
}

<|endoftext|>


<commit_before><commit_msg>Add missing function for GCC/C++ support<commit_after><|endoftext|>


<commit_before><commit_msg>Rolled back changes in version 1.21 as SalBitmap needs to handle GWorld's with varying bit counts<commit_after><|endoftext|>


<commit_before>#ifndef PLOT_HPP
#define PLOT_HPP

#include <vector>
#include "Legend.hpp"
#include "Color.hpp"
#include "Delegate.hpp"

namespace hpl
{
class Plot
{

public:
    Plot();
    virtual ~Plot();

    virtual inline std::vector<PlotPart*> getPlotParts() {
        return parts;
    }

    virtual void init(GLuint lineprogram, GLuint textprogram);
    virtual void destroy();
    virtual void draw(float const* mvp);

    virtual void addLegend(Legend* legend);
    Legend* getLegend();

    virtual void setLegendColor(const Color& c);

    Delegate<> changed;

protected:
    Legend* legend;
    std::vector<PlotPart*> parts;
};
}

#endif // PLOT_HPP
<commit_msg>Forgot to commit half<commit_after>#ifndef PLOT_HPP
#define PLOT_HPP

#include <vector>
#include "Legend.hpp"
#include "Color.hpp"
#include "Delegate.hpp"

namespace hpl
{
class Plot
{

public:
    Plot();
    virtual ~Plot();

    virtual inline std::vector<PlotPart*> getPlotParts() {
        return parts;
    }

    virtual void init(GLuint lineprogram, GLuint textprogram);
    virtual void destroy();
    virtual void draw(float const* mvp);

    virtual void addLegend(Legend* legend);
    inline Legend* getLegend() {
        return legend;
    }

    virtual void setLegendColor(const Color& c);

    Delegate<> changed;

protected:
    Legend* legend;
    std::vector<PlotPart*> parts;
};
}

#endif // PLOT_HPP
<|endoftext|>


<commit_before>// $Id$
// -*-C++-*-
// * BeginRiceCopyright *****************************************************
//
// Copyright ((c)) 2002, Rice University
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright
//   notice, this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// * Neither the name of Rice University (RICE) nor the names of its
//   contributors may be used to endorse or promote products derived from
//   this software without specific prior written permission.
//
// This software is provided by RICE and contributors "as is" and any
// express or implied warranties, including, but not limited to, the
// implied warranties of merchantability and fitness for a particular
// purpose are disclaimed. In no event shall RICE or contributors be
// liable for any direct, indirect, incidental, special, exemplary, or
// consequential damages (including, but not limited to, procurement of
// substitute goods or services; loss of use, data, or profits; or
// business interruption) however caused and on any theory of liability,
// whether in contract, strict liability, or tort (including negligence
// or otherwise) arising in any way out of the use of this software, even
// if advised of the possibility of such damage.
//
// ******************************************************* EndRiceCopyright *

//***************************************************************************
//
// File:
//    Instruction.C
//
// Purpose:
//    [The purpose of this file]
//
// Description:
//    [The set of functions, macros, etc. defined in the file]
//
//***************************************************************************

//************************* System Include Files ****************************

#include <iostream>

//*************************** User Include Files ****************************

#include "Instruction.h"

//*************************** Forward Declarations ***************************

using std::hex;
using std::dec;

//****************************************************************************

//****************************************************************************
// Instruction
//****************************************************************************

void
Instruction::Dump(std::ostream& o, const char* pre) const
{
  String p(pre);
  Addr target = GetTargetAddr(pc);

  o << p << hex << "0x" << pc << dec << ": " << GetDesc().ToString();

  if (target != 0) { o << " <0x" << hex << target << dec << "> "; }
  else             { o << " "; }

  DumpSelf(o, p);
  o << "\n";
}

void
Instruction::DDump() const
{
  Dump(std::cerr);
}

void
Instruction::DumpSelf(std::ostream& o, const char* pre) const
{
}

//***************************************************************************
// CISCInstruction
//***************************************************************************

void
CISCInstruction::Dump(std::ostream& o, const char* pre) const
{
  Instruction::Dump(o, pre);
}

void
CISCInstruction::DDump() const
{
  Dump(std::cerr);
}

void
CISCInstruction::DumpSelf(std::ostream& o, const char* pre) const
{
  o << "(CISC sz:" << GetSize() << ")";
}

//***************************************************************************
// RISCInstruction
//***************************************************************************

void
RISCInstruction::Dump(std::ostream& o, const char* pre) const
{
  Instruction::Dump(o, pre);
}

void
RISCInstruction::DDump() const
{
  Dump(std::cerr);
}

void
RISCInstruction::DumpSelf(std::ostream& o, const char* pre) const
{
  o << "(RISC)";
}

//***************************************************************************
// VLIWInstruction
//***************************************************************************

void
VLIWInstruction::Dump(std::ostream& o, const char* pre) const
{
  Instruction::Dump(o, pre);
}

void
VLIWInstruction::DDump() const
{
  Dump(std::cerr);
}

void
VLIWInstruction::DumpSelf(std::ostream& o, const char* pre) const
{
  o << "(VLIW opIdx:" << GetOpIndex() << ")";
}
<commit_msg>Update test for dumping target address<commit_after>// $Id$
// -*-C++-*-
// * BeginRiceCopyright *****************************************************
//
// Copyright ((c)) 2002, Rice University
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright
//   notice, this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// * Neither the name of Rice University (RICE) nor the names of its
//   contributors may be used to endorse or promote products derived from
//   this software without specific prior written permission.
//
// This software is provided by RICE and contributors "as is" and any
// express or implied warranties, including, but not limited to, the
// implied warranties of merchantability and fitness for a particular
// purpose are disclaimed. In no event shall RICE or contributors be
// liable for any direct, indirect, incidental, special, exemplary, or
// consequential damages (including, but not limited to, procurement of
// substitute goods or services; loss of use, data, or profits; or
// business interruption) however caused and on any theory of liability,
// whether in contract, strict liability, or tort (including negligence
// or otherwise) arising in any way out of the use of this software, even
// if advised of the possibility of such damage.
//
// ******************************************************* EndRiceCopyright *

//***************************************************************************
//
// File:
//    Instruction.C
//
// Purpose:
//    [The purpose of this file]
//
// Description:
//    [The set of functions, macros, etc. defined in the file]
//
//***************************************************************************

//************************* System Include Files ****************************

#include <iostream>

//*************************** User Include Files ****************************

#include "Instruction.h"

//*************************** Forward Declarations ***************************

using std::hex;
using std::dec;

//****************************************************************************

//****************************************************************************
// Instruction
//****************************************************************************

void
Instruction::Dump(std::ostream& o, const char* pre) const
{
  String p(pre);
  Addr target = GetTargetAddr(pc);

  o << p << hex << "0x" << pc << dec << ": " << GetDesc().ToString();

  if (target != 0 || GetOpIndex() != 0) {
    o << " <0x" << hex << target << dec << "> ";
  }
  else {
    o << " ";
  }

  DumpSelf(o, p);
  o << "\n";
}

void
Instruction::DDump() const
{
  Dump(std::cerr);
}

void
Instruction::DumpSelf(std::ostream& o, const char* pre) const
{
}

//***************************************************************************
// CISCInstruction
//***************************************************************************

void
CISCInstruction::Dump(std::ostream& o, const char* pre) const
{
  Instruction::Dump(o, pre);
}

void
CISCInstruction::DDump() const
{
  Dump(std::cerr);
}

void
CISCInstruction::DumpSelf(std::ostream& o, const char* pre) const
{
  o << "(CISC sz:" << GetSize() << ")";
}

//***************************************************************************
// RISCInstruction
//***************************************************************************

void
RISCInstruction::Dump(std::ostream& o, const char* pre) const
{
  Instruction::Dump(o, pre);
}

void
RISCInstruction::DDump() const
{
  Dump(std::cerr);
}

void
RISCInstruction::DumpSelf(std::ostream& o, const char* pre) const
{
  o << "(RISC)";
}

//***************************************************************************
// VLIWInstruction
//***************************************************************************

void
VLIWInstruction::Dump(std::ostream& o, const char* pre) const
{
  Instruction::Dump(o, pre);
}

void
VLIWInstruction::DDump() const
{
  Dump(std::cerr);
}

void
VLIWInstruction::DumpSelf(std::ostream& o, const char* pre) const
{
  o << "(VLIW opIdx:" << GetOpIndex() << ")";
}
<|endoftext|>


<commit_before>// Copyright 2017 Google Inc. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "src/libfuzzer/libfuzzer_macro.h"

#include "src/binary_format.h"
#include "src/libfuzzer/libfuzzer_mutator.h"
#include "src/text_format.h"

namespace protobuf_mutator {
namespace libfuzzer {

namespace {

class InputReader {
 public:
  InputReader(const uint8_t* data, size_t size) : data_(data), size_(size) {}
  virtual ~InputReader() = default;

  virtual bool Read(protobuf::Message* message) const = 0;

  const uint8_t* data() const { return data_; }
  size_t size() const { return size_; }

 private:
  const uint8_t* data_;
  size_t size_;
};

class OutputWriter {
 public:
  OutputWriter(uint8_t* data, size_t size) : data_(data), size_(size) {}
  virtual ~OutputWriter() = default;

  virtual size_t Write(const protobuf::Message& message) = 0;

  uint8_t* data() const { return data_; }
  size_t size() const { return size_; }

 private:
  uint8_t* data_;
  size_t size_;
};

class TextInputReader : public InputReader {
 public:
  using InputReader::InputReader;

  bool Read(protobuf::Message* message) const override {
    return ParseTextMessage(data(), size(), message);
  }
};

class TextOutputWriter : public OutputWriter {
 public:
  using OutputWriter::OutputWriter;

  size_t Write(const protobuf::Message& message) override {
    return SaveMessageAsText(message, data(), size());
  }
};

class BinaryInputReader : public InputReader {
 public:
  using InputReader::InputReader;

  bool Read(protobuf::Message* message) const override {
    return ParseBinaryMessage(data(), size(), message);
  }
};

class BinaryOutputWriter : public OutputWriter {
 public:
  using OutputWriter::OutputWriter;

  size_t Write(const protobuf::Message& message) override {
    return SaveMessageAsBinary(message, data(), size());
  }
};

size_t MutateMessage(unsigned int seed, const InputReader& input,
                     OutputWriter* output, protobuf::Message* message) {
  Mutator mutator;
  mutator.Seed(seed);
  input.Read(message);
  mutator.Mutate(message, output->size() > input.size()
                              ? (output->size() - input.size())
                              : 0);
  if (size_t new_size = output->Write(*message)) {
    assert(new_size <= output->size());
    return new_size;
  }
  return 0;
}

size_t CrossOverMessages(unsigned int seed, const InputReader& input1,
                         const InputReader& input2, OutputWriter* output,
                         protobuf::Message* message1,
                         protobuf::Message* message2) {
  Mutator mutator;
  mutator.Seed(seed);
  input1.Read(message1);
  input2.Read(message2);
  mutator.CrossOver(*message2, message1);
  if (size_t new_size = output->Write(*message1)) {
    assert(new_size <= output->size());
    return new_size;
  }
  return 0;
}

size_t MutateTextMessage(uint8_t* data, size_t size, size_t max_size,
                         unsigned int seed, protobuf::Message* message) {
  TextInputReader input(data, size);
  TextOutputWriter output(data, max_size);
  return MutateMessage(seed, input, &output, message);
}

size_t CrossOverTextMessages(const uint8_t* data1, size_t size1,
                             const uint8_t* data2, size_t size2, uint8_t* out,
                             size_t max_out_size, unsigned int seed,
                             protobuf::Message* message1,
                             protobuf::Message* message2) {
  TextInputReader input1(data1, size1);
  TextInputReader input2(data2, size2);
  TextOutputWriter output(out, max_out_size);
  return CrossOverMessages(seed, input1, input2, &output, message1, message2);
}

size_t MutateBinaryMessage(uint8_t* data, size_t size, size_t max_size,
                           unsigned int seed, protobuf::Message* message) {
  BinaryInputReader input(data, size);
  BinaryOutputWriter output(data, max_size);
  return MutateMessage(seed, input, &output, message);
}

size_t CrossOverBinaryMessages(const uint8_t* data1, size_t size1,
                               const uint8_t* data2, size_t size2, uint8_t* out,
                               size_t max_out_size, unsigned int seed,
                               protobuf::Message* message1,
                               protobuf::Message* message2) {
  BinaryInputReader input1(data1, size1);
  BinaryInputReader input2(data2, size2);
  BinaryOutputWriter output(out, max_out_size);
  return CrossOverMessages(seed, input1, input2, &output, message1, message2);
}

}  // namespace

size_t CustomProtoMutator(bool binary, uint8_t* data, size_t size,
                          size_t max_size, unsigned int seed,
                          protobuf::Message* input) {
  auto mutate = binary ? &MutateBinaryMessage : &MutateTextMessage;
  return mutate(data, size, max_size, seed, input);
}

size_t CustomProtoCrossOver(bool binary, const uint8_t* data1, size_t size1,
                            const uint8_t* data2, size_t size2, uint8_t* out,
                            size_t max_out_size, unsigned int seed,
                            protobuf::Message* input1,
                            protobuf::Message* input2) {
  auto cross = binary ? &CrossOverBinaryMessages : &CrossOverTextMessages;
  return cross(data1, size1, data2, size2, out, max_out_size, seed, input1,
               input2);
}

bool LoadProtoInput(bool binary, const uint8_t* data, size_t size,
                    protobuf::Message* input) {
  return binary ? ParseBinaryMessage(data, size, input)
                : ParseTextMessage(data, size, input);
}

void RegisterProtoFieldMutator(
    const protobuf::FieldDescriptor* field,
    std::function<void(protobuf::Message*)> callback) {
  protobuf_mutator::Mutator::RegisterCustomMutation(field, callback);
}

}  // namespace libfuzzer
}  // namespace protobuf_mutator
<commit_msg>Use Mutator as singleton<commit_after>// Copyright 2017 Google Inc. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "src/libfuzzer/libfuzzer_macro.h"

#include "src/binary_format.h"
#include "src/libfuzzer/libfuzzer_mutator.h"
#include "src/text_format.h"

namespace protobuf_mutator {
namespace libfuzzer {

namespace {

class InputReader {
 public:
  InputReader(const uint8_t* data, size_t size) : data_(data), size_(size) {}
  virtual ~InputReader() = default;

  virtual bool Read(protobuf::Message* message) const = 0;

  const uint8_t* data() const { return data_; }
  size_t size() const { return size_; }

 private:
  const uint8_t* data_;
  size_t size_;
};

class OutputWriter {
 public:
  OutputWriter(uint8_t* data, size_t size) : data_(data), size_(size) {}
  virtual ~OutputWriter() = default;

  virtual size_t Write(const protobuf::Message& message) = 0;

  uint8_t* data() const { return data_; }
  size_t size() const { return size_; }

 private:
  uint8_t* data_;
  size_t size_;
};

class TextInputReader : public InputReader {
 public:
  using InputReader::InputReader;

  bool Read(protobuf::Message* message) const override {
    return ParseTextMessage(data(), size(), message);
  }
};

class TextOutputWriter : public OutputWriter {
 public:
  using OutputWriter::OutputWriter;

  size_t Write(const protobuf::Message& message) override {
    return SaveMessageAsText(message, data(), size());
  }
};

class BinaryInputReader : public InputReader {
 public:
  using InputReader::InputReader;

  bool Read(protobuf::Message* message) const override {
    return ParseBinaryMessage(data(), size(), message);
  }
};

class BinaryOutputWriter : public OutputWriter {
 public:
  using OutputWriter::OutputWriter;

  size_t Write(const protobuf::Message& message) override {
    return SaveMessageAsBinary(message, data(), size());
  }
};

Mutator* GetMutator() {
  static Mutator mutator;
  return &mutator;
}

size_t MutateMessage(unsigned int seed, const InputReader& input,
                     OutputWriter* output, protobuf::Message* message) {
  GetMutator()->Seed(seed);
  input.Read(message);
  GetMutator()->Mutate(message, output->size() > input.size()
                                    ? (output->size() - input.size())
                                    : 0);
  if (size_t new_size = output->Write(*message)) {
    assert(new_size <= output->size());
    return new_size;
  }
  return 0;
}

size_t CrossOverMessages(unsigned int seed, const InputReader& input1,
                         const InputReader& input2, OutputWriter* output,
                         protobuf::Message* message1,
                         protobuf::Message* message2) {
  GetMutator()->Seed(seed);
  input1.Read(message1);
  input2.Read(message2);
  GetMutator()->CrossOver(*message2, message1);
  if (size_t new_size = output->Write(*message1)) {
    assert(new_size <= output->size());
    return new_size;
  }
  return 0;
}

size_t MutateTextMessage(uint8_t* data, size_t size, size_t max_size,
                         unsigned int seed, protobuf::Message* message) {
  TextInputReader input(data, size);
  TextOutputWriter output(data, max_size);
  return MutateMessage(seed, input, &output, message);
}

size_t CrossOverTextMessages(const uint8_t* data1, size_t size1,
                             const uint8_t* data2, size_t size2, uint8_t* out,
                             size_t max_out_size, unsigned int seed,
                             protobuf::Message* message1,
                             protobuf::Message* message2) {
  TextInputReader input1(data1, size1);
  TextInputReader input2(data2, size2);
  TextOutputWriter output(out, max_out_size);
  return CrossOverMessages(seed, input1, input2, &output, message1, message2);
}

size_t MutateBinaryMessage(uint8_t* data, size_t size, size_t max_size,
                           unsigned int seed, protobuf::Message* message) {
  BinaryInputReader input(data, size);
  BinaryOutputWriter output(data, max_size);
  return MutateMessage(seed, input, &output, message);
}

size_t CrossOverBinaryMessages(const uint8_t* data1, size_t size1,
                               const uint8_t* data2, size_t size2, uint8_t* out,
                               size_t max_out_size, unsigned int seed,
                               protobuf::Message* message1,
                               protobuf::Message* message2) {
  BinaryInputReader input1(data1, size1);
  BinaryInputReader input2(data2, size2);
  BinaryOutputWriter output(out, max_out_size);
  return CrossOverMessages(seed, input1, input2, &output, message1, message2);
}

}  // namespace

size_t CustomProtoMutator(bool binary, uint8_t* data, size_t size,
                          size_t max_size, unsigned int seed,
                          protobuf::Message* input) {
  auto mutate = binary ? &MutateBinaryMessage : &MutateTextMessage;
  return mutate(data, size, max_size, seed, input);
}

size_t CustomProtoCrossOver(bool binary, const uint8_t* data1, size_t size1,
                            const uint8_t* data2, size_t size2, uint8_t* out,
                            size_t max_out_size, unsigned int seed,
                            protobuf::Message* input1,
                            protobuf::Message* input2) {
  auto cross = binary ? &CrossOverBinaryMessages : &CrossOverTextMessages;
  return cross(data1, size1, data2, size2, out, max_out_size, seed, input1,
               input2);
}

bool LoadProtoInput(bool binary, const uint8_t* data, size_t size,
                    protobuf::Message* input) {
  return binary ? ParseBinaryMessage(data, size, input)
                : ParseTextMessage(data, size, input);
}

void RegisterProtoFieldMutator(
    const protobuf::FieldDescriptor* field,
    std::function<void(protobuf::Message*)> callback) {
  protobuf_mutator::Mutator::RegisterCustomMutation(field, callback);
}

}  // namespace libfuzzer
}  // namespace protobuf_mutator
<|endoftext|>


<commit_before>// Copyright (c) 2009 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.

#include "libmv/multiview/affine_2d.h"
#include <iostream>
namespace libmv {

// Parametrization
// cos -sin tx
// sin  cos ty
// 0    0   1

// It give the following system A x = B :
// | Xa -Ya 0 1 | | sin |   | Xb |
// | Xa  Ya 1 0 | | cos |   | Yb |
//                | tx  | =
//                | ty  |
// Scale can be estimated if the angle is known. Because x(0) = s * sin(angle).
bool Affine2D_FromCorrespondencesLinear(const Mat &x1, const Mat &x2,
                                            Mat3 *M)  {

  assert(2 == x1.rows());
  assert(2 <= x1.cols());
  assert(x1.rows() == x2.rows());
  assert(x1.cols() == x2.cols());

  const int n = x1.cols();
  Mat A = Mat::Zero(2*n, 4);
  Mat b = Mat::Zero(2*n, 1);
  for (int i = 0; i < n; ++i) {
    const int j= i * 2;
    A(j,0) = -x1(1,i);
    A(j,1) =  x1(0,i);
    A(j,2) =  1.0;
    A(j,3) =  0.0;

    A(j+1,0) = x1(0,i);
    A(j+1,1) = x1(1,i);
    A(j+1,2) = 0.0;
    A(j+1,3) = 1.0;

    b(j,0)   = x2(0,i);
    b(j+1,0) = x2(1,i);
  }
  // Solve Ax=B
  Vec x;
  A.lu().solve(b, &x);

  // Configure output matrix :
  (*M)<<x(1), x(0), x(2), // cos sin tx
       -x(0), x(1), x(3), // sin cos ty
         0.0,  0.0, 1.0;
  return true;
}

}
<commit_msg>Handle degenerate case ( aka no solution found via LU solving).<commit_after>// Copyright (c) 2009 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.

#include "libmv/multiview/affine_2d.h"

namespace libmv {

// Parametrization
// cos -sin tx
// sin  cos ty
// 0    0   1

// It give the following system A x = B :
// | Xa -Ya 0 1 | | sin |   | Xb |
// | Xa  Ya 1 0 | | cos |   | Yb |
//                | tx  | =
//                | ty  |
// Scale can be estimated if the angle is known. Because x(0) = s * sin(angle).
bool Affine2D_FromCorrespondencesLinear(const Mat &x1, const Mat &x2,
                                            Mat3 *M)  {

  assert(2 == x1.rows());
  assert(2 <= x1.cols());
  assert(x1.rows() == x2.rows());
  assert(x1.cols() == x2.cols());

  const int n = x1.cols();
  Mat A = Mat::Zero(2*n, 4);
  Mat b = Mat::Zero(2*n, 1);
  for (int i = 0; i < n; ++i) {
    const int j= i * 2;
    A(j,0) = -x1(1,i);
    A(j,1) =  x1(0,i);
    A(j,2) =  1.0;
    A(j,3) =  0.0;

    A(j+1,0) = x1(0,i);
    A(j+1,1) = x1(1,i);
    A(j+1,2) = 0.0;
    A(j+1,3) = 1.0;

    b(j,0)   = x2(0,i);
    b(j+1,0) = x2(1,i);
  }
  // Solve Ax=B
  Vec x;
  if (A.lu().solve(b, &x))  {
    // Configure output matrix :
    (*M)<<x(1), x(0), x(2), // cos sin tx
         -x(0), x(1), x(3), // sin cos ty
           0.0,  0.0, 1.0;
    return true;
  }
  else  {
    return false;
  }
}

}
<|endoftext|>