<commit_before>/*************************************************************************
 *
 * 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: textparagraphpropertiescontext.cxx,v $
 * $Revision: 1.5 $
 *
 * 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.
 *
 ************************************************************************/

#include "oox/drawingml/textparagraphpropertiescontext.hxx"

#include <com/sun/star/text/WritingMode.hpp>
#include <com/sun/star/awt/FontDescriptor.hpp>

#include "oox/drawingml/colorchoicecontext.hxx"
#include "oox/drawingml/textcharacterpropertiescontext.hxx"
#include "oox/drawingml/drawingmltypes.hxx"
#include "oox/drawingml/textfontcontext.hxx"
#include "oox/helper/attributelist.hxx"
#include "oox/core/namespaces.hxx"
#include "textspacingcontext.hxx"
#include "texttabstoplistcontext.hxx"
#include "tokens.hxx"

using ::rtl::OUString;
using namespace ::oox::core;
using ::com::sun::star::awt::FontDescriptor;
using namespace ::com::sun::star::uno;
using namespace ::com::sun::star::xml::sax;
using namespace ::com::sun::star::style;
using namespace ::com::sun::star::text;

namespace oox { namespace drawingml {

// CT_TextParagraphProperties
TextParagraphPropertiesContext::TextParagraphPropertiesContext( ContextHandler& rParent,
                                                                const Reference< XFastAttributeList >& xAttribs,
                                                                TextParagraphProperties& rTextParagraphProperties )
: ContextHandler( rParent )
, mrTextParagraphProperties( rTextParagraphProperties )
, mrSpaceBefore( rTextParagraphProperties.getParaTopMargin() )
, mrSpaceAfter( rTextParagraphProperties.getParaBottomMargin() )
, mrBulletList( rTextParagraphProperties.getBulletList() )
{
    OUString sValue;
    AttributeList attribs( xAttribs );

    PropertyMap& rPropertyMap( mrTextParagraphProperties.getTextParagraphPropertyMap() );

    // ST_TextAlignType
    if ( xAttribs->hasAttribute( XML_algn ) )
    {
        sal_Int32 nAlign = xAttribs->getOptionalValueToken( XML_algn, XML_l );
        const OUString sParaAdjust( CREATE_OUSTRING( "ParaAdjust" ) );
        rPropertyMap[ sParaAdjust ] <<= GetParaAdjust( nAlign );
    }
//  OSL_TRACE( "OOX: para adjust %d", GetParaAdjust( nAlign ));
    // TODO see to do the same with RubyAdjust

    // ST_Coordinate32
//  sValue = xAttribs->getOptionalValue( XML_defTabSz );    SJ: we need to be able to set the default tab size for each text object,
//                                                          this is possible at the moment only for the whole document.
//  sal_Int32 nDefTabSize = ( sValue.getLength() == 0 ? 0 : GetCoordinate(  sValue ) );
    // TODO

//  bool bEaLineBrk = attribs.getBool( XML_eaLnBrk, true );
    if ( xAttribs->hasAttribute( XML_latinLnBrk ) )
    {
        bool bLatinLineBrk = attribs.getBool( XML_latinLnBrk, true );
        const OUString sParaIsHyphenation( CREATE_OUSTRING( "ParaIsHyphenation" ) );
        rPropertyMap[ sParaIsHyphenation ] <<= bLatinLineBrk;
    }
    // TODO see what to do with Asian hyphenation

    // ST_TextFontAlignType
    // TODO
//  sal_Int32 nFontAlign = xAttribs->getOptionalValueToken( XML_fontAlgn, XML_base );

    if ( xAttribs->hasAttribute( XML_hangingPunct ) )
    {
        bool bHangingPunct = attribs.getBool( XML_hangingPunct, false );
        const OUString sParaIsHangingPunctuation( CREATE_OUSTRING( "ParaIsHangingPunctuation" ) );
        rPropertyMap[ sParaIsHangingPunctuation ] <<= bHangingPunct;
    }

  // ST_Coordinate
    if ( xAttribs->hasAttribute( XML_indent ) )
    {
        sValue = xAttribs->getOptionalValue( XML_indent );
        const OUString sParaFirstLineIndent( CREATE_OUSTRING( "ParaFirstLineIndent" ) );
        rPropertyMap[ sParaFirstLineIndent ] <<= ( sValue.getLength() == 0 ? 0 : GetCoordinate(  sValue ) );
    }

  // ST_TextIndentLevelType
    // -1 is an invalid value and denote the lack of level
    sal_Int32 nLevel = attribs.getInteger( XML_lvl, 0 );
    if( nLevel > 8 || nLevel < 0 )
    {
        nLevel = 0;
    }

    mrTextParagraphProperties.setLevel( static_cast< sal_Int16 >( nLevel ) );

    char name[] = "Outline X";
    name[8] = static_cast<char>( '1' + nLevel );
    const OUString sStyleNameValue( rtl::OUString::createFromAscii( name ) );
    mrBulletList.setStyleName( sStyleNameValue );

    // ST_TextMargin
    // ParaLeftMargin
    if ( xAttribs->hasAttribute( XML_marL ) )
    {
        sValue = xAttribs->getOptionalValue( XML_marL );
        sal_Int32 nMarL = ( sValue.getLength() == 0 ? 0 : GetCoordinate(  sValue ) );
        const OUString sParaLeftMargin( CREATE_OUSTRING( "ParaLeftMargin" ) );
        rPropertyMap[ sParaLeftMargin ] <<= nMarL;
    }

    // ParaRightMargin
    if ( xAttribs->hasAttribute( XML_marR ) )
    {
        sValue = xAttribs->getOptionalValue( XML_marR );
        sal_Int32 nMarR  = ( sValue.getLength() == 0 ? 0 : GetCoordinate( sValue ) );
        const OUString sParaRightMargin( CREATE_OUSTRING( "ParaRightMargin" ) );
        rPropertyMap[ sParaRightMargin ] <<= nMarR;
    }

    if ( xAttribs->hasAttribute( XML_rtl ) )
    {
        bool bRtl = attribs.getBool( XML_rtl, false );
        const OUString sTextWritingMode( CREATE_OUSTRING( "TextWritingMode" ) );
        rPropertyMap[ sTextWritingMode ] <<= ( bRtl ? WritingMode_RL_TB : WritingMode_LR_TB );
    }
}

TextParagraphPropertiesContext::~TextParagraphPropertiesContext()
{
    PropertyMap& rPropertyMap( mrTextParagraphProperties.getTextParagraphPropertyMap() );
    if ( maLineSpacing.bHasValue )
    {
        const OUString sParaLineSpacing( CREATE_OUSTRING( "ParaLineSpacing" ) );
        //OSL_TRACE( "OOX: ParaLineSpacing unit = %d, value = %d", maLineSpacing.nUnit, maLineSpacing.nValue );
        rPropertyMap[ sParaLineSpacing ] <<= maLineSpacing.toLineSpacing();
    }
    ::std::list< TabStop >::size_type nTabCount = maTabList.size();
    if( nTabCount != 0 )
    {
        Sequence< TabStop > aSeq( nTabCount );
        TabStop * aArray = aSeq.getArray();
        OSL_ENSURE( aArray != NULL, "sequence array is NULL" );
        ::std::copy( maTabList.begin(), maTabList.end(), aArray );
        const OUString sParaTabStops( CREATE_OUSTRING( "ParaTabStops" ) );
        rPropertyMap[ sParaTabStops ] <<= aSeq;
    }
    if( mrBulletList.is() )
    {
        const rtl::OUString sIsNumbering( CREATE_OUSTRING( "IsNumbering" ) );
        rPropertyMap[ sIsNumbering ] <<= sal_True;
    }
    sal_Int16 nLevel = mrTextParagraphProperties.getLevel();
//  OSL_TRACE("OOX: numbering level = %d", nLevel );
    const OUString sNumberingLevel( CREATE_OUSTRING( "NumberingLevel" ) );
    rPropertyMap[ sNumberingLevel ] <<= (sal_Int16)nLevel;
    sal_Bool bTmp = sal_True;
    const OUString sNumberingIsNumber( CREATE_OUSTRING( "NumberingIsNumber" ) );
    rPropertyMap[ sNumberingIsNumber ] <<= bTmp;
}

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

void TextParagraphPropertiesContext::endFastElement( sal_Int32 ) throw (SAXException, RuntimeException)
{
}

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

Reference< XFastContextHandler > TextParagraphPropertiesContext::createFastChildContext( sal_Int32 aElementToken, const Reference< XFastAttributeList >& rXAttributes ) throw (SAXException, RuntimeException)
{
    Reference< XFastContextHandler > xRet;
    switch( aElementToken )
    {
        case NMSP_DRAWINGML|XML_lnSpc:          // CT_TextSpacing
            xRet.set( new TextSpacingContext( *this, maLineSpacing ) );
            break;
        case NMSP_DRAWINGML|XML_spcBef:         // CT_TextSpacing
            xRet.set( new TextSpacingContext( *this, mrSpaceBefore ) );
            break;
        case NMSP_DRAWINGML|XML_spcAft:         // CT_TextSpacing
            xRet.set( new TextSpacingContext( *this, mrSpaceAfter ) );
            break;

        // EG_TextBulletColor
        case NMSP_DRAWINGML|XML_buClrTx:        // CT_TextBulletColorFollowText ???
            mrBulletList.mbBulletColorFollowText <<= sal_True;
            break;
        case NMSP_DRAWINGML|XML_buClr:          // CT_Color
            xRet.set( new colorChoiceContext( *this, *mrBulletList.maBulletColorPtr ) );
            break;

        // EG_TextBulletSize
        case NMSP_DRAWINGML|XML_buSzTx:         // CT_TextBulletSizeFollowText
            mrBulletList.setBulletSize(100);
            break;
        case NMSP_DRAWINGML|XML_buSzPct:        // CT_TextBulletSizePercent
            mrBulletList.setBulletSize( static_cast<sal_Int16>( GetPercent( rXAttributes->getOptionalValue( XML_val ) ) / 1000 ) );
            break;
        case NMSP_DRAWINGML|XML_buSzPts:        // CT_TextBulletSizePoint
            mrBulletList.setBulletSize(0);
            mrBulletList.setFontSize( static_cast<sal_Int16>(GetTextSize( rXAttributes->getOptionalValue( XML_val ) ) ) );
            break;

        // EG_TextBulletTypeface
        case NMSP_DRAWINGML|XML_buFontTx:       // CT_TextBulletTypefaceFollowText
            mrBulletList.mbBulletFontFollowText <<= sal_True;
            break;
        case NMSP_DRAWINGML|XML_buFont:         // CT_TextFont
            xRet.set( new TextFontContext( *this, aElementToken, rXAttributes, mrBulletList.maBulletFont ) );
            break;

        // EG_TextBullet
        case NMSP_DRAWINGML|XML_buNone:         // CT_TextNoBullet
            mrBulletList.setNone();
            break;
        case NMSP_DRAWINGML|XML_buAutoNum:      // CT_TextAutonumberBullet
        {
            AttributeList attribs( rXAttributes );
            try {
                sal_Int32 nType = rXAttributes->getValueToken( XML_type );
                sal_Int32 nStartAt = attribs.getInteger( XML_startAt, 1 );
                if( nStartAt > 32767 )
                {
                    nStartAt = 32767;
                }
                else if( nStartAt < 1 )
                {
                    nStartAt = 1;
                }
                mrBulletList.setStartAt( nStartAt );
                mrBulletList.setType( nType );
            }
            catch(SAXException& /* e */ )
            {
                OSL_TRACE("OOX: SAXException in XML_buAutoNum");
            }
            break;
        }
        case NMSP_DRAWINGML|XML_buChar:         // CT_TextCharBullet
            try {
                mrBulletList.setBulletChar( rXAttributes->getValue( XML_char ) );
            }
            catch(SAXException& /* e */)
            {
                OSL_TRACE("OOX: SAXException in XML_buChar");
            }
            break;
        case NMSP_DRAWINGML|XML_buBlip:         // CT_TextBlipBullet
            // TODO
            break;

        case NMSP_DRAWINGML|XML_tabLst:         // CT_TextTabStopList
            xRet.set( new TextTabStopListContext( *this, maTabList ) );
            break;
        case NMSP_DRAWINGML|XML_defRPr:         // CT_TextCharacterProperties
            xRet.set( new TextCharacterPropertiesContext( *this, rXAttributes, *mrTextParagraphProperties.getTextCharacterProperties() ) );
            break;
    }
    if ( !xRet.is() )
        xRet.set( this );
    return xRet;
}

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

} }

<commit_msg>INTEGRATION: CWS impress146 (1.4.12); FILE MERGED 2008/07/07 12:29:32 dr 1.4.12.3: RESYNC: (1.4-1.5); FILE MERGED 2008/07/02 14:24:07 sj 1.4.12.2: #i90625# taking care of api changes 2008/07/01 15:13:56 sj 1.4.12.1: #i90625# taking care of api changes<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: textparagraphpropertiescontext.cxx,v $
 * $Revision: 1.6 $
 *
 * 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.
 *
 ************************************************************************/

#include "oox/drawingml/textparagraphpropertiescontext.hxx"

#include <com/sun/star/text/WritingMode.hpp>
#include <com/sun/star/awt/FontDescriptor.hpp>

#include "oox/drawingml/colorchoicecontext.hxx"
#include "oox/drawingml/textcharacterpropertiescontext.hxx"
#include "oox/drawingml/drawingmltypes.hxx"
#include "oox/drawingml/textfontcontext.hxx"
#include "oox/helper/attributelist.hxx"
#include "oox/core/namespaces.hxx"
#include "textspacingcontext.hxx"
#include "texttabstoplistcontext.hxx"
#include "tokens.hxx"

using ::rtl::OUString;
using namespace ::oox::core;
using ::com::sun::star::awt::FontDescriptor;
using namespace ::com::sun::star::uno;
using namespace ::com::sun::star::xml::sax;
using namespace ::com::sun::star::style;
using namespace ::com::sun::star::text;

namespace oox { namespace drawingml {

// CT_TextParagraphProperties
TextParagraphPropertiesContext::TextParagraphPropertiesContext( ContextHandler& rParent,
                                                                const Reference< XFastAttributeList >& xAttribs,
                                                                TextParagraphProperties& rTextParagraphProperties )
: ContextHandler( rParent )
, mrTextParagraphProperties( rTextParagraphProperties )
, mrSpaceBefore( rTextParagraphProperties.getParaTopMargin() )
, mrSpaceAfter( rTextParagraphProperties.getParaBottomMargin() )
, mrBulletList( rTextParagraphProperties.getBulletList() )
{
    OUString sValue;
    AttributeList attribs( xAttribs );

    PropertyMap& rPropertyMap( mrTextParagraphProperties.getTextParagraphPropertyMap() );

    // ST_TextAlignType
    if ( xAttribs->hasAttribute( XML_algn ) )
    {
        sal_Int32 nAlign = xAttribs->getOptionalValueToken( XML_algn, XML_l );
        const OUString sParaAdjust( CREATE_OUSTRING( "ParaAdjust" ) );
        rPropertyMap[ sParaAdjust ] <<= GetParaAdjust( nAlign );
    }
//  OSL_TRACE( "OOX: para adjust %d", GetParaAdjust( nAlign ));
    // TODO see to do the same with RubyAdjust

    // ST_Coordinate32
//  sValue = xAttribs->getOptionalValue( XML_defTabSz );    SJ: we need to be able to set the default tab size for each text object,
//                                                          this is possible at the moment only for the whole document.
//  sal_Int32 nDefTabSize = ( sValue.getLength() == 0 ? 0 : GetCoordinate(  sValue ) );
    // TODO

//  bool bEaLineBrk = attribs.getBool( XML_eaLnBrk, true );
    if ( xAttribs->hasAttribute( XML_latinLnBrk ) )
    {
        bool bLatinLineBrk = attribs.getBool( XML_latinLnBrk, true );
        const OUString sParaIsHyphenation( CREATE_OUSTRING( "ParaIsHyphenation" ) );
        rPropertyMap[ sParaIsHyphenation ] <<= bLatinLineBrk;
    }
    // TODO see what to do with Asian hyphenation

    // ST_TextFontAlignType
    // TODO
//  sal_Int32 nFontAlign = xAttribs->getOptionalValueToken( XML_fontAlgn, XML_base );

    if ( xAttribs->hasAttribute( XML_hangingPunct ) )
    {
        bool bHangingPunct = attribs.getBool( XML_hangingPunct, false );
        const OUString sParaIsHangingPunctuation( CREATE_OUSTRING( "ParaIsHangingPunctuation" ) );
        rPropertyMap[ sParaIsHangingPunctuation ] <<= bHangingPunct;
    }

  // ST_Coordinate
    if ( xAttribs->hasAttribute( XML_indent ) )
    {
        sValue = xAttribs->getOptionalValue( XML_indent );
        mrTextParagraphProperties.getFirstLineIndentation() = boost::optional< sal_Int32 >( sValue.getLength() == 0 ? 0 : GetCoordinate( sValue ) );
    }

  // ST_TextIndentLevelType
    // -1 is an invalid value and denote the lack of level
    sal_Int32 nLevel = attribs.getInteger( XML_lvl, 0 );
    if( nLevel > 8 || nLevel < 0 )
    {
        nLevel = 0;
    }

    mrTextParagraphProperties.setLevel( static_cast< sal_Int16 >( nLevel ) );

    char name[] = "Outline X";
    name[8] = static_cast<char>( '1' + nLevel );
    const OUString sStyleNameValue( rtl::OUString::createFromAscii( name ) );
    mrBulletList.setStyleName( sStyleNameValue );

    // ST_TextMargin
    // ParaLeftMargin
    if ( xAttribs->hasAttribute( XML_marL ) )
    {
        sValue = xAttribs->getOptionalValue( XML_marL );
        mrTextParagraphProperties.getParaLeftMargin() = boost::optional< sal_Int32 >( sValue.getLength() == 0 ? 0 : GetCoordinate( sValue ) );
    }

    // ParaRightMargin
    if ( xAttribs->hasAttribute( XML_marR ) )
    {
        sValue = xAttribs->getOptionalValue( XML_marR );
        sal_Int32 nMarR  = ( sValue.getLength() == 0 ? 0 : GetCoordinate( sValue ) );
        const OUString sParaRightMargin( CREATE_OUSTRING( "ParaRightMargin" ) );
        rPropertyMap[ sParaRightMargin ] <<= nMarR;
    }

    if ( xAttribs->hasAttribute( XML_rtl ) )
    {
        bool bRtl = attribs.getBool( XML_rtl, false );
        const OUString sTextWritingMode( CREATE_OUSTRING( "TextWritingMode" ) );
        rPropertyMap[ sTextWritingMode ] <<= ( bRtl ? WritingMode_RL_TB : WritingMode_LR_TB );
    }
}

TextParagraphPropertiesContext::~TextParagraphPropertiesContext()
{
    PropertyMap& rPropertyMap( mrTextParagraphProperties.getTextParagraphPropertyMap() );
    if ( maLineSpacing.bHasValue )
    {
        const OUString sParaLineSpacing( CREATE_OUSTRING( "ParaLineSpacing" ) );
        //OSL_TRACE( "OOX: ParaLineSpacing unit = %d, value = %d", maLineSpacing.nUnit, maLineSpacing.nValue );
        rPropertyMap[ sParaLineSpacing ] <<= maLineSpacing.toLineSpacing();
    }
    ::std::list< TabStop >::size_type nTabCount = maTabList.size();
    if( nTabCount != 0 )
    {
        Sequence< TabStop > aSeq( nTabCount );
        TabStop * aArray = aSeq.getArray();
        OSL_ENSURE( aArray != NULL, "sequence array is NULL" );
        ::std::copy( maTabList.begin(), maTabList.end(), aArray );
        const OUString sParaTabStops( CREATE_OUSTRING( "ParaTabStops" ) );
        rPropertyMap[ sParaTabStops ] <<= aSeq;
    }
    if ( mpFillPropertiesPtr )
    {
        ::com::sun::star::uno::Reference< ::com::sun::star::graphic::XGraphic > xGraphic( mpFillPropertiesPtr->getXGraphic() );
        if ( xGraphic.is() )
            mrBulletList.setGraphic( xGraphic );
    }
    if( mrBulletList.is() )
    {
        const rtl::OUString sIsNumbering( CREATE_OUSTRING( "IsNumbering" ) );
        rPropertyMap[ sIsNumbering ] <<= sal_True;
    }
    sal_Int16 nLevel = mrTextParagraphProperties.getLevel();
//  OSL_TRACE("OOX: numbering level = %d", nLevel );
    const OUString sNumberingLevel( CREATE_OUSTRING( "NumberingLevel" ) );
    rPropertyMap[ sNumberingLevel ] <<= (sal_Int16)nLevel;
    sal_Bool bTmp = sal_True;
    const OUString sNumberingIsNumber( CREATE_OUSTRING( "NumberingIsNumber" ) );
    rPropertyMap[ sNumberingIsNumber ] <<= bTmp;
}

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

void TextParagraphPropertiesContext::endFastElement( sal_Int32 ) throw (SAXException, RuntimeException)
{
}

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

Reference< XFastContextHandler > TextParagraphPropertiesContext::createFastChildContext( sal_Int32 aElementToken, const Reference< XFastAttributeList >& rXAttributes ) throw (SAXException, RuntimeException)
{
    Reference< XFastContextHandler > xRet;
    switch( aElementToken )
    {
        case NMSP_DRAWINGML|XML_lnSpc:          // CT_TextSpacing
            xRet.set( new TextSpacingContext( *this, maLineSpacing ) );
            break;
        case NMSP_DRAWINGML|XML_spcBef:         // CT_TextSpacing
            xRet.set( new TextSpacingContext( *this, mrSpaceBefore ) );
            break;
        case NMSP_DRAWINGML|XML_spcAft:         // CT_TextSpacing
            xRet.set( new TextSpacingContext( *this, mrSpaceAfter ) );
            break;

        // EG_TextBulletColor
        case NMSP_DRAWINGML|XML_buClrTx:        // CT_TextBulletColorFollowText ???
            mrBulletList.mbBulletColorFollowText <<= sal_True;
            break;
        case NMSP_DRAWINGML|XML_buClr:          // CT_Color
            xRet.set( new colorChoiceContext( *this, *mrBulletList.maBulletColorPtr ) );
            break;

        // EG_TextBulletSize
        case NMSP_DRAWINGML|XML_buSzTx:         // CT_TextBulletSizeFollowText
            mrBulletList.setBulletSize(100);
            break;
        case NMSP_DRAWINGML|XML_buSzPct:        // CT_TextBulletSizePercent
            mrBulletList.setBulletSize( static_cast<sal_Int16>( GetPercent( rXAttributes->getOptionalValue( XML_val ) ) / 1000 ) );
            break;
        case NMSP_DRAWINGML|XML_buSzPts:        // CT_TextBulletSizePoint
            mrBulletList.setBulletSize(0);
            mrBulletList.setFontSize( static_cast<sal_Int16>(GetTextSize( rXAttributes->getOptionalValue( XML_val ) ) ) );
            break;

        // EG_TextBulletTypeface
        case NMSP_DRAWINGML|XML_buFontTx:       // CT_TextBulletTypefaceFollowText
            mrBulletList.mbBulletFontFollowText <<= sal_True;
            break;
        case NMSP_DRAWINGML|XML_buFont:         // CT_TextFont
            xRet.set( new TextFontContext( *this, aElementToken, rXAttributes, mrBulletList.maBulletFont ) );
            break;

        // EG_TextBullet
        case NMSP_DRAWINGML|XML_buNone:         // CT_TextNoBullet
            mrBulletList.setNone();
            break;
        case NMSP_DRAWINGML|XML_buAutoNum:      // CT_TextAutonumberBullet
        {
            AttributeList attribs( rXAttributes );
            try {
                sal_Int32 nType = rXAttributes->getValueToken( XML_type );
                sal_Int32 nStartAt = attribs.getInteger( XML_startAt, 1 );
                if( nStartAt > 32767 )
                {
                    nStartAt = 32767;
                }
                else if( nStartAt < 1 )
                {
                    nStartAt = 1;
                }
                mrBulletList.setStartAt( nStartAt );
                mrBulletList.setType( nType );
            }
            catch(SAXException& /* e */ )
            {
                OSL_TRACE("OOX: SAXException in XML_buAutoNum");
            }
            break;
        }
        case NMSP_DRAWINGML|XML_buChar:         // CT_TextCharBullet
            try {
                mrBulletList.setBulletChar( rXAttributes->getValue( XML_char ) );
            }
            catch(SAXException& /* e */)
            {
                OSL_TRACE("OOX: SAXException in XML_buChar");
            }
            break;
        case NMSP_DRAWINGML|XML_buBlip:         // CT_TextBlipBullet
            {
                mpFillPropertiesPtr = FillPropertiesPtr( new FillProperties() );
                xRet.set( new BlipFillPropertiesContext( *this, rXAttributes, *mpFillPropertiesPtr.get() ) );
            }
            break;

        case NMSP_DRAWINGML|XML_tabLst:         // CT_TextTabStopList
            xRet.set( new TextTabStopListContext( *this, maTabList ) );
            break;
        case NMSP_DRAWINGML|XML_defRPr:         // CT_TextCharacterProperties
            xRet.set( new TextCharacterPropertiesContext( *this, rXAttributes, *mrTextParagraphProperties.getTextCharacterProperties() ) );
            break;
    }
    if ( !xRet.is() )
        xRet.set( this );
    return xRet;
}

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

} }

<|endoftext|>


<commit_before>#include "VectorDefiner.h"
#include <algorithm>
#include <cctype>
#include <sstream>
#include <iostream>
#include <fstream>

//yes
VectorDefiner::VectorDefiner()
{
	this->eqr = NULL;
	this->vectors = NULL;
	this->is_file = false;
	this->filename = NULL;
	this->culled_vectors = NULL;
	this->culled_space = NULL;
	this->min = NULL;
	this->max = NULL;
	this->space = NULL;
}

VectorDefiner::~VectorDefiner()
{
	delete this->culled_vectors;
	delete this->culled_space;
	delete this->filename;
	delete this->eqr;
	delete this->vectors;
}

void VectorDefiner::give_input(std::string str){
	//supposedly removes whitespace from the string, effectively trimming it
	std::string* mystr = new std::string(str);
	mystr->erase(std::remove_if(mystr->begin(), mystr->end(), isspace), mystr->end());
	this->is_file = false;
	switch(str[0]){
		case 'a':
		case 'b':
		case 'c':
		case 'd':
		case 'e':
		case 'f':
		case 'g':
		case 'h':
		case 'i':
		case 'j':
		case 'k':
		case 'l':
		case 'm':
		case 'n':
		case 'o':
		case 'p':
		case 'q':
		case 'r':
		case 's':
		case 't':
		case 'u':
		case 'v':
		case 'w':
		case 'x':
		case 'y':
		case 'z':
		case 'A':
		case 'B':
		case 'C':
		case 'D':
		case 'E':
		case 'F':
		case 'G':
		case 'H':
		case 'I':
		case 'J':
		case 'K':
		case 'L':
		case 'M':
		case 'N':
		case 'O':
		case 'P':
		case 'Q':
		case 'R':
		case 'S':
		case 'T':
		case 'U':
		case 'V':
		case 'W':
		case 'X':
		case 'Y':
		case 'Z':
		case '~':
		case '/':
			this->is_file = true;
	}

	if(this->is_file){
		this->filename = mystr;
	}else{
		delete mystr;
		equation_factory eqrf;
		this->eqr = eqrf.vector_equation(str);
	}

}

std::string VectorDefiner::replacer(std::string subject, const std::string& search, const std::string& replace) {
	size_t pos = 0;
	while ((pos = subject.find(search, pos)) != std::string::npos) {
		subject.replace(pos, search.length(), replace);
		pos += replace.length();
	}
	return subject;
}

void VectorDefiner::populate(std::vector<vector3d*>* space){
	if(this->is_file){
		//set up of internal data structures
		if (this->space != NULL) {
			delete this->space;
		}
		this->space = new std::vector<vector3d*>();
		if (this->vectors != NULL) {
			delete this->vectors;
		}
		this->vectors = new std::vector<vector3d*>();

		//I mean, Ideally, the file would be formatted as: x_space,y_space,z_space,x_vector,y_vector,z_vector\n
		//I can deal with whitespace, actually
		//std::string the_file = *(this->filename);
		std::string line;
		std::ifstream csv(this->filename->c_str());
		std::string newline("\n");
		std::string space(" ");
		if(csv.is_open()){ //read the file if it's open
			while(std::getline(csv, line)){ //read the file
				//line has the data inside of it now. like a civilized language and library should
				std::vector<std::string> eqrs;
				std::istringstream ss(line);
				std::string token;
				while (std::getline(ss, token, ',')) {
					//adds the newline, also replaces any newline tokens, which would be bad to have at the end.
					eqrs.push_back(replacer(token, newline, space));
				}
				//ok, now the items are inside eqrs;
				vector3d* spatial = new vector3d(atof(eqrs[0].c_str()), atof(eqrs[1].c_str()), atof(eqrs[2].c_str()));
				vector3d* vectorFiled = new vector3d(atof(eqrs[3].c_str()), atof(eqrs[4].c_str()), atof(eqrs[5].c_str()));
				this->space->push_back(spatial);
				this->vectors->push_back(vectorFiled);
			}
			csv.close();//be polite
		}
	}else{
		float* f = new float[3];

		if(this->vectors != NULL){
			delete this->vectors;
		}
		this->vectors = new std::vector<vector3d*>();

		for(unsigned int i=0; i<space->size(); ++i){
			float* temp = space->at(i)->xyz(); // get the spacial point
			f = eqr->eval(temp[0],temp[1],temp[2],f); //eval field at that point
			vector3d* vvv = new vector3d(f[0],f[1],f[2]); //make new vector to represent vector at that point in field
			this->vectors->push_back(vvv); //add it to out list of vectors
		}

		this->space = space;//EWW
		delete f;
	}
}

std::vector<vector3d*>* VectorDefiner::cull_vectors(float xmin, float xmax, float ymin, float ymax, float zmin, float zmax){
	std::vector<vector3d*>* vec = new std::vector<vector3d*>();

	vector3d* mmax = NULL;
	vector3d* mmin = NULL;
	vector3d* cmin = NULL;
	vector3d* cmax = NULL;

	for(unsigned int i=0; i<this->vectors->size(); ++i){
		vector3d* v = this->vectors->at(i);
		float* f = v->xyz();
		if(xmin < f[0] && f[0] < xmax){
			if(ymin < f[1] && f[1] < ymax){
				if(zmin < f[2] && f[2] < zmax){
					vec->push_back(v); //vector is in bounds, yay!
					//find culled min and max
					if (cmax == NULL || cmax->magnitude() < v->magnitude()) {
						cmax = v;
					}
					if (cmin == NULL || cmin->magnitude() > v->magnitude()) {
						cmin = v;
					}
				}
			}
		}
		//find min and max
		if(mmax == NULL || mmax->magnitude() < v->magnitude()){
			mmax = v;
		}
		if(mmin == NULL || mmin->magnitude() > v->magnitude()){
			mmin = v;
		}
	}
	//set up the cache
	this->min = mmin;
	this->max = mmax;
	this->cull_max = cmax;
	this->cull_max = cmin;
	//NOTE, I AM DELETING DATA
	if(this->culled_vectors != NULL) {
		delete this->culled_vectors;
	}
	this->culled_vectors = vec;
	//return
	return vec;
}

std::vector<vector3d*>* VectorDefiner::cull_space(float xmin, float xmax, float ymin, float ymax, float zmin, float zmax){
	std::vector<vector3d*>* vec = new std::vector<vector3d*>();

	for(unsigned int i=0; i<this->vectors->size(); ++i){
		vector3d* v = this->vectors->at(i);
		vector3d* space_v = this->space->at(i);
		float* f = v->xyz();
		if(xmin < f[0] && f[0] < xmax){
			if(ymin < f[1] && f[1] < ymax){
				if(zmin < f[2] && f[2] < zmax){
					vec->push_back(space_v); //vector is in bounds, yay!
				}
			}
		}
	}
	if(this->culled_space != NULL){
		delete this->culled_space;
	}
	this->culled_space = vec;
	return vec;
}

std::vector<vector3d*>* VectorDefiner::get_cull_vectors_cache(){
	return this->culled_vectors;
}
std::vector<vector3d*>* VectorDefiner::get_cull_space_cache(){
	return this->culled_space;
}
vector3d* VectorDefiner::get_vector_cull_min(){
	return this->cull_min;
}
vector3d* VectorDefiner::get_vector_cull_max(){
	return this->cull_max;
}
vector3d* VectorDefiner::get_vector_min() {
	return this->min;
}
vector3d* VectorDefiner::get_vector_max() {
	return this->max;
}<commit_msg>fixed bug issue<commit_after>#include "VectorDefiner.h"
#include <algorithm>
#include <cctype>
#include <sstream>
#include <iostream>
#include <fstream>

//yes
VectorDefiner::VectorDefiner()
{
	this->eqr = NULL;
	this->vectors = NULL;
	this->is_file = false;
	this->filename = NULL;
	this->culled_vectors = NULL;
	this->culled_space = NULL;
	this->min = NULL;
	this->max = NULL;
	this->space = NULL;
}

VectorDefiner::~VectorDefiner()
{
	delete this->culled_vectors;
	delete this->culled_space;
	delete this->filename;
	delete this->eqr;
	delete this->vectors;
}

void VectorDefiner::give_input(std::string str){
	//supposedly removes whitespace from the string, effectively trimming it
	std::string* mystr = new std::string(str);
	mystr->erase(std::remove_if(mystr->begin(), mystr->end(), isspace), mystr->end());
	this->is_file = false;
	switch(str[0]){
		case 'a':
		case 'b':
		case 'c':
		case 'd':
		case 'e':
		case 'f':
		case 'g':
		case 'h':
		case 'i':
		case 'j':
		case 'k':
		case 'l':
		case 'm':
		case 'n':
		case 'o':
		case 'p':
		case 'q':
		case 'r':
		case 's':
		case 't':
		case 'u':
		case 'v':
		case 'w':
		case 'x':
		case 'y':
		case 'z':
		case 'A':
		case 'B':
		case 'C':
		case 'D':
		case 'E':
		case 'F':
		case 'G':
		case 'H':
		case 'I':
		case 'J':
		case 'K':
		case 'L':
		case 'M':
		case 'N':
		case 'O':
		case 'P':
		case 'Q':
		case 'R':
		case 'S':
		case 'T':
		case 'U':
		case 'V':
		case 'W':
		case 'X':
		case 'Y':
		case 'Z':
		case '~':
		case '/':
			this->is_file = true;
	}

	if(this->is_file){
		this->filename = mystr;
	}else{
		delete mystr;
		equation_factory eqrf;
		this->eqr = eqrf.vector_equation(str);
	}

}

std::string VectorDefiner::replacer(std::string subject, const std::string& search, const std::string& replace) {
	size_t pos = 0;
	while ((pos = subject.find(search, pos)) != std::string::npos) {
		subject.replace(pos, search.length(), replace);
		pos += replace.length();
	}
	return subject;
}

void VectorDefiner::populate(std::vector<vector3d*>* space){
	if(this->is_file){
		//set up of internal data structures
		if (this->space != NULL) {
			delete this->space;
		}
		this->space = new std::vector<vector3d*>();
		if (this->vectors != NULL) {
			delete this->vectors;
		}
		this->vectors = new std::vector<vector3d*>();

		//I mean, Ideally, the file would be formatted as: x_space,y_space,z_space,x_vector,y_vector,z_vector\n
		//I can deal with whitespace, actually
		//std::string the_file = *(this->filename);
		std::string line;
		std::ifstream csv(this->filename->c_str());
		std::string newline("\n");
		std::string space(" ");
		if(csv.is_open()){ //read the file if it's open
			while(std::getline(csv, line)){ //read the file
				//line has the data inside of it now. like a civilized language and library should
				std::vector<std::string> eqrs;
				std::istringstream ss(line);
				std::string token;
				while (std::getline(ss, token, ',')) {
					//adds the newline, also replaces any newline tokens, which would be bad to have at the end.
					eqrs.push_back(replacer(token, newline, space));
				}
				//ok, now the items are inside eqrs;
				vector3d* spatial = new vector3d(atof(eqrs[0].c_str()), atof(eqrs[1].c_str()), atof(eqrs[2].c_str()));
				vector3d* vectorFiled = new vector3d(atof(eqrs[3].c_str()), atof(eqrs[4].c_str()), atof(eqrs[5].c_str()));
				this->space->push_back(spatial);
				this->vectors->push_back(vectorFiled);
			}
			csv.close();//be polite
		}
	}else{
		float* f = new float[3];

		if(this->vectors != NULL){
			delete this->vectors;
		}
		this->vectors = new std::vector<vector3d*>();

		for(unsigned int i=0; i<space->size(); ++i){
			float* temp = space->at(i)->xyz(); // get the spacial point
			f = eqr->eval(temp[0],temp[1],temp[2],f); //eval field at that point
			vector3d* vvv = new vector3d(f[0],f[1],f[2]); //make new vector to represent vector at that point in field
			this->vectors->push_back(vvv); //add it to out list of vectors
		}

		this->space = space;//EWW
		delete f;
	}
}

std::vector<vector3d*>* VectorDefiner::cull_vectors(float xmin, float xmax, float ymin, float ymax, float zmin, float zmax){
	std::vector<vector3d*>* vec = new std::vector<vector3d*>();

	vector3d* mmax = NULL;
	vector3d* mmin = NULL;
	vector3d* cmin = NULL;
	vector3d* cmax = NULL;

	for(unsigned int i=0; i<this->vectors->size(); ++i){
		vector3d* v = this->vectors->at(i);
		float* f = v->xyz();
		if(xmin < f[0] && f[0] < xmax){
			if(ymin < f[1] && f[1] < ymax){
				if(zmin < f[2] && f[2] < zmax){
					vec->push_back(v); //vector is in bounds, yay!
					//find culled min and max
					if (cmax == NULL || cmax->magnitude() < v->magnitude()) {
						cmax = v;
					}
					if (cmin == NULL || cmin->magnitude() > v->magnitude()) {
						cmin = v;
					}
				}
			}
		}
		//find min and max
		if(mmax == NULL || mmax->magnitude() < v->magnitude()){
			mmax = v;
		}
		if(mmin == NULL || mmin->magnitude() > v->magnitude()){
			mmin = v;
		}
	}
	//set up the cache
	this->min = mmin;
	this->max = mmax;
	this->cull_max = cmax;
	this->cull_min = cmin;
	//NOTE, I AM DELETING DATA
	if(this->culled_vectors != NULL) {
		delete this->culled_vectors;
	}
	this->culled_vectors = vec;
	//return
	return vec;
}

std::vector<vector3d*>* VectorDefiner::cull_space(float xmin, float xmax, float ymin, float ymax, float zmin, float zmax){
	std::vector<vector3d*>* vec = new std::vector<vector3d*>();

	for(unsigned int i=0; i<this->vectors->size(); ++i){
		vector3d* v = this->vectors->at(i);
		vector3d* space_v = this->space->at(i);
		float* f = v->xyz();
		if(xmin < f[0] && f[0] < xmax){
			if(ymin < f[1] && f[1] < ymax){
				if(zmin < f[2] && f[2] < zmax){
					vec->push_back(space_v); //vector is in bounds, yay!
				}
			}
		}
	}
	if(this->culled_space != NULL){
		delete this->culled_space;
	}
	this->culled_space = vec;
	return vec;
}

std::vector<vector3d*>* VectorDefiner::get_cull_vectors_cache(){
	return this->culled_vectors;
}
std::vector<vector3d*>* VectorDefiner::get_cull_space_cache(){
	return this->culled_space;
}
vector3d* VectorDefiner::get_vector_cull_min(){
	return this->cull_min;
}
vector3d* VectorDefiner::get_vector_cull_max(){
	return this->cull_max;
}
vector3d* VectorDefiner::get_vector_min() {
	return this->min;
}
vector3d* VectorDefiner::get_vector_max() {
	return this->max;
}<|endoftext|>


<commit_before>// Copyright (C) 2018 Elviss Strazdins
// This file is part of the Ouzel engine.

#include "RenderTargetResourceEmpty.hpp"

namespace ouzel
{
    namespace graphics
    {
    } // namespace graphics
} // namespace ouzel
<commit_msg>Fix the render target D3D11 include<commit_after>// Copyright (C) 2018 Elviss Strazdins
// This file is part of the Ouzel engine.

#include "RenderTargetResourceD3D11.hpp"

namespace ouzel
{
    namespace graphics
    {
    } // namespace graphics
} // namespace ouzel
<|endoftext|>


<commit_before>#include <Arduino.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>

#include "utils.h"
#include "hardwaredefs.h"
#include "motordrive.h"
#include "state.h"

Adafruit_BNO055 bno = Adafruit_BNO055(55);

struct ConfigData {
  uint16_t sig;
  ServoConfig lconfig;
  ServoConfig rconfig;
  double Kp;
  double Ki;
  double Kd;
  uint8_t checksum;
};

uint8_t checksum(uint8_t* data, int len) {
  uint8_t chk = 0;
  for (int n = 0; n < 0; n++) {
    chk += data[n];
  }
  return ~chk + 1;
}

void writeDefaultConfig() {
  ConfigData cd;

  cd.sig = 0xCAFE;

  cd.lconfig.zeropoint = 1500;
  cd.lconfig.range = 500;
  cd.rconfig.zeropoint = 1500;
  cd.rconfig.range = 500;

  cd.Ki = 1.0;
  cd.Kd = 1.0;
  cd.Kp = 1.0;

  cd.checksum = 0;

  cd.checksum = checksum((uint8_t*)&cd, sizeof(cd));

  writeStructEEPROM(cd, 0);

  Serial.println(F("\n  Default Config Written \n"));
}

bool readConfig(ConfigData& config) {
  readStructEEPROM(config, 0);
  uint8_t chk = config.checksum;
  config.checksum = 0;
  config.checksum = checksum((uint8_t*)&config, sizeof(config));

  return config.sig == 0xCAFE && chk == config.checksum;
}

void writeConfig(ConfigData& config) {
  writeStructEEPROM(config, 0);
  config.checksum = 0;
  config.checksum = checksum((uint8_t*)&config, sizeof(config));
}

struct GyroCalState : public State {
  void enter() { Serial.println(F("\n\nEntering: Gyro Offest Calibration")); }

  void action() {
    sensors_event_t event;
    bno.getEvent(&event);

    /* Display the floating point data */
    Serial.print("X: ");
    Serial.print(event.orientation.x, 4);
    Serial.print("\tY: ");
    Serial.print(event.orientation.y, 4);
    Serial.print("\tZ: ");
    Serial.print(event.orientation.z, 4);
    Serial.println("");

    delay(100);

    int ch = Serial.read();
    if (ch > 0) {
      switch (ch) {
        case 'q':
          stateGoto(NULL);
          break;
      }
    }
  }

  void leave() {}
};

GyroCalState gyroCalState;

struct ServoCalState : public State {
  Servo leftServo;
  Servo rightServo;

  ConfigData cd;
  MotorDrive drive;

  ServoCalState() : drive(&leftServo, &rightServo) {}

  void update() {
    drive.setServoConfig(cd.lconfig, cd.rconfig);
    drive.drive(0);

    Serial.print(cd.lconfig.zeropoint);
    Serial.print(' ');
    Serial.println(cd.rconfig.zeropoint);
  }

  void enter() {
    Serial.println(F("\n\nEntering: Servo Zero Point Calibration"));
    leftServo.attach(servoLeftPin);
    rightServo.attach(servoRightPin);

    if (!readConfig(cd)) {
      writeDefaultConfig();
      readConfig(cd);
    }

    update();
  }

  void action() {
    int ch = Serial.read();
    if (ch > 0) {
      switch (ch) {
        case 'q':
          stateGoto(NULL);
          break;

        case '1':
          cd.lconfig.zeropoint -= 10;
          break;
        case '2':
          cd.lconfig.zeropoint -= 1;
          break;
        case '3':
          cd.lconfig.zeropoint += 1;
          break;
        case '4':
          cd.lconfig.zeropoint += 10;
          break;

        case '7':
          cd.rconfig.zeropoint -= 10;
          break;
        case '8':
          cd.rconfig.zeropoint -= 1;
          break;
        case '9':
          cd.rconfig.zeropoint += 1;
          break;
        case '0':
          cd.rconfig.zeropoint += 10;
          break;
      }
      update();
    }
  }

  void leave() {
    leftServo.detach();
    rightServo.detach();

    writeConfig(cd);

    writeStructEEPROM(cd, 0);
  }
};

ServoCalState servoCalState;

struct MenuState : public State {
  int twirly;

  void enter() {
    Serial.println(F("Mk1 Calibration. Select"));
    Serial.println(F("  0: Default Config"));
    Serial.println(F("  1: Gyro Offest Calibration"));
    Serial.println(F("  2: Servo Zero Point Calibration"));

    twirly = 0;
  }

  void action() {
    int ch = Serial.read();
    if (ch > 0) {
      switch (ch) {
        case '1':
          stateGoto(&gyroCalState);
          break;
        case '2':
          stateGoto(&servoCalState);
          break;
        case '0':
          writeDefaultConfig();
          enter();
          break;
      }
    }
    delay(250);
    Serial.print("\\|/-"[twirly++]);
    Serial.print('\r');
    twirly &= 3;
  }

  void leave() {}
};

MenuState menuState;

StateMachine mainStateMachine;

void setup() {
  pinMode(8, OUTPUT);
  digitalWrite(8, HIGH);  // turn on the radio
  Serial.begin(115200);

  /* Initialise the sensor */
  if (!bno.begin()) {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.println(F("No BNO055 detected. Check your wiring or I2C ADDR!\n"));
    while (1)
      ;
  }

  Serial.println(F("\n\nBNO055 detected OK\n\n"));
  delay(1000);
  bno.setExtCrystalUse(true);
  pinMode(internalLEDPin, OUTPUT);

  // initial state is menu
  mainStateMachine.stateGoto(&menuState);
}

void loop() {
  if (mainStateMachine.currentState == NULL)
    mainStateMachine.stateGoto(&menuState);

  mainStateMachine.stateAction();
}
<commit_msg>fix checksum bug<commit_after>#include <Arduino.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <EEPROM.h>

#include "utils.h"
#include "hardwaredefs.h"
#include "motordrive.h"
#include "state.h"

Adafruit_BNO055 bno = Adafruit_BNO055(55);

struct ConfigData {
  uint16_t sig;
  ServoConfig lconfig;
  ServoConfig rconfig;
  double Kp;
  double Ki;
  double Kd;
  uint8_t checksum;
};

uint8_t checksum(uint8_t* data, int len) {
  uint8_t chk = 0;
  for (int n = 0; n < len; n++) {
    chk += data[n];
  }
  return ~chk + 1;
}

void writeDefaultConfig() {
  ConfigData cd;

  cd.sig = 0xCAFE;

  cd.lconfig.zeropoint = 1500;
  cd.lconfig.range = 500;
  cd.rconfig.zeropoint = 1500;
  cd.rconfig.range = 500;

  cd.Ki = 1.0;
  cd.Kd = 1.0;
  cd.Kp = 1.0;

  cd.checksum = 0;

  cd.checksum = checksum((uint8_t*)&cd, sizeof(cd));

  writeStructEEPROM(cd, 0);

  Serial.println(F("\n  Default Config Written \n"));
}

bool readConfig(ConfigData& config) {
  readStructEEPROM(config, 0);
  uint8_t chk = config.checksum;
  config.checksum = 0;
  config.checksum = checksum((uint8_t*)&config, sizeof(config));

  return config.sig == 0xCAFE && chk == config.checksum;
}

void writeConfig(ConfigData& config) {
  writeStructEEPROM(config, 0);
  config.checksum = 0;
  config.checksum = checksum((uint8_t*)&config, sizeof(config));
}

struct GyroCalState : public State {
  void enter() { Serial.println(F("\n\nEntering: Gyro Offest Calibration")); }

  void action() {
    sensors_event_t event;
    bno.getEvent(&event);

    /* Display the floating point data */
    Serial.print("X: ");
    Serial.print(event.orientation.x, 4);
    Serial.print("\tY: ");
    Serial.print(event.orientation.y, 4);
    Serial.print("\tZ: ");
    Serial.print(event.orientation.z, 4);
    Serial.println("");

    delay(100);

    int ch = Serial.read();
    if (ch > 0) {
      switch (ch) {
        case 'q':
          stateGoto(NULL);
          break;
      }
    }
  }

  void leave() {}
};

GyroCalState gyroCalState;

struct ServoCalState : public State {
  Servo leftServo;
  Servo rightServo;

  ConfigData cd;
  MotorDrive drive;

  ServoCalState() : drive(&leftServo, &rightServo) {}

  void update() {
    drive.setServoConfig(cd.lconfig, cd.rconfig);
    drive.drive(0);

    Serial.print(cd.lconfig.zeropoint);
    Serial.print(' ');
    Serial.println(cd.rconfig.zeropoint);
  }

  void enter() {
    Serial.println(F("\n\nEntering: Servo Zero Point Calibration"));
    leftServo.attach(servoLeftPin);
    rightServo.attach(servoRightPin);

    if (!readConfig(cd)) {
      writeDefaultConfig();
      readConfig(cd);
    }

    update();
  }

  void action() {
    int ch = Serial.read();
    if (ch > 0) {
      switch (ch) {
        case 'q':
          stateGoto(NULL);
          break;

        case '1':
          cd.lconfig.zeropoint -= 10;
          break;
        case '2':
          cd.lconfig.zeropoint -= 1;
          break;
        case '3':
          cd.lconfig.zeropoint += 1;
          break;
        case '4':
          cd.lconfig.zeropoint += 10;
          break;

        case '7':
          cd.rconfig.zeropoint -= 10;
          break;
        case '8':
          cd.rconfig.zeropoint -= 1;
          break;
        case '9':
          cd.rconfig.zeropoint += 1;
          break;
        case '0':
          cd.rconfig.zeropoint += 10;
          break;
      }
      update();
    }
  }

  void leave() {
    leftServo.detach();
    rightServo.detach();

    writeConfig(cd);

    writeStructEEPROM(cd, 0);
  }
};

ServoCalState servoCalState;

struct MenuState : public State {
  int twirly;

  void enter() {
    Serial.println(F("Mk1 Calibration. Select"));
    Serial.println(F("  0: Default Config"));
    Serial.println(F("  1: Gyro Offest Calibration"));
    Serial.println(F("  2: Servo Zero Point Calibration"));

    twirly = 0;
  }

  void action() {
    int ch = Serial.read();
    if (ch > 0) {
      switch (ch) {
        case '1':
          stateGoto(&gyroCalState);
          break;
        case '2':
          stateGoto(&servoCalState);
          break;
        case '0':
          writeDefaultConfig();
          enter();
          break;
      }
    }
    delay(250);
    Serial.print("\\|/-"[twirly++]);
    Serial.print('\r');
    twirly &= 3;
  }

  void leave() {}
};

MenuState menuState;

StateMachine mainStateMachine;

void setup() {
  pinMode(8, OUTPUT);
  digitalWrite(8, HIGH);  // turn on the radio
  Serial.begin(115200);

  /* Initialise the sensor */
  if (!bno.begin()) {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.println(F("No BNO055 detected. Check your wiring or I2C ADDR!\n"));
    while (1)
      ;
  }

  Serial.println(F("\n\nBNO055 detected OK\n\n"));
  delay(1000);
  bno.setExtCrystalUse(true);
  pinMode(internalLEDPin, OUTPUT);

  // initial state is menu
  mainStateMachine.stateGoto(&menuState);
}

void loop() {
  if (mainStateMachine.currentState == NULL)
    mainStateMachine.stateGoto(&menuState);

  mainStateMachine.stateAction();
}
<|endoftext|>


<commit_before>/*
 * Copyright 2011-2013 Branimir Karadzic. All rights reserved.
 * License: http://www.opensource.org/licenses/BSD-2-Clause
 */

#include "entry_p.h"

#if ENTRY_CONFIG_USE_NATIVE && BX_PLATFORM_LINUX

#define XK_MISCELLANY
#define XK_LATIN1
#include <X11/keysymdef.h>
#include <bgfxplatform.h> // will include X11 which #defines None... Don't mess with order of includes.

#undef None
#include <bx/thread.h>
#include <bx/os.h>
#include <string.h> // memset

namespace entry
{
	static uint8_t s_translateKey[512];

	static void initTranslateKey(uint16_t _xk, Key::Enum _key)
	{
		_xk += 256;
		BX_CHECK(_xk < BX_COUNTOF(s_translateKey), "Out of bounds %d.", _xk);
		s_translateKey[_xk&0x1ff] = (uint8_t)_key;
	}

	Key::Enum fromXk(uint16_t _xk)
	{
		_xk += 256;
		return 512 > _xk ? (Key::Enum)s_translateKey[_xk] : Key::None;
	}

	struct MainThreadEntry
	{
		int m_argc;
		char** m_argv;

		static int32_t threadFunc(void* _userData);
	};

	struct Context
	{
		Context()
			: m_modifiers(Modifier::None)
			, m_exit(false)
		{
			memset(s_translateKey, 0, sizeof(s_translateKey) );
			initTranslateKey(XK_Escape,       Key::Esc);
			initTranslateKey(XK_Return,       Key::Return);
			initTranslateKey(XK_Tab,          Key::Tab);
			initTranslateKey(XK_BackSpace,    Key::Backspace);
			initTranslateKey(XK_space,        Key::Space);
			initTranslateKey(XK_Up,           Key::Up);
			initTranslateKey(XK_Down,         Key::Down);
			initTranslateKey(XK_Left,         Key::Left);
			initTranslateKey(XK_Right,        Key::Right);
			initTranslateKey(XK_Page_Up,      Key::PageUp);
			initTranslateKey(XK_Page_Down,    Key::PageUp);
			initTranslateKey(XK_Home,         Key::Home);
			initTranslateKey(XK_KP_End,       Key::End);
			initTranslateKey(XK_Print,        Key::Print);
			initTranslateKey(XK_equal,        Key::Plus);
			initTranslateKey(XK_minus,        Key::Minus);
			initTranslateKey(XK_F1,           Key::F1);
			initTranslateKey(XK_F2,           Key::F2);
			initTranslateKey(XK_F3,           Key::F3);
			initTranslateKey(XK_F4,           Key::F4);
			initTranslateKey(XK_F5,           Key::F5);
			initTranslateKey(XK_F6,           Key::F6);
			initTranslateKey(XK_F7,           Key::F7);
			initTranslateKey(XK_F8,           Key::F8);
			initTranslateKey(XK_F9,           Key::F9);
			initTranslateKey(XK_F10,          Key::F10);
			initTranslateKey(XK_F11,          Key::F11);
			initTranslateKey(XK_F12,          Key::F12);
			initTranslateKey(XK_KP_Insert,    Key::NumPad0);
			initTranslateKey(XK_KP_End,       Key::NumPad1);
			initTranslateKey(XK_KP_Down,      Key::NumPad2);
			initTranslateKey(XK_KP_Page_Down, Key::NumPad3);
			initTranslateKey(XK_KP_Left,      Key::NumPad4);
			initTranslateKey(XK_KP_Begin,     Key::NumPad5);
			initTranslateKey(XK_KP_Right,     Key::NumPad6);
			initTranslateKey(XK_KP_Home,      Key::NumPad7);
			initTranslateKey(XK_KP_Up,        Key::NumPad8);
			initTranslateKey(XK_KP_Page_Up,   Key::NumPad9);
			initTranslateKey('0',             Key::Key0);
			initTranslateKey('1',             Key::Key1);
			initTranslateKey('2',             Key::Key2);
			initTranslateKey('3',             Key::Key3);
			initTranslateKey('4',             Key::Key4);
			initTranslateKey('5',             Key::Key5);
			initTranslateKey('6',             Key::Key6);
			initTranslateKey('7',             Key::Key7);
			initTranslateKey('8',             Key::Key8);
			initTranslateKey('9',             Key::Key9);
			initTranslateKey('a',             Key::KeyA);
			initTranslateKey('b',             Key::KeyB);
			initTranslateKey('c',             Key::KeyC);
			initTranslateKey('d',             Key::KeyD);
			initTranslateKey('e',             Key::KeyE);
			initTranslateKey('f',             Key::KeyF);
			initTranslateKey('g',             Key::KeyG);
			initTranslateKey('h',             Key::KeyH);
			initTranslateKey('i',             Key::KeyI);
			initTranslateKey('j',             Key::KeyJ);
			initTranslateKey('k',             Key::KeyK);
			initTranslateKey('l',             Key::KeyL);
			initTranslateKey('m',             Key::KeyM);
			initTranslateKey('n',             Key::KeyN);
			initTranslateKey('o',             Key::KeyO);
			initTranslateKey('p',             Key::KeyP);
			initTranslateKey('q',             Key::KeyQ);
			initTranslateKey('r',             Key::KeyR);
			initTranslateKey('s',             Key::KeyS);
			initTranslateKey('t',             Key::KeyT);
			initTranslateKey('u',             Key::KeyU);
			initTranslateKey('v',             Key::KeyV);
			initTranslateKey('w',             Key::KeyW);
			initTranslateKey('x',             Key::KeyX);
			initTranslateKey('y',             Key::KeyY);
			initTranslateKey('z',             Key::KeyZ);
		}

		int32_t run(int _argc, char** _argv)
		{
			XInitThreads();
			m_display = XOpenDisplay(0);

			int32_t screen = DefaultScreen(m_display);
			int32_t depth = DefaultDepth(m_display, screen);
			Visual* visual = DefaultVisual(m_display, screen);
			Window root = RootWindow(m_display, screen);

			XSetWindowAttributes windowAttrs;
			memset(&windowAttrs, 0, sizeof(windowAttrs) );
			windowAttrs.background_pixmap = 0;
			windowAttrs.border_pixel = 0;
			windowAttrs.event_mask = 0
					| ButtonPressMask
					| ButtonReleaseMask
					| ExposureMask
					| KeyPressMask
					| KeyReleaseMask
					| PointerMotionMask
					| ResizeRedirectMask
					| StructureNotifyMask
					;

			m_window = XCreateWindow(m_display
									, root
									, 0, 0
									, ENTRY_DEFAULT_WIDTH, ENTRY_DEFAULT_HEIGHT, 0, depth
									, InputOutput
									, visual
									, CWBorderPixel|CWEventMask
									, &windowAttrs
									);

			XMapWindow(m_display, m_window);
			XStoreName(m_display, m_window, "BGFX");

			bgfx::x11SetDisplayWindow(m_display, m_window);

			MainThreadEntry mte;
			mte.m_argc = _argc;
			mte.m_argv = _argv;

			bx::Thread thread;
			thread.init(mte.threadFunc, &mte);

			while (!m_exit)
			{
				if (XPending(m_display) )
				{
					XEvent event;
					XNextEvent(m_display, &event);

					switch (event.type)
					{
						case Expose:
							break;

						case ConfigureNotify:
							break;

						case ButtonPress:
						case ButtonRelease:
							{
								const XButtonEvent& xbutton = event.xbutton;
								MouseButton::Enum mb;
								switch (xbutton.button)
								{
									case Button1: mb = MouseButton::Left;   break;
									case Button2: mb = MouseButton::Middle; break;
									case Button3: mb = MouseButton::Right;  break;
									default:      mb = MouseButton::None;   break;
								}

								if (MouseButton::None != mb)
								{
									m_eventQueue.postMouseEvent(xbutton.x
										, xbutton.y
										, mb
										, event.type == ButtonPress
										);
								}
							}
							break;

						case MotionNotify:
							{
								const XMotionEvent& xmotion = event.xmotion;
								m_eventQueue.postMouseEvent(xmotion.x
										, xmotion.y
										);
							}
							break;

						case KeyPress:
						case KeyRelease:
							{
								XKeyEvent& xkey = event.xkey;
								KeySym keysym = XLookupKeysym(&xkey, 0);
								switch (keysym)
								{
								case XK_Meta_L:    setModifier(Modifier::LeftMeta,   KeyPress == event.type); break;
								case XK_Meta_R:    setModifier(Modifier::RightMeta,  KeyPress == event.type); break;
								case XK_Control_L: setModifier(Modifier::LeftCtrl,   KeyPress == event.type); break;
								case XK_Control_R: setModifier(Modifier::RightCtrl,  KeyPress == event.type); break;
								case XK_Shift_L:   setModifier(Modifier::LeftShift,  KeyPress == event.type); break;
								case XK_Shift_R:   setModifier(Modifier::RightShift, KeyPress == event.type); break;
								case XK_Alt_L:     setModifier(Modifier::LeftAlt,    KeyPress == event.type); break;
								case XK_Alt_R:     setModifier(Modifier::RightAlt,   KeyPress == event.type); break;

								default:
									{
										Key::Enum key = fromXk(keysym);
										if (Key::None != key)
										{
											m_eventQueue.postKeyEvent(key, m_modifiers, KeyPress == event.type);
										}
									}
									break;
								}
							}
							break;

						case ResizeRequest:
							{
								const XResizeRequestEvent& xresize = event.xresizerequest;
								XResizeWindow(m_display, m_window, xresize.width, xresize.height);
							}
							break;
					}
				}
			}

			thread.shutdown();

			XUnmapWindow(m_display, m_window);
			XDestroyWindow(m_display, m_window);

			return EXIT_SUCCESS;
		}

		void setModifier(Modifier::Enum _modifier, bool _set)
		{
			m_modifiers &= ~_modifier;
			m_modifiers |= _set ? _modifier : 0;
		}

		uint8_t m_modifiers;
		Display* m_display;
		Window m_window;
		bool m_exit;

		EventQueue m_eventQueue;
	};

	static Context s_ctx;

	int32_t MainThreadEntry::threadFunc(void* _userData)
	{
		MainThreadEntry* self = (MainThreadEntry*)_userData;
		int32_t result = main(self->m_argc, self->m_argv);
		s_ctx.m_exit = true;
		return result;
	}

	const Event* poll()
	{
		return s_ctx.m_eventQueue.poll();
	}

	void release(const Event* _event)
	{
		s_ctx.m_eventQueue.release(_event);
	}

	void setWindowSize(uint32_t _width, uint32_t _height)
	{
		XResizeRequestEvent ev;
		ev.type = ResizeRequest;
		ev.serial = 0;
		ev.send_event = true;
		ev.display = s_ctx.m_display;
		ev.window = s_ctx.m_window;
		ev.width = (int)_width;
		ev.height = (int)_height;
		XSendEvent(s_ctx.m_display, s_ctx.m_window, false, ResizeRedirectMask, (XEvent*)&ev);
	}

	void toggleWindowFrame()
	{
	}

	void setMouseLock(bool _lock)
	{
	}

} // namespace entry

int main(int _argc, char** _argv)
{
	using namespace entry;
	return s_ctx.run(_argc, _argv);
}

#endif // ENTRY_CONFIG_USE_NATIVE && BX_PLATFORM_LINUX
<commit_msg>Handle exit message from window manager on Linux<commit_after>/*
 * Copyright 2011-2013 Branimir Karadzic. All rights reserved.
 * License: http://www.opensource.org/licenses/BSD-2-Clause
 */

#include "entry_p.h"

#if ENTRY_CONFIG_USE_NATIVE && BX_PLATFORM_LINUX

#define XK_MISCELLANY
#define XK_LATIN1
#include <X11/keysymdef.h>
#include <bgfxplatform.h> // will include X11 which #defines None... Don't mess with order of includes.

#undef None
#include <bx/thread.h>
#include <bx/os.h>
#include <string.h> // memset

namespace entry
{
	static uint8_t s_translateKey[512];

	static void initTranslateKey(uint16_t _xk, Key::Enum _key)
	{
		_xk += 256;
		BX_CHECK(_xk < BX_COUNTOF(s_translateKey), "Out of bounds %d.", _xk);
		s_translateKey[_xk&0x1ff] = (uint8_t)_key;
	}

	Key::Enum fromXk(uint16_t _xk)
	{
		_xk += 256;
		return 512 > _xk ? (Key::Enum)s_translateKey[_xk] : Key::None;
	}

	struct MainThreadEntry
	{
		int m_argc;
		char** m_argv;

		static int32_t threadFunc(void* _userData);
	};

	struct Context
	{
		Context()
			: m_modifiers(Modifier::None)
			, m_exit(false)
		{
			memset(s_translateKey, 0, sizeof(s_translateKey) );
			initTranslateKey(XK_Escape,       Key::Esc);
			initTranslateKey(XK_Return,       Key::Return);
			initTranslateKey(XK_Tab,          Key::Tab);
			initTranslateKey(XK_BackSpace,    Key::Backspace);
			initTranslateKey(XK_space,        Key::Space);
			initTranslateKey(XK_Up,           Key::Up);
			initTranslateKey(XK_Down,         Key::Down);
			initTranslateKey(XK_Left,         Key::Left);
			initTranslateKey(XK_Right,        Key::Right);
			initTranslateKey(XK_Page_Up,      Key::PageUp);
			initTranslateKey(XK_Page_Down,    Key::PageUp);
			initTranslateKey(XK_Home,         Key::Home);
			initTranslateKey(XK_KP_End,       Key::End);
			initTranslateKey(XK_Print,        Key::Print);
			initTranslateKey(XK_equal,        Key::Plus);
			initTranslateKey(XK_minus,        Key::Minus);
			initTranslateKey(XK_F1,           Key::F1);
			initTranslateKey(XK_F2,           Key::F2);
			initTranslateKey(XK_F3,           Key::F3);
			initTranslateKey(XK_F4,           Key::F4);
			initTranslateKey(XK_F5,           Key::F5);
			initTranslateKey(XK_F6,           Key::F6);
			initTranslateKey(XK_F7,           Key::F7);
			initTranslateKey(XK_F8,           Key::F8);
			initTranslateKey(XK_F9,           Key::F9);
			initTranslateKey(XK_F10,          Key::F10);
			initTranslateKey(XK_F11,          Key::F11);
			initTranslateKey(XK_F12,          Key::F12);
			initTranslateKey(XK_KP_Insert,    Key::NumPad0);
			initTranslateKey(XK_KP_End,       Key::NumPad1);
			initTranslateKey(XK_KP_Down,      Key::NumPad2);
			initTranslateKey(XK_KP_Page_Down, Key::NumPad3);
			initTranslateKey(XK_KP_Left,      Key::NumPad4);
			initTranslateKey(XK_KP_Begin,     Key::NumPad5);
			initTranslateKey(XK_KP_Right,     Key::NumPad6);
			initTranslateKey(XK_KP_Home,      Key::NumPad7);
			initTranslateKey(XK_KP_Up,        Key::NumPad8);
			initTranslateKey(XK_KP_Page_Up,   Key::NumPad9);
			initTranslateKey('0',             Key::Key0);
			initTranslateKey('1',             Key::Key1);
			initTranslateKey('2',             Key::Key2);
			initTranslateKey('3',             Key::Key3);
			initTranslateKey('4',             Key::Key4);
			initTranslateKey('5',             Key::Key5);
			initTranslateKey('6',             Key::Key6);
			initTranslateKey('7',             Key::Key7);
			initTranslateKey('8',             Key::Key8);
			initTranslateKey('9',             Key::Key9);
			initTranslateKey('a',             Key::KeyA);
			initTranslateKey('b',             Key::KeyB);
			initTranslateKey('c',             Key::KeyC);
			initTranslateKey('d',             Key::KeyD);
			initTranslateKey('e',             Key::KeyE);
			initTranslateKey('f',             Key::KeyF);
			initTranslateKey('g',             Key::KeyG);
			initTranslateKey('h',             Key::KeyH);
			initTranslateKey('i',             Key::KeyI);
			initTranslateKey('j',             Key::KeyJ);
			initTranslateKey('k',             Key::KeyK);
			initTranslateKey('l',             Key::KeyL);
			initTranslateKey('m',             Key::KeyM);
			initTranslateKey('n',             Key::KeyN);
			initTranslateKey('o',             Key::KeyO);
			initTranslateKey('p',             Key::KeyP);
			initTranslateKey('q',             Key::KeyQ);
			initTranslateKey('r',             Key::KeyR);
			initTranslateKey('s',             Key::KeyS);
			initTranslateKey('t',             Key::KeyT);
			initTranslateKey('u',             Key::KeyU);
			initTranslateKey('v',             Key::KeyV);
			initTranslateKey('w',             Key::KeyW);
			initTranslateKey('x',             Key::KeyX);
			initTranslateKey('y',             Key::KeyY);
			initTranslateKey('z',             Key::KeyZ);
		}

		int32_t run(int _argc, char** _argv)
		{
			XInitThreads();
			m_display = XOpenDisplay(0);

			int32_t screen = DefaultScreen(m_display);
			int32_t depth = DefaultDepth(m_display, screen);
			Visual* visual = DefaultVisual(m_display, screen);
			Window root = RootWindow(m_display, screen);

			XSetWindowAttributes windowAttrs;
			memset(&windowAttrs, 0, sizeof(windowAttrs) );
			windowAttrs.background_pixmap = 0;
			windowAttrs.border_pixel = 0;
			windowAttrs.event_mask = 0
					| ButtonPressMask
					| ButtonReleaseMask
					| ExposureMask
					| KeyPressMask
					| KeyReleaseMask
					| PointerMotionMask
					| ResizeRedirectMask
					| StructureNotifyMask
					;

			m_window = XCreateWindow(m_display
									, root
									, 0, 0
									, ENTRY_DEFAULT_WIDTH, ENTRY_DEFAULT_HEIGHT, 0, depth
									, InputOutput
									, visual
									, CWBorderPixel|CWEventMask
									, &windowAttrs
									);

			const char *wmDeleteWindowName = "WM_DELETE_WINDOW";
			Atom wmDeleteWindow;
			XInternAtoms(m_display, (char **)&wmDeleteWindowName, 1, False, &wmDeleteWindow);
			XSetWMProtocols(m_display, m_window, &wmDeleteWindow, 1);

			XMapWindow(m_display, m_window);
			XStoreName(m_display, m_window, "BGFX");

			bgfx::x11SetDisplayWindow(m_display, m_window);

			MainThreadEntry mte;
			mte.m_argc = _argc;
			mte.m_argv = _argv;

			bx::Thread thread;
			thread.init(mte.threadFunc, &mte);

			while (!m_exit)
			{
				if (XPending(m_display) )
				{
					XEvent event;
					XNextEvent(m_display, &event);

					switch (event.type)
					{
						case Expose:
							break;

						case ConfigureNotify:
							break;

						case ClientMessage:
							if((Atom)event.xclient.data.l[0] == wmDeleteWindow)
							{
								m_eventQueue.postExitEvent();
							}
							break;

						case ButtonPress:
						case ButtonRelease:
							{
								const XButtonEvent& xbutton = event.xbutton;
								MouseButton::Enum mb;
								switch (xbutton.button)
								{
									case Button1: mb = MouseButton::Left;   break;
									case Button2: mb = MouseButton::Middle; break;
									case Button3: mb = MouseButton::Right;  break;
									default:      mb = MouseButton::None;   break;
								}

								if (MouseButton::None != mb)
								{
									m_eventQueue.postMouseEvent(xbutton.x
										, xbutton.y
										, mb
										, event.type == ButtonPress
										);
								}
							}
							break;

						case MotionNotify:
							{
								const XMotionEvent& xmotion = event.xmotion;
								m_eventQueue.postMouseEvent(xmotion.x
										, xmotion.y
										);
							}
							break;

						case KeyPress:
						case KeyRelease:
							{
								XKeyEvent& xkey = event.xkey;
								KeySym keysym = XLookupKeysym(&xkey, 0);
								switch (keysym)
								{
								case XK_Meta_L:    setModifier(Modifier::LeftMeta,   KeyPress == event.type); break;
								case XK_Meta_R:    setModifier(Modifier::RightMeta,  KeyPress == event.type); break;
								case XK_Control_L: setModifier(Modifier::LeftCtrl,   KeyPress == event.type); break;
								case XK_Control_R: setModifier(Modifier::RightCtrl,  KeyPress == event.type); break;
								case XK_Shift_L:   setModifier(Modifier::LeftShift,  KeyPress == event.type); break;
								case XK_Shift_R:   setModifier(Modifier::RightShift, KeyPress == event.type); break;
								case XK_Alt_L:     setModifier(Modifier::LeftAlt,    KeyPress == event.type); break;
								case XK_Alt_R:     setModifier(Modifier::RightAlt,   KeyPress == event.type); break;

								default:
									{
										Key::Enum key = fromXk(keysym);
										if (Key::None != key)
										{
											m_eventQueue.postKeyEvent(key, m_modifiers, KeyPress == event.type);
										}
									}
									break;
								}
							}
							break;

						case ResizeRequest:
							{
								const XResizeRequestEvent& xresize = event.xresizerequest;
								XResizeWindow(m_display, m_window, xresize.width, xresize.height);
							}
							break;
					}
				}
			}

			thread.shutdown();

			XUnmapWindow(m_display, m_window);
			XDestroyWindow(m_display, m_window);

			return EXIT_SUCCESS;
		}

		void setModifier(Modifier::Enum _modifier, bool _set)
		{
			m_modifiers &= ~_modifier;
			m_modifiers |= _set ? _modifier : 0;
		}

		uint8_t m_modifiers;
		Display* m_display;
		Window m_window;
		bool m_exit;

		EventQueue m_eventQueue;
	};

	static Context s_ctx;

	int32_t MainThreadEntry::threadFunc(void* _userData)
	{
		MainThreadEntry* self = (MainThreadEntry*)_userData;
		int32_t result = main(self->m_argc, self->m_argv);
		s_ctx.m_exit = true;
		return result;
	}

	const Event* poll()
	{
		return s_ctx.m_eventQueue.poll();
	}

	void release(const Event* _event)
	{
		s_ctx.m_eventQueue.release(_event);
	}

	void setWindowSize(uint32_t _width, uint32_t _height)
	{
		XResizeRequestEvent ev;
		ev.type = ResizeRequest;
		ev.serial = 0;
		ev.send_event = true;
		ev.display = s_ctx.m_display;
		ev.window = s_ctx.m_window;
		ev.width = (int)_width;
		ev.height = (int)_height;
		XSendEvent(s_ctx.m_display, s_ctx.m_window, false, ResizeRedirectMask, (XEvent*)&ev);
	}

	void toggleWindowFrame()
	{
	}

	void setMouseLock(bool _lock)
	{
	}

} // namespace entry

int main(int _argc, char** _argv)
{
	using namespace entry;
	return s_ctx.run(_argc, _argv);
}

#endif // ENTRY_CONFIG_USE_NATIVE && BX_PLATFORM_LINUX
<|endoftext|>


<commit_before>/**
 * This file is part of TelepathyQt4
 *
 * @copyright Copyright (C) 2011 Collabora Ltd. <http://www.collabora.co.uk/>
 * @copyright Copyright (C) 2011 Nokia Corporation
 * @license LGPL 2.1
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * 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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include "sender.h"
#include "_gen/sender.moc.hpp"

#include <TelepathyQt4/Account>
#include <TelepathyQt4/Debug>
#include <TelepathyQt4/Constants>
#include <TelepathyQt4/ContactMessenger>
#include <TelepathyQt4/PendingSendMessage>
#include <TelepathyQt4/Types>

#include <QCoreApplication>

Sender::Sender(const QString &accountPath,
        const QString &contactIdentifier, const QString &message)
{
    Tp::AccountPtr acc = Tp::Account::create(TP_QT4_IFACE_ACCOUNT, accountPath);
    messenger = Tp::ContactMessenger::create(acc, contactIdentifier);
    connect(messenger->sendMessage(message),
            SIGNAL(finished(Tp::PendingOperation*)),
            SLOT(onSendMessageFinished(Tp::PendingOperation*)));
}

Sender::~Sender()
{
}

void Sender::onSendMessageFinished(Tp::PendingOperation *op)
{
    if (op->isError()) {
        qDebug() << "Error sending message:" << op->errorName() << "-" << op->errorMessage();
        QCoreApplication::exit(1);
        return;
    }

    Tp::PendingSendMessage *psm = qobject_cast<Tp::PendingSendMessage *>(op);
    qDebug() << "Message sent, token is" << psm->sentMessageToken();
    QCoreApplication::exit(0);
}

int main(int argc, char **argv)
{
    QCoreApplication app(argc, argv);

    Tp::registerTypes();
    Tp::enableDebug(true);
    Tp::enableWarnings(true);

    if (argc < 4) {
        qDebug() << "Usage: contact-messenger account_path contact_id message";
        return -1;
    }

    Sender *sender = new Sender(QLatin1String(argv[1]), QLatin1String(argv[2]),
            QLatin1String(argv[3]));

    int ret = app.exec();
    delete sender;
    return ret;
}
<commit_msg>contact-messenger example: Properly create account.<commit_after>/**
 * This file is part of TelepathyQt4
 *
 * @copyright Copyright (C) 2011 Collabora Ltd. <http://www.collabora.co.uk/>
 * @copyright Copyright (C) 2011 Nokia Corporation
 * @license LGPL 2.1
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * 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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include "sender.h"
#include "_gen/sender.moc.hpp"

#include <TelepathyQt4/Account>
#include <TelepathyQt4/Debug>
#include <TelepathyQt4/Constants>
#include <TelepathyQt4/ContactMessenger>
#include <TelepathyQt4/PendingSendMessage>
#include <TelepathyQt4/Types>

#include <QCoreApplication>

Sender::Sender(const QString &accountPath,
        const QString &contactIdentifier, const QString &message)
{
    Tp::AccountPtr acc = Tp::Account::create(TP_QT4_ACCOUNT_MANAGER_BUS_NAME,
            accountPath);
    messenger = Tp::ContactMessenger::create(acc, contactIdentifier);
    connect(messenger->sendMessage(message),
            SIGNAL(finished(Tp::PendingOperation*)),
            SLOT(onSendMessageFinished(Tp::PendingOperation*)));
}

Sender::~Sender()
{
}

void Sender::onSendMessageFinished(Tp::PendingOperation *op)
{
    if (op->isError()) {
        qDebug() << "Error sending message:" << op->errorName() << "-" << op->errorMessage();
        QCoreApplication::exit(1);
        return;
    }

    Tp::PendingSendMessage *psm = qobject_cast<Tp::PendingSendMessage *>(op);
    qDebug() << "Message sent, token is" << psm->sentMessageToken();
    QCoreApplication::exit(0);
}

int main(int argc, char **argv)
{
    QCoreApplication app(argc, argv);

    Tp::registerTypes();
    Tp::enableDebug(true);
    Tp::enableWarnings(true);

    if (argc < 4) {
        qDebug() << "Usage: contact-messenger account_path contact_id message";
        return -1;
    }

    Sender *sender = new Sender(QLatin1String(argv[1]), QLatin1String(argv[2]),
            QLatin1String(argv[3]));

    int ret = app.exec();
    delete sender;
    return ret;
}
<|endoftext|>


<commit_before># include <cstdlib> // std :: rand ()
# include <vector> // std :: vector <>
# include <list> // std :: list <>
# include <iostream> // std :: cout
# include <iterator> // std :: ostream_iterator <>
# include <algorithm> // std :: reverse , std :: generate
# include <set>

int main ()
{

	std::list <unsigned int> l1(100);
	for (auto& l : l1){
		l = std::rand() % 101;
	}
	// std::copy(std::begin(l1), std::end(l1),
	// 	std::ostream_iterator<int>(std::cout, "\n"));

	std::vector<unsigned int> v1(l1.size());
	std::copy(std::begin(l1), std::end(l1), std::begin(v1));
	// std::copy(std::begin(v1), std::end(v1),
	// 	std::ostream_iterator<int>(std::cout, "\n"));

	std::set<int> s1;
	for(auto& i : v1){
		s1.insert(i);
	}
	std::copy(std::begin(s1), std::end(s1),
		std::ostream_iterator<int>(std::cout, "\n"));

	std::cout << "In der Liste stehen " << s1.size() << " verschiedene Zahlen. \n";

	std::cout << "Diese Zahlen zwischen 0 und 100 fehlen: \n";

	for (int j = 0; j <= 100; ++j){
		if(s1.find(j) == s1.end()){
			std::cout << j << "\n";
		}
	}

	return 0;
}<commit_msg>Add 3.4 Map<commit_after># include <cstdlib> // std :: rand ()
# include <vector> // std :: vector <>
# include <list> // std :: list <>
# include <iostream> // std :: cout
# include <iterator> // std :: ostream_iterator <>
# include <algorithm> // std :: reverse , std :: generate
# include <set>
#include <map>

int main ()
{

	std::list <unsigned int> l1(100);
	//Liste mit random-Zahlen füllen
	for (auto& l : l1){
		l = std::rand() % 101;
	}
	// std::copy(std::begin(l1), std::end(l1),
	// 	std::ostream_iterator<int>(std::cout, "\n"));

	std::vector<unsigned int> v1(l1.size());
	//Inhalt der Liste in Vektor kopieren
	std::copy(std::begin(l1), std::end(l1), std::begin(v1));
	std::copy(std::begin(v1), std::end(v1),
		std::ostream_iterator<int>(std::cout, "\n"));

	std::set<int> s1;
	//Inhalt des Vektors in set sortieren (doppelte werden nur 1x einsortiert)
	for(auto& i : v1){
		s1.insert(i);
	}
	std::copy(std::begin(s1), std::end(s1),
	//set ausgeben
		std::ostream_iterator<int>(std::cout, "\n"));

	std::cout << "In der Liste stehen " << s1.size() << " verschiedene Zahlen. \n";

	std::cout << "Diese Zahlen zwischen 0 und 100 fehlen: \n";

	for (int j = 0; j <= 100; ++j){
		//alle Zahlen zw 1 u 100 finden, die nicht im set stehen.
		if(s1.find(j) == s1.end()){
			std::cout << j << "\n";
		}
	}

	std::map<int, int> map1;
	/* mit map, weil man bei map der jeweiligen Zahl (key) einen Wert (value)
	*zuweisen und diesen entsprechend erhöhen kann, wenn die Zahl erneut
	*gefunden wird. Map ist ein assoziativer Container. */
	int i{0};
	int c;
	while(i<v1.size()){
		c = v1[i];
		++map1[c];
		/*Jede Zahl, die in v1 gefunden wird, wird mit Schlüssel 0 hinzugefügt
		*und dann durch ++ um 1 erhöht. Wird sie nochmal gefunden, wird der
		*Schlüssel nochmal um 1 erhöht */
		++i;
	}

	typedef std::map<int,int>::const_iterator Iter;
	//Ausgabe der map als key : value
	for(Iter p = map1.begin(); p!= map1.end(); ++p){
		std::cout << p -> first << " : " << p -> second << '\n';
	}

	return 0;
}
<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  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 the Willow Garage 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 THE COPYRIGHT HOLDERS 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 THE
*  COPYRIGHT OWNER 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.
*********************************************************************/

/* Author: Wim Meeussen */

#include "kdl_format_io/urdf_import.hpp"
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
#include <console_bridge/console.h>
#include <fstream>
#include <iostream>

using namespace std;
using namespace KDL;

namespace kdl_format_io{

void printTree(boost::shared_ptr<const urdf::Link> link,int level = 0)
{
  level+=2;
  int count = 0;
  for (std::vector<boost::shared_ptr<urdf::Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
  {
    if (*child)
    {
      for(int j=0;j<level;j++) std::cout << "  "; //indent
      std::cout << "child(" << (count++)+1 << "):  " << (*child)->name  << std::endl;
      // first grandchild
      printTree(*child,level);
    }
    else
    {
      for(int j=0;j<level;j++) std::cout << " "; //indent
      std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
    }
  }

}

// construct vector
Vector toKdl(urdf::Vector3 v)
{
  return Vector(v.x, v.y, v.z);
}

// construct rotation
Rotation toKdl(urdf::Rotation r)
{
  return Rotation::Quaternion(r.x, r.y, r.z, r.w);
}

// construct pose
Frame toKdl(urdf::Pose p)
{
  return Frame(toKdl(p.rotation), toKdl(p.position));
}

// construct joint
Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
{
  Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);

  switch (jnt->type){
  case urdf::Joint::FIXED:{
    return Joint(jnt->name, Joint::None);
  }
  case urdf::Joint::REVOLUTE:{
    Vector axis = toKdl(jnt->axis);
    return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
  }
  case urdf::Joint::CONTINUOUS:{
    Vector axis = toKdl(jnt->axis);
    return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
  }
  case urdf::Joint::PRISMATIC:{
    Vector axis = toKdl(jnt->axis);
    return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
  }
  default:{
    logWarn("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
    return Joint(jnt->name, Joint::None);
  }
  }
  return Joint();
}

// construct inertia
RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
{
  Frame origin = toKdl(i->origin);
  // kdl and urdf both specify the com position in the reference frame of the link
  // kdl specifies the inertia in the reference frame of the link, the urdf specifies the inertia in the inertia reference frame
  return  origin.M *RigidBodyInertia(i->mass, origin.p, RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
}

// recursive function to walk through tree
bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
{
  std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links;
  logDebug("Link %s had %i children", root->name.c_str(), (int)children.size());

  // constructs the optional inertia
  RigidBodyInertia inert(0);
  if (root->inertial)
    inert = toKdl(root->inertial);

  // constructs the kdl joint
  Joint jnt = toKdl(root->parent_joint);

  // construct the kdl segment
  Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);

  // add segment to tree
  tree.addSegment(sgm, root->parent_joint->parent_link_name);

  // recurslively add all children
  for (size_t i=0; i<children.size(); i++){
    if (!addChildrenToTree(children[i], tree))
      return false;
  }
  return true;
}

bool treeFromUrdfFile(const string& file, Tree& tree)
{
    ifstream ifs(file.c_str());
    std::string xml_string( (std::istreambuf_iterator<char>(ifs) ),
                       (std::istreambuf_iterator<char>()    ) );

    return treeFromUrdfString(xml_string,tree);
}

/*
bool treeFromParam(const string& param, Tree& tree)
{
  urdf::ModelInterface robot_model;
  if (!robot_model.initParam(param)){
    logError("Could not generate robot model");
    return false;
  }
  return treeFromUrdfModel(robot_model, tree);
}
*/

int print_tree(const boost::shared_ptr<urdf::ModelInterface> & robot) {
std::cout << "robot name is: " << robot->getName() << std::endl;

  // get info from parser
  std::cout << "---------- Successfully Parsed XML ---------------" << std::endl;
  // get root link
  boost::shared_ptr<const urdf::Link> root_link=robot->getRoot();
  if (!root_link) return -1;

  std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;

  // print entire tree
  printTree(root_link);

  return 0;
}

bool treeFromUrdfString(const string& xml, Tree& tree)
{
  boost::shared_ptr<urdf::ModelInterface> urdf_model;
  urdf_model = urdf::parseURDF(xml);
  if( urdf_model.use_count() == 0 || !urdf_model ) { logError("Could not parse string to urdf::ModelInterface"); return false; }
  return treeFromUrdfModel(*urdf_model,tree);
}

/*
bool treeFromUrdfXml(TiXmlDocument *xml_doc, Tree& tree)
{
  boost::shared_ptr<urdf::ModelInterface> robot_model;
  robot_model = urdf::parseURDF(
  if (!robot_model.parse(xml_doc)){
    logError("Could not generate robot model");
    return false;
  }
  return treeFromUrdfModel(robot_model, tree);
}*/

bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree, const bool consider_root_link_inertia)
{
  if (consider_root_link_inertia) {
    //For giving a name to the root of KDL using the robot name,
    //as it is not used elsewhere in the KDL tree
    std::string fake_root_name = "__kdl_import__" + robot_model.getName()+"__fake_root__";
    std::string fake_root_fixed_joint_name = "__kdl_import__" + robot_model.getName()+"__fake_root_fixed_joint__";

    tree = Tree(fake_root_name);

    boost::shared_ptr<const urdf::Link> root = robot_model.getRoot();

    // constructs the optional inertia
    RigidBodyInertia inert(0);
    if (root->inertial)
      inert = toKdl(root->inertial);

    // constructs the kdl joint
    Joint jnt = Joint(fake_root_fixed_joint_name, Joint::None);

    // construct the kdl segment
    Segment sgm(root->name, jnt, Frame::Identity(), inert);

    // add segment to tree
    tree.addSegment(sgm, fake_root_name);

  } else {
    tree = Tree(robot_model.getRoot()->name);

    // warn if root link has inertia. KDL does not support this
    if (robot_model.getRoot()->inertial)
      logWarn("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.", robot_model.getRoot()->name.c_str());
  }

  //  add all children
  for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
    if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
      return false;

  return true;
}

}

<commit_msg>[urdf_import] Properly fix bug to importing com<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  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 the Willow Garage 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 THE COPYRIGHT HOLDERS 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 THE
*  COPYRIGHT OWNER 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.
*********************************************************************/

/* Author: Wim Meeussen */

#include "kdl_format_io/urdf_import.hpp"
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
#include <console_bridge/console.h>
#include <fstream>
#include <iostream>

using namespace std;
using namespace KDL;

namespace kdl_format_io{

void printTree(boost::shared_ptr<const urdf::Link> link,int level = 0)
{
  level+=2;
  int count = 0;
  for (std::vector<boost::shared_ptr<urdf::Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
  {
    if (*child)
    {
      for(int j=0;j<level;j++) std::cout << "  "; //indent
      std::cout << "child(" << (count++)+1 << "):  " << (*child)->name  << std::endl;
      // first grandchild
      printTree(*child,level);
    }
    else
    {
      for(int j=0;j<level;j++) std::cout << " "; //indent
      std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
    }
  }

}

// construct vector
Vector toKdl(urdf::Vector3 v)
{
  return Vector(v.x, v.y, v.z);
}

// construct rotation
Rotation toKdl(urdf::Rotation r)
{
  return Rotation::Quaternion(r.x, r.y, r.z, r.w);
}

// construct pose
Frame toKdl(urdf::Pose p)
{
  return Frame(toKdl(p.rotation), toKdl(p.position));
}

// construct joint
Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
{
  Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);

  switch (jnt->type){
  case urdf::Joint::FIXED:{
    return Joint(jnt->name, Joint::None);
  }
  case urdf::Joint::REVOLUTE:{
    Vector axis = toKdl(jnt->axis);
    return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
  }
  case urdf::Joint::CONTINUOUS:{
    Vector axis = toKdl(jnt->axis);
    return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
  }
  case urdf::Joint::PRISMATIC:{
    Vector axis = toKdl(jnt->axis);
    return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
  }
  default:{
    logWarn("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
    return Joint(jnt->name, Joint::None);
  }
  }
  return Joint();
}

// construct inertia
RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
{
  Frame origin = toKdl(i->origin);

  // the mass is frame indipendent
  double kdl_mass = i->mass;

  // kdl and urdf both specify the com position in the reference frame of the link
  Vector kdl_com = origin.p;

  // kdl specifies the inertia matrix in the reference frame of the link,
  // while the urdf specifies the inertia matrix in the inertia reference frame
  RotationalInertia urdf_inertia =
    RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);

  // Rotation operators are not defined for rotational inertia,
  // so we use the RigidBodyInertia operators (with com = 0) as a workaround
  RigidBodyInertia kdl_inertia_wrt_com_workaround =
    origin.M *RigidBodyInertia(0, Vector::Zero(), urdf_inertia);

  // Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com
  // while the getRotationalInertia method returns the 3d inertia wrt the frame origin
  // (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match)
  RotationalInertia kdl_inertia_wrt_com =
    kdl_inertia_wrt_com_workaround.getRotationalInertia();

  return RigidBodyInertia(kdl_mass,kdl_com,kdl_inertia_wrt_com);
}

// recursive function to walk through tree
bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
{
  std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links;
  logDebug("Link %s had %i children", root->name.c_str(), (int)children.size());

  // constructs the optional inertia
  RigidBodyInertia inert(0);
  if (root->inertial)
    inert = toKdl(root->inertial);

  // constructs the kdl joint
  Joint jnt = toKdl(root->parent_joint);

  // construct the kdl segment
  Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);

  // add segment to tree
  tree.addSegment(sgm, root->parent_joint->parent_link_name);

  // recurslively add all children
  for (size_t i=0; i<children.size(); i++){
    if (!addChildrenToTree(children[i], tree))
      return false;
  }
  return true;
}

bool treeFromUrdfFile(const string& file, Tree& tree)
{
    ifstream ifs(file.c_str());
    std::string xml_string( (std::istreambuf_iterator<char>(ifs) ),
                       (std::istreambuf_iterator<char>()    ) );

    return treeFromUrdfString(xml_string,tree);
}

/*
bool treeFromParam(const string& param, Tree& tree)
{
  urdf::ModelInterface robot_model;
  if (!robot_model.initParam(param)){
    logError("Could not generate robot model");
    return false;
  }
  return treeFromUrdfModel(robot_model, tree);
}
*/

int print_tree(const boost::shared_ptr<urdf::ModelInterface> & robot) {
std::cout << "robot name is: " << robot->getName() << std::endl;

  // get info from parser
  std::cout << "---------- Successfully Parsed XML ---------------" << std::endl;
  // get root link
  boost::shared_ptr<const urdf::Link> root_link=robot->getRoot();
  if (!root_link) return -1;

  std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;

  // print entire tree
  printTree(root_link);

  return 0;
}

bool treeFromUrdfString(const string& xml, Tree& tree)
{
  boost::shared_ptr<urdf::ModelInterface> urdf_model;
  urdf_model = urdf::parseURDF(xml);
  if( urdf_model.use_count() == 0 || !urdf_model ) { logError("Could not parse string to urdf::ModelInterface"); return false; }
  return treeFromUrdfModel(*urdf_model,tree);
}

/*
bool treeFromUrdfXml(TiXmlDocument *xml_doc, Tree& tree)
{
  boost::shared_ptr<urdf::ModelInterface> robot_model;
  robot_model = urdf::parseURDF(
  if (!robot_model.parse(xml_doc)){
    logError("Could not generate robot model");
    return false;
  }
  return treeFromUrdfModel(robot_model, tree);
}*/

bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree, const bool consider_root_link_inertia)
{
  if (consider_root_link_inertia) {
    //For giving a name to the root of KDL using the robot name,
    //as it is not used elsewhere in the KDL tree
    std::string fake_root_name = "__kdl_import__" + robot_model.getName()+"__fake_root__";
    std::string fake_root_fixed_joint_name = "__kdl_import__" + robot_model.getName()+"__fake_root_fixed_joint__";

    tree = Tree(fake_root_name);

    boost::shared_ptr<const urdf::Link> root = robot_model.getRoot();

    // constructs the optional inertia
    RigidBodyInertia inert(0);
    if (root->inertial)
      inert = toKdl(root->inertial);

    // constructs the kdl joint
    Joint jnt = Joint(fake_root_fixed_joint_name, Joint::None);

    // construct the kdl segment
    Segment sgm(root->name, jnt, Frame::Identity(), inert);

    // add segment to tree
    tree.addSegment(sgm, fake_root_name);

  } else {
    tree = Tree(robot_model.getRoot()->name);

    // warn if root link has inertia. KDL does not support this
    if (robot_model.getRoot()->inertial)
      logWarn("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.", robot_model.getRoot()->name.c_str());
  }

  //  add all children
  for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
    if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
      return false;

  return true;
}

}

<|endoftext|>