<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: autocorrmigration.cxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-08 17:39:55 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _DESKTOP_AUTOCORRMIGRATION_HXX_
#include "autocorrmigration.hxx"
#endif

#ifndef _ISOLANG_HXX
#include <tools/isolang.hxx>
#endif
#ifndef _URLOBJ_HXX
#include <tools/urlobj.hxx>
#endif
#ifndef _UTL_BOOTSTRAP_HXX
#include <unotools/bootstrap.hxx>
#endif

using namespace ::com::sun::star;
using namespace ::com::sun::star::uno;

//.........................................................................
namespace migration
{
//.........................................................................

    static ::rtl::OUString sSourceSubDir = ::rtl::OUString( RTL_CONSTASCII_USTRINGPARAM( "/user/autocorr" ) );
    static ::rtl::OUString sTargetSubDir = ::rtl::OUString( RTL_CONSTASCII_USTRINGPARAM( "/user/autocorr" ) );
    static ::rtl::OUString sBaseName = ::rtl::OUString( RTL_CONSTASCII_USTRINGPARAM( "/acor" ) );
    static ::rtl::OUString sSuffix = ::rtl::OUString( RTL_CONSTASCII_USTRINGPARAM( ".dat" ) );

    // =============================================================================
    // component operations
    // =============================================================================

    ::rtl::OUString AutocorrectionMigration_getImplementationName()
    {
        static ::rtl::OUString* pImplName = 0;
        if ( !pImplName )
        {
            ::osl::MutexGuard aGuard( ::osl::Mutex::getGlobalMutex() );
            if ( !pImplName )
            {
                static ::rtl::OUString aImplName( RTL_CONSTASCII_USTRINGPARAM( "com.sun.star.comp.desktop.migration.Autocorrection" ) );
                pImplName = &aImplName;
            }
        }
        return *pImplName;
    }

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

    Sequence< ::rtl::OUString > AutocorrectionMigration_getSupportedServiceNames()
    {
        static Sequence< ::rtl::OUString >* pNames = 0;
        if ( !pNames )
        {
            ::osl::MutexGuard aGuard( ::osl::Mutex::getGlobalMutex() );
            if ( !pNames )
            {
                static Sequence< ::rtl::OUString > aNames(1);
                aNames.getArray()[0] = ::rtl::OUString( RTL_CONSTASCII_USTRINGPARAM( "com.sun.star.migration.Autocorrection" ) );
                pNames = &aNames;
            }
        }
        return *pNames;
    }

    // =============================================================================
    // AutocorrectionMigration
    // =============================================================================

    AutocorrectionMigration::AutocorrectionMigration()
    {
    }

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

    AutocorrectionMigration::~AutocorrectionMigration()
    {
    }

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

    TStringVectorPtr AutocorrectionMigration::getFiles( const ::rtl::OUString& rBaseURL ) const
    {
        TStringVectorPtr aResult( new TStringVector );
        ::osl::Directory aDir( rBaseURL);

        if ( aDir.open() == ::osl::FileBase::E_None )
        {
            // iterate over directory content
            TStringVector aSubDirs;
            ::osl::DirectoryItem aItem;
            while ( aDir.getNextItem( aItem ) == ::osl::FileBase::E_None )
            {
                ::osl::FileStatus aFileStatus( FileStatusMask_Type | FileStatusMask_FileURL );
                if ( aItem.getFileStatus( aFileStatus ) == ::osl::FileBase::E_None )
                {
                    if ( aFileStatus.getFileType() == ::osl::FileStatus::Directory )
                        aSubDirs.push_back( aFileStatus.getFileURL() );
                    else
                        aResult->push_back( aFileStatus.getFileURL() );
                }
            }

            // iterate recursive over subfolders
            TStringVector::const_iterator aI = aSubDirs.begin();
            while ( aI != aSubDirs.end() )
            {
                TStringVectorPtr aSubResult = getFiles( *aI );
                aResult->insert( aResult->end(), aSubResult->begin(), aSubResult->end() );
                ++aI;
            }
        }

        return aResult;
    }

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

    ::osl::FileBase::RC AutocorrectionMigration::checkAndCreateDirectory( INetURLObject& rDirURL )
    {
        ::osl::FileBase::RC aResult = ::osl::Directory::create( rDirURL.GetMainURL( INetURLObject::DECODE_TO_IURI ) );
        if ( aResult == ::osl::FileBase::E_NOENT )
        {
            INetURLObject aBaseURL( rDirURL );
            aBaseURL.removeSegment();
            checkAndCreateDirectory( aBaseURL );
            return ::osl::Directory::create( rDirURL.GetMainURL( INetURLObject::DECODE_TO_IURI ) );
        }
        else
        {
            return aResult;
        }
    }

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

    void AutocorrectionMigration::copyFiles()
    {
        ::rtl::OUString sTargetDir;
        ::utl::Bootstrap::PathStatus aStatus = ::utl::Bootstrap::locateUserInstallation( sTargetDir );
        if ( aStatus == ::utl::Bootstrap::PATH_EXISTS )
        {
            sTargetDir += sTargetSubDir;
            TStringVectorPtr aFileList = getFiles( m_sSourceDir );
            TStringVector::const_iterator aI = aFileList->begin();
            while ( aI != aFileList->end() )
            {
                ::rtl::OUString sSourceLocalName = aI->copy( m_sSourceDir.getLength() );
                sal_Int32 nStart = sBaseName.getLength();
                sal_Int32 nEnd = sSourceLocalName.lastIndexOf ( sSuffix );
                ::rtl::OUString sLanguageType = sSourceLocalName.copy( nStart, nEnd - nStart );
                ::rtl::OUString sIsoName = ConvertLanguageToIsoString( (LanguageType) sLanguageType.toInt32() );
                ::rtl::OUString sTargetLocalName = sBaseName;
                sTargetLocalName += ::rtl::OUString::createFromAscii( "_" );
                sTargetLocalName += sIsoName;
                sTargetLocalName += sSuffix;
                ::rtl::OUString sTargetName = sTargetDir + sTargetLocalName;
                INetURLObject aURL( sTargetName );
                aURL.removeSegment();
                checkAndCreateDirectory( aURL );
                ::osl::FileBase::RC aResult = ::osl::File::copy( *aI, sTargetName );
                if ( aResult != ::osl::FileBase::E_None )
                {
                    ::rtl::OString aMsg( "AutocorrectionMigration::copyFiles: cannot copy " );
                    aMsg += ::rtl::OUStringToOString( *aI, RTL_TEXTENCODING_UTF8 ) + " to "
                         +  ::rtl::OUStringToOString( sTargetName, RTL_TEXTENCODING_UTF8 );
                    OSL_ENSURE( sal_False, aMsg.getStr() );
                }
                ++aI;
            }
        }
        else
        {
            OSL_ENSURE( sal_False, "AutocorrectionMigration::copyFiles: no user installation!" );
        }
    }

    // -----------------------------------------------------------------------------
    // XServiceInfo
    // -----------------------------------------------------------------------------

    ::rtl::OUString AutocorrectionMigration::getImplementationName() throw (RuntimeException)
    {
        return AutocorrectionMigration_getImplementationName();
    }

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

    sal_Bool AutocorrectionMigration::supportsService( const ::rtl::OUString& rServiceName ) throw (RuntimeException)
    {
        Sequence< ::rtl::OUString > aNames( getSupportedServiceNames() );
        const ::rtl::OUString* pNames = aNames.getConstArray();
        const ::rtl::OUString* pEnd = pNames + aNames.getLength();
        for ( ; pNames != pEnd && !pNames->equals( rServiceName ); ++pNames )
            ;

        return pNames != pEnd;
    }

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

    Sequence< ::rtl::OUString > AutocorrectionMigration::getSupportedServiceNames() throw (RuntimeException)
    {
        return AutocorrectionMigration_getSupportedServiceNames();
    }

    // -----------------------------------------------------------------------------
    // XInitialization
    // -----------------------------------------------------------------------------

    void AutocorrectionMigration::initialize( const Sequence< Any >& aArguments ) throw (Exception, RuntimeException)
    {
        ::osl::MutexGuard aGuard( m_aMutex );

        const Any* pIter = aArguments.getConstArray();
        const Any* pEnd = pIter + aArguments.getLength();
        for ( ; pIter != pEnd ; ++pIter )
        {
            beans::NamedValue aValue;
            *pIter >>= aValue;
            if ( aValue.Name.equalsAscii( "UserData" ) )
            {
                sal_Bool bSuccess = aValue.Value >>= m_sSourceDir;
                OSL_ENSURE( bSuccess == sal_True, "AutocorrectionMigration::initialize: argument UserData has wrong type!" );
                m_sSourceDir += sSourceSubDir;
                break;
            }
        }
    }

    // -----------------------------------------------------------------------------
    // XJob
    // -----------------------------------------------------------------------------

    Any AutocorrectionMigration::execute( const Sequence< beans::NamedValue >& Arguments )
        throw (lang::IllegalArgumentException, Exception, RuntimeException)
    {
        ::osl::MutexGuard aGuard( m_aMutex );

        copyFiles();

        return Any();
    }

    // =============================================================================
    // component operations
    // =============================================================================

    Reference< XInterface > SAL_CALL AutocorrectionMigration_create(
        Reference< XComponentContext > const & xContext )
        SAL_THROW( () )
    {
        return static_cast< lang::XTypeProvider * >( new AutocorrectionMigration() );
    }

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

//.........................................................................
}   // namespace migration
//.........................................................................
<commit_msg>INTEGRATION: CWS internatiodel (1.3.102); FILE MERGED 2006/02/10 19:29:43 er 1.3.102.1: #i52115# move LangIDs and ISO conversion from tools to i18npool; introduce class MsLangId and libi18nisolang<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: autocorrmigration.cxx,v $
 *
 *  $Revision: 1.4 $
 *
 *  last change: $Author: vg $ $Date: 2006-04-07 14:47:20 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _DESKTOP_AUTOCORRMIGRATION_HXX_
#include "autocorrmigration.hxx"
#endif

#ifndef INCLUDED_I18NPOOL_MSLANGID_HXX
#include <i18npool/mslangid.hxx>
#endif
#ifndef _URLOBJ_HXX
#include <tools/urlobj.hxx>
#endif
#ifndef _UTL_BOOTSTRAP_HXX
#include <unotools/bootstrap.hxx>
#endif

using namespace ::com::sun::star;
using namespace ::com::sun::star::uno;

//.........................................................................
namespace migration
{
//.........................................................................

    static ::rtl::OUString sSourceSubDir = ::rtl::OUString( RTL_CONSTASCII_USTRINGPARAM( "/user/autocorr" ) );
    static ::rtl::OUString sTargetSubDir = ::rtl::OUString( RTL_CONSTASCII_USTRINGPARAM( "/user/autocorr" ) );
    static ::rtl::OUString sBaseName = ::rtl::OUString( RTL_CONSTASCII_USTRINGPARAM( "/acor" ) );
    static ::rtl::OUString sSuffix = ::rtl::OUString( RTL_CONSTASCII_USTRINGPARAM( ".dat" ) );

    // =============================================================================
    // component operations
    // =============================================================================

    ::rtl::OUString AutocorrectionMigration_getImplementationName()
    {
        static ::rtl::OUString* pImplName = 0;
        if ( !pImplName )
        {
            ::osl::MutexGuard aGuard( ::osl::Mutex::getGlobalMutex() );
            if ( !pImplName )
            {
                static ::rtl::OUString aImplName( RTL_CONSTASCII_USTRINGPARAM( "com.sun.star.comp.desktop.migration.Autocorrection" ) );
                pImplName = &aImplName;
            }
        }
        return *pImplName;
    }

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

    Sequence< ::rtl::OUString > AutocorrectionMigration_getSupportedServiceNames()
    {
        static Sequence< ::rtl::OUString >* pNames = 0;
        if ( !pNames )
        {
            ::osl::MutexGuard aGuard( ::osl::Mutex::getGlobalMutex() );
            if ( !pNames )
            {
                static Sequence< ::rtl::OUString > aNames(1);
                aNames.getArray()[0] = ::rtl::OUString( RTL_CONSTASCII_USTRINGPARAM( "com.sun.star.migration.Autocorrection" ) );
                pNames = &aNames;
            }
        }
        return *pNames;
    }

    // =============================================================================
    // AutocorrectionMigration
    // =============================================================================

    AutocorrectionMigration::AutocorrectionMigration()
    {
    }

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

    AutocorrectionMigration::~AutocorrectionMigration()
    {
    }

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

    TStringVectorPtr AutocorrectionMigration::getFiles( const ::rtl::OUString& rBaseURL ) const
    {
        TStringVectorPtr aResult( new TStringVector );
        ::osl::Directory aDir( rBaseURL);

        if ( aDir.open() == ::osl::FileBase::E_None )
        {
            // iterate over directory content
            TStringVector aSubDirs;
            ::osl::DirectoryItem aItem;
            while ( aDir.getNextItem( aItem ) == ::osl::FileBase::E_None )
            {
                ::osl::FileStatus aFileStatus( FileStatusMask_Type | FileStatusMask_FileURL );
                if ( aItem.getFileStatus( aFileStatus ) == ::osl::FileBase::E_None )
                {
                    if ( aFileStatus.getFileType() == ::osl::FileStatus::Directory )
                        aSubDirs.push_back( aFileStatus.getFileURL() );
                    else
                        aResult->push_back( aFileStatus.getFileURL() );
                }
            }

            // iterate recursive over subfolders
            TStringVector::const_iterator aI = aSubDirs.begin();
            while ( aI != aSubDirs.end() )
            {
                TStringVectorPtr aSubResult = getFiles( *aI );
                aResult->insert( aResult->end(), aSubResult->begin(), aSubResult->end() );
                ++aI;
            }
        }

        return aResult;
    }

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

    ::osl::FileBase::RC AutocorrectionMigration::checkAndCreateDirectory( INetURLObject& rDirURL )
    {
        ::osl::FileBase::RC aResult = ::osl::Directory::create( rDirURL.GetMainURL( INetURLObject::DECODE_TO_IURI ) );
        if ( aResult == ::osl::FileBase::E_NOENT )
        {
            INetURLObject aBaseURL( rDirURL );
            aBaseURL.removeSegment();
            checkAndCreateDirectory( aBaseURL );
            return ::osl::Directory::create( rDirURL.GetMainURL( INetURLObject::DECODE_TO_IURI ) );
        }
        else
        {
            return aResult;
        }
    }

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

    void AutocorrectionMigration::copyFiles()
    {
        ::rtl::OUString sTargetDir;
        ::utl::Bootstrap::PathStatus aStatus = ::utl::Bootstrap::locateUserInstallation( sTargetDir );
        if ( aStatus == ::utl::Bootstrap::PATH_EXISTS )
        {
            sTargetDir += sTargetSubDir;
            TStringVectorPtr aFileList = getFiles( m_sSourceDir );
            TStringVector::const_iterator aI = aFileList->begin();
            while ( aI != aFileList->end() )
            {
                ::rtl::OUString sSourceLocalName = aI->copy( m_sSourceDir.getLength() );
                sal_Int32 nStart = sBaseName.getLength();
                sal_Int32 nEnd = sSourceLocalName.lastIndexOf ( sSuffix );
                ::rtl::OUString sLanguageType = sSourceLocalName.copy( nStart, nEnd - nStart );
                ::rtl::OUString sIsoName = MsLangId::convertLanguageToIsoString( (LanguageType) sLanguageType.toInt32() );
                ::rtl::OUString sTargetLocalName = sBaseName;
                sTargetLocalName += ::rtl::OUString::createFromAscii( "_" );
                sTargetLocalName += sIsoName;
                sTargetLocalName += sSuffix;
                ::rtl::OUString sTargetName = sTargetDir + sTargetLocalName;
                INetURLObject aURL( sTargetName );
                aURL.removeSegment();
                checkAndCreateDirectory( aURL );
                ::osl::FileBase::RC aResult = ::osl::File::copy( *aI, sTargetName );
                if ( aResult != ::osl::FileBase::E_None )
                {
                    ::rtl::OString aMsg( "AutocorrectionMigration::copyFiles: cannot copy " );
                    aMsg += ::rtl::OUStringToOString( *aI, RTL_TEXTENCODING_UTF8 ) + " to "
                         +  ::rtl::OUStringToOString( sTargetName, RTL_TEXTENCODING_UTF8 );
                    OSL_ENSURE( sal_False, aMsg.getStr() );
                }
                ++aI;
            }
        }
        else
        {
            OSL_ENSURE( sal_False, "AutocorrectionMigration::copyFiles: no user installation!" );
        }
    }

    // -----------------------------------------------------------------------------
    // XServiceInfo
    // -----------------------------------------------------------------------------

    ::rtl::OUString AutocorrectionMigration::getImplementationName() throw (RuntimeException)
    {
        return AutocorrectionMigration_getImplementationName();
    }

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

    sal_Bool AutocorrectionMigration::supportsService( const ::rtl::OUString& rServiceName ) throw (RuntimeException)
    {
        Sequence< ::rtl::OUString > aNames( getSupportedServiceNames() );
        const ::rtl::OUString* pNames = aNames.getConstArray();
        const ::rtl::OUString* pEnd = pNames + aNames.getLength();
        for ( ; pNames != pEnd && !pNames->equals( rServiceName ); ++pNames )
            ;

        return pNames != pEnd;
    }

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

    Sequence< ::rtl::OUString > AutocorrectionMigration::getSupportedServiceNames() throw (RuntimeException)
    {
        return AutocorrectionMigration_getSupportedServiceNames();
    }

    // -----------------------------------------------------------------------------
    // XInitialization
    // -----------------------------------------------------------------------------

    void AutocorrectionMigration::initialize( const Sequence< Any >& aArguments ) throw (Exception, RuntimeException)
    {
        ::osl::MutexGuard aGuard( m_aMutex );

        const Any* pIter = aArguments.getConstArray();
        const Any* pEnd = pIter + aArguments.getLength();
        for ( ; pIter != pEnd ; ++pIter )
        {
            beans::NamedValue aValue;
            *pIter >>= aValue;
            if ( aValue.Name.equalsAscii( "UserData" ) )
            {
                sal_Bool bSuccess = aValue.Value >>= m_sSourceDir;
                OSL_ENSURE( bSuccess == sal_True, "AutocorrectionMigration::initialize: argument UserData has wrong type!" );
                m_sSourceDir += sSourceSubDir;
                break;
            }
        }
    }

    // -----------------------------------------------------------------------------
    // XJob
    // -----------------------------------------------------------------------------

    Any AutocorrectionMigration::execute( const Sequence< beans::NamedValue >& Arguments )
        throw (lang::IllegalArgumentException, Exception, RuntimeException)
    {
        ::osl::MutexGuard aGuard( m_aMutex );

        copyFiles();

        return Any();
    }

    // =============================================================================
    // component operations
    // =============================================================================

    Reference< XInterface > SAL_CALL AutocorrectionMigration_create(
        Reference< XComponentContext > const & xContext )
        SAL_THROW( () )
    {
        return static_cast< lang::XTypeProvider * >( new AutocorrectionMigration() );
    }

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

//.........................................................................
}   // namespace migration
//.........................................................................
<|endoftext|>


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

/**
 * @file
 **/

#include "modules/planning/common/trajectory/trajectory_stitcher.h"

#include <algorithm>
#include <list>
#include <utility>

#include "cybertron/common/log.h"
#include "modules/common/configs/config_gflags.h"
#include "modules/common/math/angle.h"
#include "modules/common/math/quaternion.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::math::Vec2d;
using apollo::common::util::DistanceXY;

std::vector<TrajectoryPoint>
TrajectoryStitcher::ComputeReinitStitchingTrajectory(
    const VehicleState& vehicle_state) {
  TrajectoryPoint init_point;
  init_point.mutable_path_point()->set_s(0.0);
  init_point.mutable_path_point()->set_x(vehicle_state.x());
  init_point.mutable_path_point()->set_y(vehicle_state.y());
  init_point.mutable_path_point()->set_z(vehicle_state.z());
  init_point.mutable_path_point()->set_theta(vehicle_state.heading());
  init_point.mutable_path_point()->set_kappa(vehicle_state.kappa());
  init_point.set_v(vehicle_state.linear_velocity());
  init_point.set_a(vehicle_state.linear_acceleration());
  init_point.set_relative_time(0.0);

  return std::vector<TrajectoryPoint>(1, init_point);
}

// only used in navigation mode
void TrajectoryStitcher::TransformLastPublishedTrajectory(
    const double x_diff, const double y_diff, const double theta_diff,
    PublishableTrajectory* prev_trajectory) {
  if (!prev_trajectory) {
    return;
  }

  // R^-1
  double cos_theta = std::cos(theta_diff);
  double sin_theta = -std::sin(theta_diff);

  // -R^-1 * t
  auto tx = -(cos_theta * x_diff - sin_theta * y_diff);
  auto ty = -(sin_theta * x_diff + cos_theta * y_diff);

  std::for_each(prev_trajectory->trajectory_points().begin(),
                prev_trajectory->trajectory_points().end(),
                [&cos_theta, &sin_theta, &tx, &ty,
                 &theta_diff](common::TrajectoryPoint& p) {
                  auto x = p.path_point().x();
                  auto y = p.path_point().y();
                  auto theta = p.path_point().theta();

                  auto x_new = cos_theta * x - sin_theta * y + tx;
                  auto y_new = sin_theta * x + cos_theta * y + ty;
                  auto theta_new = common::math::WrapAngle(theta - theta_diff);

                  p.mutable_path_point()->set_x(x_new);
                  p.mutable_path_point()->set_y(y_new);
                  p.mutable_path_point()->set_theta(theta_new);
                });
}

// Planning from current vehicle state:
// if 1. the auto-driving mode is off or
//    2. we don't have the trajectory from last planning cycle or
//    3. the position deviation from actual and target is too high
std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const VehicleState& vehicle_state, const double current_timestamp,
    const double planning_cycle_time,
    const PublishableTrajectory* prev_trajectory) {
  if (!FLAGS_enable_trajectory_stitcher) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (!prev_trajectory) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  std::size_t prev_trajectory_size = prev_trajectory->NumOfPoints();

  if (prev_trajectory_size == 0) {
    ADEBUG << "Projected trajectory at time [" << prev_trajectory->header_time()
           << "] size is zero! Previous planning not exist or failed. Use "
              "origin car status instead.";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  const double veh_rel_time =
      current_timestamp - prev_trajectory->header_time();

  std::size_t time_matched_index =
      prev_trajectory->QueryLowerBoundPoint(veh_rel_time);

  if (time_matched_index == 0 &&
      veh_rel_time < prev_trajectory->StartPoint().relative_time()) {
    AWARN << "current time smaller than the previous trajectory's first time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (time_matched_index + 1 >= prev_trajectory_size) {
    AWARN << "current time beyond the previous trajectory's last time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  auto time_matched_point =
      prev_trajectory->TrajectoryPointAt(time_matched_index);

  if (!time_matched_point.has_path_point()) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  std::size_t position_matched_index =
      prev_trajectory->QueryNearestPointWithBuffer(
      {vehicle_state.x(), vehicle_state.y()}, 1.0e-6);

  auto frenet_sd = ComputePositionProjection(
      vehicle_state.x(), vehicle_state.y(),
      prev_trajectory->TrajectoryPointAt(position_matched_index));

  auto lon_diff = time_matched_point.path_point().s() - frenet_sd.first;
  auto lat_diff = frenet_sd.second;

  ADEBUG << "Control lateral diff: " << lat_diff
         << ", longitudinal diff: " << lon_diff;

  if (std::fabs(lat_diff) > FLAGS_replan_lateral_distance_threshold ||
      std::fabs(lon_diff) > FLAGS_replan_longitudinal_distance_threshold) {
    AERROR << "the distance between matched point and actual position is too "
              "large. Replan is triggered. lat_diff = "
           << lat_diff << ", lon_diff = " << lon_diff;
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  double forward_rel_time =
      prev_trajectory->TrajectoryPointAt(time_matched_index).relative_time() +
      planning_cycle_time;

  std::size_t forward_time_index =
      prev_trajectory->QueryLowerBoundPoint(forward_rel_time);

  std::vector<TrajectoryPoint> stitching_trajectory(
      prev_trajectory->trajectory_points().begin() +
          std::max(0, static_cast<int>(time_matched_index - 1)),
      prev_trajectory->trajectory_points().begin() + forward_time_index + 1);

  const double zero_s = stitching_trajectory.back().path_point().s();
  for (auto& tp : stitching_trajectory) {
    if (!tp.has_path_point()) {
      return ComputeReinitStitchingTrajectory(vehicle_state);
    }
    tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() -
                         current_timestamp);
    tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
  }
  return stitching_trajectory;
}

std::pair<double, double> TrajectoryStitcher::ComputePositionProjection(
    const double x, const double y, const TrajectoryPoint& p) {
  Vec2d v(x - p.path_point().x(), y - p.path_point().y());
  Vec2d n(common::math::cos(
              common::math::Angle16::from_rad(p.path_point().theta())),
          common::math::sin(
              common::math::Angle16::from_rad(p.path_point().theta())));

  std::pair<double, double> frenet_sd;
  frenet_sd.first = v.InnerProd(n) + p.path_point().s();
  frenet_sd.second = v.CrossProd(n);
  return frenet_sd;
}

}  // namespace planning
}  // namespace apollo
<commit_msg>planning: fixed trajectory stitcher<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/common/trajectory/trajectory_stitcher.h"

#include <algorithm>
#include <list>
#include <utility>

#include "cybertron/common/log.h"
#include "modules/common/configs/config_gflags.h"
#include "modules/common/math/angle.h"
#include "modules/common/math/quaternion.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::math::Vec2d;
using apollo::common::util::DistanceXY;

std::vector<TrajectoryPoint>
TrajectoryStitcher::ComputeReinitStitchingTrajectory(
    const VehicleState& vehicle_state) {
  TrajectoryPoint init_point;
  init_point.mutable_path_point()->set_s(0.0);
  init_point.mutable_path_point()->set_x(vehicle_state.x());
  init_point.mutable_path_point()->set_y(vehicle_state.y());
  init_point.mutable_path_point()->set_z(vehicle_state.z());
  init_point.mutable_path_point()->set_theta(vehicle_state.heading());
  init_point.mutable_path_point()->set_kappa(vehicle_state.kappa());
  init_point.set_v(vehicle_state.linear_velocity());
  init_point.set_a(vehicle_state.linear_acceleration());
  init_point.set_relative_time(0.0);

  return std::vector<TrajectoryPoint>(1, init_point);
}

// only used in navigation mode
void TrajectoryStitcher::TransformLastPublishedTrajectory(
    const double x_diff, const double y_diff, const double theta_diff,
    PublishableTrajectory* prev_trajectory) {
  if (!prev_trajectory) {
    return;
  }

  // R^-1
  double cos_theta = std::cos(theta_diff);
  double sin_theta = -std::sin(theta_diff);

  // -R^-1 * t
  auto tx = -(cos_theta * x_diff - sin_theta * y_diff);
  auto ty = -(sin_theta * x_diff + cos_theta * y_diff);

  std::for_each(prev_trajectory->trajectory_points().begin(),
                prev_trajectory->trajectory_points().end(),
                [&cos_theta, &sin_theta, &tx, &ty,
                 &theta_diff](common::TrajectoryPoint& p) {
                  auto x = p.path_point().x();
                  auto y = p.path_point().y();
                  auto theta = p.path_point().theta();

                  auto x_new = cos_theta * x - sin_theta * y + tx;
                  auto y_new = sin_theta * x + cos_theta * y + ty;
                  auto theta_new = common::math::WrapAngle(theta - theta_diff);

                  p.mutable_path_point()->set_x(x_new);
                  p.mutable_path_point()->set_y(y_new);
                  p.mutable_path_point()->set_theta(theta_new);
                });
}

// Planning from current vehicle state:
// if 1. the auto-driving mode is off or
//    2. we don't have the trajectory from last planning cycle or
//    3. the position deviation from actual and target is too high
std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const VehicleState& vehicle_state, const double current_timestamp,
    const double planning_cycle_time,
    const PublishableTrajectory* prev_trajectory) {
  if (!FLAGS_enable_trajectory_stitcher) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (!prev_trajectory) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  std::size_t prev_trajectory_size = prev_trajectory->NumOfPoints();

  if (prev_trajectory_size == 0) {
    ADEBUG << "Projected trajectory at time [" << prev_trajectory->header_time()
           << "] size is zero! Previous planning not exist or failed. Use "
              "origin car status instead.";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  const double veh_rel_time =
      current_timestamp - prev_trajectory->header_time();

  std::size_t time_matched_index =
      prev_trajectory->QueryLowerBoundPoint(veh_rel_time);

  if (time_matched_index == 0 &&
      veh_rel_time < prev_trajectory->StartPoint().relative_time()) {
    AWARN << "current time smaller than the previous trajectory's first time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (time_matched_index + 1 >= prev_trajectory_size) {
    AWARN << "current time beyond the previous trajectory's last time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  auto time_matched_point =
      prev_trajectory->TrajectoryPointAt(time_matched_index);

  if (!time_matched_point.has_path_point()) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  std::size_t position_matched_index =
      prev_trajectory->QueryNearestPointWithBuffer(
      {vehicle_state.x(), vehicle_state.y()}, 1.0e-6);

  auto frenet_sd = ComputePositionProjection(
      vehicle_state.x(), vehicle_state.y(),
      prev_trajectory->TrajectoryPointAt(position_matched_index));

  auto lon_diff = time_matched_point.path_point().s() - frenet_sd.first;
  auto lat_diff = frenet_sd.second;

  ADEBUG << "Control lateral diff: " << lat_diff
         << ", longitudinal diff: " << lon_diff;

  if (std::fabs(lat_diff) > FLAGS_replan_lateral_distance_threshold ||
      std::fabs(lon_diff) > FLAGS_replan_longitudinal_distance_threshold) {
    AERROR << "the distance between matched point and actual position is too "
              "large. Replan is triggered. lat_diff = "
           << lat_diff << ", lon_diff = " << lon_diff;
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  double forward_rel_time =
      prev_trajectory->TrajectoryPointAt(time_matched_index).relative_time() +
      planning_cycle_time;

  std::size_t forward_time_index =
      prev_trajectory->QueryLowerBoundPoint(forward_rel_time);

  ADEBUG << "Position matched index:\t" << position_matched_index;
  ADEBUG << "Time matched index:\t" << time_matched_index;

  auto matched_index = std::min(time_matched_index, position_matched_index);

  std::vector<TrajectoryPoint> stitching_trajectory(
      prev_trajectory->trajectory_points().begin() +
          std::max(0, static_cast<int>(matched_index - 1)),
      prev_trajectory->trajectory_points().begin() + forward_time_index + 1);

  const double zero_s = stitching_trajectory.back().path_point().s();
  for (auto& tp : stitching_trajectory) {
    if (!tp.has_path_point()) {
      return ComputeReinitStitchingTrajectory(vehicle_state);
    }
    tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() -
                         current_timestamp);
    tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
  }
  return stitching_trajectory;
}

std::pair<double, double> TrajectoryStitcher::ComputePositionProjection(
    const double x, const double y, const TrajectoryPoint& p) {
  Vec2d v(x - p.path_point().x(), y - p.path_point().y());
  Vec2d n(common::math::cos(
              common::math::Angle16::from_rad(p.path_point().theta())),
          common::math::sin(
              common::math::Angle16::from_rad(p.path_point().theta())));

  std::pair<double, double> frenet_sd;
  frenet_sd.first = v.InnerProd(n) + p.path_point().s();
  frenet_sd.second = v.CrossProd(n);
  return frenet_sd;
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>//=======================================================================
// Copyright Baptiste Wicht 2011.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <iostream>
#include <cctype>
#include <list>
#include <string>

#include "Types.hpp"
#include "Utils.hpp"

#include "Parser.hpp"
#include "Operators.hpp"
#include "Lexer.hpp"
#include "Branches.hpp"
#include "Loops.hpp"

using std::string;
using std::ios_base;
using std::list;

using namespace eddic;

inline static void assertNextIsRightParenth(Lexer& lexer, const string& message);
inline static void assertNextIsLeftParenth(Lexer& lexer, const string& message);
inline static void assertNextIsRightBrace(Lexer& lexer, const string& message);
inline static void assertNextIsLeftBrace(Lexer& lexer, const string& message);
inline static void assertNextIsStop(Lexer& lexer, const string& message);

//Move to some utility class
bool isType(const Lexer& lexer) {
    if (!lexer.isWord()) {
        return false;
    }

    string value = lexer.getCurrentToken().value();

    return value == "int" || value == "string";
}

Program* Parser::parse() {
    //Create the global context
    currentContext = new Context();

    Program* program = new Program(currentContext);

    while (lexer.next()) {
        program->addLast(parseInstruction());
    }

    return program;
}

ParseNode* Parser::parseInstruction() {
    if (lexer.isIf()) {
        return parseIf();
    } else if (lexer.isWhile()) {
        return parseWhile();
    } else if (isType(lexer)) {
        return parseDeclaration();
    } else if (lexer.isWord()) {
        return parseCallOrAssignment();
    }

    throw TokenException("Not an instruction", lexer.getCurrentToken());
}

ParseNode* Parser::parseCallOrAssignment() {
    Token token = lexer.getCurrentToken();

    if (!lexer.next()) {
        throw TokenException("Incomplete instruction", lexer.getCurrentToken());
    }

    if (lexer.isLeftParenth()) {
        return parseCall(token);
    } else if (lexer.isAssign()) {
        return parseAssignment(token);
    } else if (lexer.isSwap()) {
        return parseSwap(token);
    }

    throw TokenException("Not an instruction", lexer.getCurrentToken());
}

ParseNode* Parser::parseCall(const Token& callToken) {
    string call = callToken.value();

    if (call != "Print" && call != "Println") {
        throw TokenException("The call \"" + call + "\" does not exist", callToken);
    }

    Value* value = parseValue();

    assertNextIsRightParenth(lexer, "The call must be closed with a right parenth");
    assertNextIsStop(lexer, "Every instruction must be closed by a semicolon");

    if (call == "Print") {
        return new Print(currentContext, value);
    } else {
        return new Println(currentContext, value);
    }
}

ParseNode* Parser::parseDeclaration() {
    string typeName = lexer.getCurrentToken().value();

    Type type;
    if (typeName == "int") {
        type = INT;
    } else {
        type = STRING;
    }

    if (!lexer.next() || !lexer.isWord()) {
        throw TokenException("A type must be followed by variable name", lexer.getCurrentToken());
    }

    string variable = lexer.getCurrentToken().value();

    if (!lexer.next() || !lexer.isAssign()) {
        throw TokenException("A variable declaration must followed by '='", lexer.getCurrentToken());
    }

    Value* value = parseValue();

    assertNextIsStop(lexer, "Every instruction must be closed by a semicolon");

    return new Declaration(currentContext, type, variable, value);
}

ParseNode* Parser::parseAssignment(const Token& variableToken) {
    Value* value = parseValue();

    assertNextIsStop(lexer, "Every instruction must be closed by a semicolon");

    return new Assignment(currentContext, variableToken.value(), value);
}

ParseNode* Parser::parseSwap(const Token& lhs) {
    if (!lexer.next() || !lexer.isWord()) {
        throw TokenException("Can only swap two variables", lexer.getCurrentToken());
    }

    string rhs = lexer.getCurrentToken().value();

    assertNextIsStop(lexer, "Every instruction must be closed by a semicolon");

    return new Swap(currentContext, lhs.value(), rhs);
}

ParseNode* Parser::parseIf() {
    assertNextIsLeftParenth(lexer, "An if instruction must be followed by a condition surrounded by parenth");

    Condition* condition = parseCondition();

    assertNextIsRightParenth(lexer, "The condition of the if must be closed by a right parenth");

    assertNextIsLeftBrace(lexer, "Waiting for a left brace");

    currentContext = new Context(currentContext);

    If* block = new If(currentContext, condition);

    lexer.next();

    while (!lexer.isRightBrace()) {
        block->addLast(parseInstruction());

        lexer.next();
    }

    currentContext = currentContext->parent();

    if (!lexer.isRightBrace()) {
        throw TokenException("If body must be closed with right brace", lexer.getCurrentToken());
    }

    while (true) {
        if (lexer.next()) {
            if (lexer.isElse()) {
                lexer.next();

                if (lexer.isIf()) {
                    block->addElseIf(parseElseIf());
                } else {
                    lexer.pushBack();

                    block->setElse(parseElse());

                    break;
                }
            } else {
                lexer.pushBack();

                break;
            }
        } else {
            break;
        }
    }

    return block;
}

ElseIf* Parser::parseElseIf() {
    assertNextIsLeftParenth(lexer, "An else if instruction must be followed by a condition surrounded by parenth");

    Condition* condition = parseCondition();

    assertNextIsRightParenth(lexer, "The condition of the else if must be closed by a right parenth");

    assertNextIsLeftBrace(lexer, "Waiting for a left brace");

    currentContext = new Context(currentContext);

    ElseIf* block = new ElseIf(currentContext, condition);

    lexer.next();

    while (!lexer.isRightBrace()) {
        block->addLast(parseInstruction());

        lexer.next();
    }

    currentContext = currentContext->parent();

    if (!lexer.isRightBrace()) {
        throw TokenException("Else ff body must be closed with right brace", lexer.getCurrentToken());
    }

    return block;
}

Else* Parser::parseElse() {
    currentContext = new Context(currentContext);

    Else* block = new Else(currentContext);

    assertNextIsLeftBrace(lexer, "else statement must be followed by left brace");

    while (lexer.next() && !lexer.isRightBrace()) {
        block->addLast(parseInstruction());
    }

    if (!lexer.isRightBrace()) {
        throw TokenException("else body must be closed with right brace", lexer.getCurrentToken());
    }

    currentContext = currentContext->parent();

    return block;
}

ParseNode* Parser::parseWhile() {
    assertNextIsLeftParenth(lexer, "A while instruction must be followed by a condition surrounded by parenth");

    Condition* condition = parseCondition();

    assertNextIsRightParenth(lexer, "The condition of the while must be closed by a right parenth");

    assertNextIsLeftBrace(lexer, "Waiting for a left brace");

    currentContext = new Context(currentContext);

    While* block = new While(currentContext, condition);

    lexer.next();

    while (!lexer.isRightBrace()) {
        block->addLast(parseInstruction());

        lexer.next();
    }

    currentContext = currentContext->parent();

    if (!lexer.isRightBrace()) {
        throw TokenException("If body must be closed with right brace", lexer.getCurrentToken());
    }

    return block;
}

enum Operator {
    ADD, MUL, SUB, DIV, MOD, ERROR
};

class Part {
    public:
        virtual bool isResolved() = 0;
        virtual Operator getOperator() {
            return ERROR;
        }
        virtual Value* getValue() {
            return NULL;
        }
};

class Resolved : public Part {
    public:
        explicit Resolved(Value* v) : value(v) {}
        bool isResolved() {
            return true;
        }
        Value* getValue() {
            return value;
        }

    private:
        Value* value;
};

class Unresolved : public Part {
    public:
        explicit Unresolved(Operator o) : op(o) {}
        bool isResolved() {
            return false;
        }
        Operator getOperator() {
            return op;
        }

    private:
        Operator op;
};

int priority(Operator op) {
    switch (op) {
        case MUL:
        case DIV:
        case MOD:
            return 10;
        case ADD:
        case SUB:
            return 0;
        default:
            return -1; //TODO should never happen
    }
}

Value* Parser::parseValue() {
    list<Part*> parts;

    while (true) {
        if (!lexer.next()) {
            throw TokenException("Waiting for a value", lexer.getCurrentToken());
        }

        Value* node = NULL;

        if (lexer.isLeftParenth()) {
            node = parseValue();

            assertNextIsRightParenth(lexer, "parenth is not closed");
        } else if (lexer.isLitteral()) {
            string litteral = lexer.getCurrentToken().value();

            node = new Litteral(currentContext, litteral);
        } else if (lexer.isWord()) {
            string variableRight = lexer.getCurrentToken().value();

            node = new VariableValue(currentContext, variableRight);
        } else if (lexer.isInteger()) {
            string integer = lexer.getCurrentToken().value();
            int value = toNumber<int>(integer);

            node = new Integer(currentContext, value);
        } else {
            throw TokenException("Invalid value", lexer.getCurrentToken());
        }

        parts.push_back(new Resolved(node));

        if (!lexer.next()) {
            break;
        }

        if (lexer.isAddition()) {
            parts.push_back(new Unresolved(ADD));
        } else if (lexer.isSubtraction()) {
            parts.push_back(new Unresolved(SUB));
        } else if (lexer.isMultiplication()) {
            parts.push_back(new Unresolved(MUL));
        } else if (lexer.isDivision()) {
            parts.push_back(new Unresolved(DIV));
        } else if (lexer.isModulo()) {
            parts.push_back(new Unresolved(MOD));
        } else {
            lexer.pushBack();
            break;
        }
    }

    while (parts.size() > 1) {
        int maxPriority = -1;
        list<Part*>::iterator it, end, max;
        for (it = parts.begin(), end = parts.end(); it != end; ++it) {
            Part* part = *it;

            if (!part->isResolved()) {
                if (priority(part->getOperator()) > maxPriority) {
                    maxPriority = priority(part->getOperator());
                    max = it;
                }
            }
        }

        list<Part*>::iterator first = max;
        --first;

        Part* left = *first;
        Part* center = *max;

        ++max;
        Part* right = *max;

        Value* lhs = left->getValue();
        Operator op = center->getOperator();
        Value* rhs = right->getValue();

        Value* value = NULL;

        if (op == ADD) {
            value = new Addition(currentContext, lhs, rhs);
        } else if (op == SUB) {
            value = new Subtraction(currentContext, lhs, rhs);
        } else if (op == MUL) {
            value = new Multiplication(currentContext, lhs, rhs);
        } else if (op == DIV) {
            value = new Division(currentContext, lhs, rhs);
        } else if (op == MOD) {
            value = new Modulo(currentContext, lhs, rhs);
        }

        parts.erase(first, ++max);
        parts.insert(max, new Resolved(value));
    }

    Value* value = (*parts.begin())->getValue();

    parts.clear();

    return value;
}

Condition* Parser::parseCondition() {
    lexer.next();

    if (lexer.isTrue()) {
        return new Condition(TRUE_VALUE);
    } else if (lexer.isFalse()) {
        return new Condition(FALSE_VALUE);
    } else {
        lexer.pushBack();
    }

    Value* lhs = parseValue();

    if (!lexer.next()) {
        throw TokenException("waiting for a boolean operator", lexer.getCurrentToken());
    }

    BooleanCondition operation;
    if (lexer.isGreater()) {
        operation = GREATER_OPERATOR;
    } else if (lexer.isLess()) {
        operation = LESS_OPERATOR;
    } else if (lexer.isEquals()) {
        operation = EQUALS_OPERATOR;
    } else if (lexer.isNotEquals()) {
        operation = NOT_EQUALS_OPERATOR;
    } else if (lexer.isGreaterOrEquals()) {
        operation = GREATER_EQUALS_OPERATOR;
    } else if (lexer.isLessOrEquals()) {
        operation = LESS_EQUALS_OPERATOR;
    } else {
        throw TokenException("waiting for a boolean operator", lexer.getCurrentToken());
    }

    Value* rhs = parseValue();

    return new Condition(operation, lhs, rhs);
}

inline static void assertNextIsRightParenth(Lexer& lexer, const string& message) {
    if (!lexer.next() || !lexer.isRightParenth()) {
        throw TokenException(message, lexer.getCurrentToken());
    }
}

inline static void assertNextIsLeftParenth(Lexer& lexer, const string& message) {
    if (!lexer.next() || !lexer.isLeftParenth()) {
        throw TokenException(message, lexer.getCurrentToken());
    }
}

inline static void assertNextIsRightBrace(Lexer& lexer, const string& message) {
    if (!lexer.next() || !lexer.isRightBrace()) {
        throw TokenException(message, lexer.getCurrentToken());
    }
}

inline static void assertNextIsLeftBrace(Lexer& lexer, const string& message) {
    if (!lexer.next() || !lexer.isLeftBrace()) {
        throw TokenException(message, lexer.getCurrentToken());
    }
}

inline static void assertNextIsStop(Lexer& lexer, const string& message) {
    if (!lexer.next() || !lexer.isStop()) {
        throw TokenException(message, lexer.getCurrentToken());
    }
}
<commit_msg>Fix memory leaks in ParseValue<commit_after>//=======================================================================
// Copyright Baptiste Wicht 2011.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <iostream>
#include <cctype>
#include <list>
#include <string>

#include "Types.hpp"
#include "Utils.hpp"

#include "Parser.hpp"
#include "Operators.hpp"
#include "Lexer.hpp"
#include "Branches.hpp"
#include "Loops.hpp"

using std::string;
using std::ios_base;
using std::list;

using namespace eddic;

inline static void assertNextIsRightParenth(Lexer& lexer, const string& message);
inline static void assertNextIsLeftParenth(Lexer& lexer, const string& message);
inline static void assertNextIsRightBrace(Lexer& lexer, const string& message);
inline static void assertNextIsLeftBrace(Lexer& lexer, const string& message);
inline static void assertNextIsStop(Lexer& lexer, const string& message);

//Move to some utility class
bool isType(const Lexer& lexer) {
    if (!lexer.isWord()) {
        return false;
    }

    string value = lexer.getCurrentToken().value();

    return value == "int" || value == "string";
}

Program* Parser::parse() {
    //Create the global context
    currentContext = new Context();

    Program* program = new Program(currentContext);

    while (lexer.next()) {
        program->addLast(parseInstruction());
    }

    return program;
}

ParseNode* Parser::parseInstruction() {
    if (lexer.isIf()) {
        return parseIf();
    } else if (lexer.isWhile()) {
        return parseWhile();
    } else if (isType(lexer)) {
        return parseDeclaration();
    } else if (lexer.isWord()) {
        return parseCallOrAssignment();
    }

    throw TokenException("Not an instruction", lexer.getCurrentToken());
}

ParseNode* Parser::parseCallOrAssignment() {
    Token token = lexer.getCurrentToken();

    if (!lexer.next()) {
        throw TokenException("Incomplete instruction", lexer.getCurrentToken());
    }

    if (lexer.isLeftParenth()) {
        return parseCall(token);
    } else if (lexer.isAssign()) {
        return parseAssignment(token);
    } else if (lexer.isSwap()) {
        return parseSwap(token);
    }

    throw TokenException("Not an instruction", lexer.getCurrentToken());
}

ParseNode* Parser::parseCall(const Token& callToken) {
    string call = callToken.value();

    if (call != "Print" && call != "Println") {
        throw TokenException("The call \"" + call + "\" does not exist", callToken);
    }

    Value* value = parseValue();

    assertNextIsRightParenth(lexer, "The call must be closed with a right parenth");
    assertNextIsStop(lexer, "Every instruction must be closed by a semicolon");

    if (call == "Print") {
        return new Print(currentContext, value);
    } else {
        return new Println(currentContext, value);
    }
}

ParseNode* Parser::parseDeclaration() {
    string typeName = lexer.getCurrentToken().value();

    Type type;
    if (typeName == "int") {
        type = INT;
    } else {
        type = STRING;
    }

    if (!lexer.next() || !lexer.isWord()) {
        throw TokenException("A type must be followed by variable name", lexer.getCurrentToken());
    }

    string variable = lexer.getCurrentToken().value();

    if (!lexer.next() || !lexer.isAssign()) {
        throw TokenException("A variable declaration must followed by '='", lexer.getCurrentToken());
    }

    Value* value = parseValue();

    assertNextIsStop(lexer, "Every instruction must be closed by a semicolon");

    return new Declaration(currentContext, type, variable, value);
}

ParseNode* Parser::parseAssignment(const Token& variableToken) {
    Value* value = parseValue();

    assertNextIsStop(lexer, "Every instruction must be closed by a semicolon");

    return new Assignment(currentContext, variableToken.value(), value);
}

ParseNode* Parser::parseSwap(const Token& lhs) {
    if (!lexer.next() || !lexer.isWord()) {
        throw TokenException("Can only swap two variables", lexer.getCurrentToken());
    }

    string rhs = lexer.getCurrentToken().value();

    assertNextIsStop(lexer, "Every instruction must be closed by a semicolon");

    return new Swap(currentContext, lhs.value(), rhs);
}

ParseNode* Parser::parseIf() {
    assertNextIsLeftParenth(lexer, "An if instruction must be followed by a condition surrounded by parenth");

    Condition* condition = parseCondition();

    assertNextIsRightParenth(lexer, "The condition of the if must be closed by a right parenth");

    assertNextIsLeftBrace(lexer, "Waiting for a left brace");

    currentContext = new Context(currentContext);

    If* block = new If(currentContext, condition);

    lexer.next();

    while (!lexer.isRightBrace()) {
        block->addLast(parseInstruction());

        lexer.next();
    }

    currentContext = currentContext->parent();

    if (!lexer.isRightBrace()) {
        throw TokenException("If body must be closed with right brace", lexer.getCurrentToken());
    }

    while (true) {
        if (lexer.next()) {
            if (lexer.isElse()) {
                lexer.next();

                if (lexer.isIf()) {
                    block->addElseIf(parseElseIf());
                } else {
                    lexer.pushBack();

                    block->setElse(parseElse());

                    break;
                }
            } else {
                lexer.pushBack();

                break;
            }
        } else {
            break;
        }
    }

    return block;
}

ElseIf* Parser::parseElseIf() {
    assertNextIsLeftParenth(lexer, "An else if instruction must be followed by a condition surrounded by parenth");

    Condition* condition = parseCondition();

    assertNextIsRightParenth(lexer, "The condition of the else if must be closed by a right parenth");

    assertNextIsLeftBrace(lexer, "Waiting for a left brace");

    currentContext = new Context(currentContext);

    ElseIf* block = new ElseIf(currentContext, condition);

    lexer.next();

    while (!lexer.isRightBrace()) {
        block->addLast(parseInstruction());

        lexer.next();
    }

    currentContext = currentContext->parent();

    if (!lexer.isRightBrace()) {
        throw TokenException("Else ff body must be closed with right brace", lexer.getCurrentToken());
    }

    return block;
}

Else* Parser::parseElse() {
    currentContext = new Context(currentContext);

    Else* block = new Else(currentContext);

    assertNextIsLeftBrace(lexer, "else statement must be followed by left brace");

    while (lexer.next() && !lexer.isRightBrace()) {
        block->addLast(parseInstruction());
    }

    if (!lexer.isRightBrace()) {
        throw TokenException("else body must be closed with right brace", lexer.getCurrentToken());
    }

    currentContext = currentContext->parent();

    return block;
}

ParseNode* Parser::parseWhile() {
    assertNextIsLeftParenth(lexer, "A while instruction must be followed by a condition surrounded by parenth");

    Condition* condition = parseCondition();

    assertNextIsRightParenth(lexer, "The condition of the while must be closed by a right parenth");

    assertNextIsLeftBrace(lexer, "Waiting for a left brace");

    currentContext = new Context(currentContext);

    While* block = new While(currentContext, condition);

    lexer.next();

    while (!lexer.isRightBrace()) {
        block->addLast(parseInstruction());

        lexer.next();
    }

    currentContext = currentContext->parent();

    if (!lexer.isRightBrace()) {
        throw TokenException("If body must be closed with right brace", lexer.getCurrentToken());
    }

    return block;
}

enum Operator {
    ADD, MUL, SUB, DIV, MOD, ERROR
};

class Part {
    public:
        virtual bool isResolved() = 0;
        virtual Operator getOperator() {
            return ERROR;
        }
        virtual Value* getValue() {
            return NULL;
        }
};

class Resolved : public Part {
    public:
        explicit Resolved(Value* v) : value(v) {}
        bool isResolved() {
            return true;
        }
        Value* getValue() {
            return value;
        }

    private:
        Value* value;
};

class Unresolved : public Part {
    public:
        explicit Unresolved(Operator o) : op(o) {}
        bool isResolved() {
            return false;
        }
        Operator getOperator() {
            return op;
        }

    private:
        Operator op;
};

int priority(Operator op) {
    switch (op) {
        case MUL:
        case DIV:
        case MOD:
            return 10;
        case ADD:
        case SUB:
            return 0;
        default:
            return -1; //TODO should never happen
    }
}

Value* Parser::parseValue() {
    list<Part*> parts;

    while (true) {
        if (!lexer.next()) {
            throw TokenException("Waiting for a value", lexer.getCurrentToken());
        }

        Value* node = NULL;

        if (lexer.isLeftParenth()) {
            node = parseValue();

            assertNextIsRightParenth(lexer, "parenth is not closed");
        } else if (lexer.isLitteral()) {
            string litteral = lexer.getCurrentToken().value();

            node = new Litteral(currentContext, litteral);
        } else if (lexer.isWord()) {
            string variableRight = lexer.getCurrentToken().value();

            node = new VariableValue(currentContext, variableRight);
        } else if (lexer.isInteger()) {
            string integer = lexer.getCurrentToken().value();
            int value = toNumber<int>(integer);

            node = new Integer(currentContext, value);
        } else {
            throw TokenException("Invalid value", lexer.getCurrentToken());
        }

        parts.push_back(new Resolved(node));

        if (!lexer.next()) {
            break;
        }

        if (lexer.isAddition()) {
            parts.push_back(new Unresolved(ADD));
        } else if (lexer.isSubtraction()) {
            parts.push_back(new Unresolved(SUB));
        } else if (lexer.isMultiplication()) {
            parts.push_back(new Unresolved(MUL));
        } else if (lexer.isDivision()) {
            parts.push_back(new Unresolved(DIV));
        } else if (lexer.isModulo()) {
            parts.push_back(new Unresolved(MOD));
        } else {
            lexer.pushBack();
            break;
        }
    }

    while (parts.size() > 1) {
        int maxPriority = -1;
        list<Part*>::iterator it, end, max;
        for (it = parts.begin(), end = parts.end(); it != end; ++it) {
            Part* part = *it;

            if (!part->isResolved()) {
                if (priority(part->getOperator()) > maxPriority) {
                    maxPriority = priority(part->getOperator());
                    max = it;
                }
            }
        }

        list<Part*>::iterator first = max;
        --first;

        Part* left = *first;
        Part* center = *max;

        ++max;
        Part* right = *max;

        Value* lhs = left->getValue();
        Operator op = center->getOperator();
        Value* rhs = right->getValue();

        Value* value = NULL;

        if (op == ADD) {
            value = new Addition(currentContext, lhs, rhs);
        } else if (op == SUB) {
            value = new Subtraction(currentContext, lhs, rhs);
        } else if (op == MUL) {
            value = new Multiplication(currentContext, lhs, rhs);
        } else if (op == DIV) {
            value = new Division(currentContext, lhs, rhs);
        } else if (op == MOD) {
            value = new Modulo(currentContext, lhs, rhs);
        }

        parts.erase(first, ++max);

        delete left;
        delete center;
        delete right;

        parts.insert(max, new Resolved(value));
    }

    Value* value = (*parts.begin())->getValue();

    delete *parts.begin();

    parts.clear();

    return value;
}

Condition* Parser::parseCondition() {
    lexer.next();

    if (lexer.isTrue()) {
        return new Condition(TRUE_VALUE);
    } else if (lexer.isFalse()) {
        return new Condition(FALSE_VALUE);
    } else {
        lexer.pushBack();
    }

    Value* lhs = parseValue();

    if (!lexer.next()) {
        throw TokenException("waiting for a boolean operator", lexer.getCurrentToken());
    }

    BooleanCondition operation;
    if (lexer.isGreater()) {
        operation = GREATER_OPERATOR;
    } else if (lexer.isLess()) {
        operation = LESS_OPERATOR;
    } else if (lexer.isEquals()) {
        operation = EQUALS_OPERATOR;
    } else if (lexer.isNotEquals()) {
        operation = NOT_EQUALS_OPERATOR;
    } else if (lexer.isGreaterOrEquals()) {
        operation = GREATER_EQUALS_OPERATOR;
    } else if (lexer.isLessOrEquals()) {
        operation = LESS_EQUALS_OPERATOR;
    } else {
        throw TokenException("waiting for a boolean operator", lexer.getCurrentToken());
    }

    Value* rhs = parseValue();

    return new Condition(operation, lhs, rhs);
}

inline static void assertNextIsRightParenth(Lexer& lexer, const string& message) {
    if (!lexer.next() || !lexer.isRightParenth()) {
        throw TokenException(message, lexer.getCurrentToken());
    }
}

inline static void assertNextIsLeftParenth(Lexer& lexer, const string& message) {
    if (!lexer.next() || !lexer.isLeftParenth()) {
        throw TokenException(message, lexer.getCurrentToken());
    }
}

inline static void assertNextIsRightBrace(Lexer& lexer, const string& message) {
    if (!lexer.next() || !lexer.isRightBrace()) {
        throw TokenException(message, lexer.getCurrentToken());
    }
}

inline static void assertNextIsLeftBrace(Lexer& lexer, const string& message) {
    if (!lexer.next() || !lexer.isLeftBrace()) {
        throw TokenException(message, lexer.getCurrentToken());
    }
}

inline static void assertNextIsStop(Lexer& lexer, const string& message) {
    if (!lexer.next() || !lexer.isStop()) {
        throw TokenException(message, lexer.getCurrentToken());
    }
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2016 Dariusz Stojaczyk. All Rights Reserved.
 * The following source code is released under an MIT-style license,
 * that can be found in the LICENSE file.
 */

/**
 * NOTE: This class requires heavy exception handling.
 * If something goes wrong it would be a real hell to debug
 */

#include <memory>
#include <regex>
#include <unordered_map>
#include <cstdlib>

#include "Atlas.h"
#include "Data.h"
#include "util/collection.h"
#include "util/file.h"
#include "Resampler.h"
#include "util/Packer.h"
#include "exceptions.h"
#include "util/log.h"

using namespace texture;

/**
 * The following state may not be available on some devices (e.g. OpenGL ES 2.0)
 */
#ifdef GL_TEXTURE_MAX_LEVEL
#define USES_MANUAL_MIPMAPS
#endif

#ifndef USES_MANUAL_MIPMAPS
#include <SOIL.h>
#endif

struct Atlas::impl {
    impl(std::vector<std::string> tiles)
        : m_tiles(util::collection::filter(tiles, FILE_EXTENSION_REGEX)) {

    }

    /**
     * OpenGL uses enum to determine a number (and type) of texture channels.
     * Since we don't handle different types of texture channels in Atlas class,
     * just a pure int number of channels is passed between methods.
     */
    GLenum getTexGLFormat(uint32_t channels) {
        GLenum ret;

        switch (channels) {
            case 1:
                ret = GL_LUMINANCE; /**< If L was converted into RGBA, it would be R=L, G=L, B=L, A=1 */
                break;
            case 2:
                ret = GL_LUMINANCE_ALPHA; /**< If LA' was converted into RGBA, it would be R=L, G=L, B=L, A=A' */
                break;
            case 3:
                ret = GL_RGB; /**< A=1 */
                break;
            case 4:
                ret = GL_RGBA;
                break;
            default:
                char msg[100];
                snprintf(msg, sizeof(msg), "Trying to convert invalid channel number into an OpenGL texture format (channels:%d).", channels);
                throw invalid_texture_error(msg);
        }

        return ret;
    }

    void writeTexToGPU(const Data &tex, uint32_t level) {
        uint32_t channels = tex.channels();

        if (channels < 1 || channels > 4) {
            char msg[90];
            snprintf(msg, sizeof(msg), "Trying to write to GPU texture with invalid amount of channels (channels:%d).", channels);
            throw invalid_texture_error(msg);
        }

#ifdef DBG_COLORMIPMAPS
        // color mipmaps in scheme: red (biggest) -> yellow (smallest)
        if (channels >= 3) {
            for (int i = 0; i < tex.width() * tex.height(); ++i) {
                resampled[channels * i + 0] = 255 - level * 40;
                resampled[channels * i + 1] = level * 40;
                resampled[channels * i + 2] = 0;
                if (channels >= 4) {
                    resampled[channels * i + 3] = 255;
                }
            }
        }
#endif // DBG_COLORMIPMAPS

        if (channels == 4) {
            // burn alpha into RGB channels
            for (int i = 0; i < tex.width() * tex.height(); ++i) {
                tex.getData()[channels * i + 0] = (uint8_t) ((tex.getData()[channels * i + 0] * tex.getData()[channels * i + 3] + 128) >> 8);
                tex.getData()[channels * i + 1] = (uint8_t) ((tex.getData()[channels * i + 1] * tex.getData()[channels * i + 3] + 128) >> 8);
                tex.getData()[channels * i + 2] = (uint8_t) ((tex.getData()[channels * i + 2] * tex.getData()[channels * i + 3] + 128) >> 8);
            }
        }

        glTexImage2D(GL_TEXTURE_2D, level, tex.channels(), tex.width(), tex.height(), 0, getTexGLFormat(tex.channels()), GL_UNSIGNED_BYTE, tex.getData().get());
    }

    util::Packer m_packer;
    std::hash<std::string> m_hash;
    std::unordered_map<uint64_t, Data> m_texData;

    std::vector<std::string> m_tiles;
    static const std::regex FILE_EXTENSION_REGEX;
};

const std::regex Atlas::impl::FILE_EXTENSION_REGEX = std::regex("(.*)(\\.(jpg|png|gif))", std::regex::ECMAScript | std::regex::icase);

Atlas::Atlas(const std::string &name)
    : Texture(name),
      m_impl(std::make_unique<Atlas::impl>(util::file::list(util::file::path<util::file::type::texture>(name).c_str()))) {

}

void Atlas::load() {
    for (const std::string &tile : m_impl->m_tiles) {
        loadTile(tile);
    }

    m_impl->m_packer.pack();

    m_width = m_impl->m_packer.size().width();
    m_height = m_impl->m_packer.size().height();
    m_channels = 4;

#ifdef USES_MANUAL_MIPMAPS
    glGenTextures(1, &m_id);
    bindTexture();

    uint32_t mipmapLevels = (uint32_t) ceil(log(std::max(width(), height())) / log(2));

    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAX_LEVEL, mipmapLevels - 1);

    for (uint8_t level = 0; level < mipmapLevels; ++level) {
#else
        uint8_t level = 0;
#endif

        uint32_t downsample = 1u << level;
        Data atlas(width() / downsample, height() / downsample, channels());

        //TODO it's totally unreadable
        for (auto &el : m_impl->m_packer.elements()) {
            Data &tile = m_impl->m_texData.find(el.first)->second;
            auto resampled = texture::Resampler::downsample(tile.getData().get(), tile.width(), tile.height(), tile.channels(), downsample);
            util::Rectangle rect(el.second.x() / downsample, el.second.y() / downsample, el.second.width() / downsample, el.second.height() / downsample);

            uint32_t inWidth = tile.width() / downsample;
            uint32_t inHeight = tile.height() / downsample;
            uint32_t outWidth = m_impl->m_packer.size().width() / downsample;

            for (int yIn = 0; yIn < inHeight; ++yIn) {
                for (int xIn = 0; xIn < inWidth; ++xIn) {
                    uint32_t inPixelPos = xIn + yIn * inWidth;
                    uint32_t outPixelPos = rect.x() + xIn + (rect.y() + yIn) * outWidth;
                    for (int channel = 0; channel < channels(); ++channel) {
                        atlas.getData()[4 * outPixelPos + channel] = resampled[channels() * inPixelPos + channel];
                    }
                }
            }
        }

#ifdef USES_MANUAL_MIPMAPS
        m_impl->writeTexToGPU(atlas, level);
    }
#else
    //TODO create additional writeTexToGPU overload (?)
    m_id = SOIL_create_OGL_texture(atlas.getData().get(), m_width, m_height, m_channels, 0, SOIL_FLAG_MULTIPLY_ALPHA);
    glGenerateMipmap(GL_TEXTURE_2D);
#endif
}

void Atlas::loadTile(const std::string &fileName) {
    Data tex(m_path + util::file::file_separator_str + fileName);
    uint64_t id = m_impl->m_hash(fileName.substr(0, fileName.find_last_of('.')));

    m_impl->m_packer.add({id, util::Rectangle(tex.width(), tex.height())});
    m_impl->m_texData.emplace(id, std::move(tex));
}

const util::Rectangle Atlas::element(const std::string &name) const {
    uint64_t id = m_impl->m_hash(name);
    auto elements = m_impl->m_packer.elements();

    auto it = std::find_if(elements.begin(), elements.end(), [id](const util::Packer::Element &element) {
        return element.first == id;
    });

    if (it == elements.end()) {
        char msg[100];
        snprintf(msg, sizeof(msg), "Trying to get inexistent element (\"%d\") from texture atlas.", name.c_str());
        throw invalid_texture_error(msg);
    }

    return it->second;
}

const uint64_t Atlas::getElementsNum() const {
    return m_impl->m_texData.size();
}

Atlas::~Atlas() {}<commit_msg>Added profiler entries<commit_after>/*
 * Copyright (c) 2016 Dariusz Stojaczyk. All Rights Reserved.
 * The following source code is released under an MIT-style license,
 * that can be found in the LICENSE file.
 */

/**
 * NOTE: This class requires heavy exception handling.
 * If something goes wrong it would be a real hell to debug
 */

#include <memory>
#include <regex>
#include <unordered_map>
#include <cstdlib>

#include "Atlas.h"
#include "Data.h"
#include "util/collection.h"
#include "util/file.h"
#include "Resampler.h"
#include "util/Packer.h"
#include "exceptions.h"
#include "util/log.h"
#include "util/prof.h"

using namespace texture;

/**
 * The following state may not be available on some devices (e.g. OpenGL ES 2.0)
 */
#ifdef GL_TEXTURE_MAX_LEVEL
#define USES_MANUAL_MIPMAPS
#endif

#ifndef USES_MANUAL_MIPMAPS
#include <SOIL.h>
#endif

struct Atlas::impl {
    impl(std::vector<std::string> tiles)
        : m_tiles(util::collection::filter(tiles, FILE_EXTENSION_REGEX)) {

    }

    /**
     * OpenGL uses enum to determine a number (and type) of texture channels.
     * Since we don't handle different types of texture channels in Atlas class,
     * just a pure int number of channels is passed between methods.
     */
    GLenum getTexGLFormat(uint32_t channels) {
        GLenum ret;

        switch (channels) {
            case 1:
                ret = GL_LUMINANCE; /**< If L was converted into RGBA, it would be R=L, G=L, B=L, A=1 */
                break;
            case 2:
                ret = GL_LUMINANCE_ALPHA; /**< If LA' was converted into RGBA, it would be R=L, G=L, B=L, A=A' */
                break;
            case 3:
                ret = GL_RGB; /**< A=1 */
                break;
            case 4:
                ret = GL_RGBA;
                break;
            default:
                char msg[100];
                snprintf(msg, sizeof(msg), "Trying to convert invalid channel number into an OpenGL texture format (channels:%d).", channels);
                throw invalid_texture_error(msg);
        }

        return ret;
    }

    void writeTexToGPU(const Data &tex, uint32_t level) {
        uint32_t channels = tex.channels();

        if (channels < 1 || channels > 4) {
            char msg[90];
            snprintf(msg, sizeof(msg), "Trying to write to GPU texture with invalid amount of channels (channels:%d).", channels);
            throw invalid_texture_error(msg);
        }

#ifdef DBG_COLORMIPMAPS
        // color mipmaps in scheme: red (biggest) -> yellow (smallest)
        if (channels >= 3) {
            for (int i = 0; i < tex.width() * tex.height(); ++i) {
                resampled[channels * i + 0] = 255 - level * 40;
                resampled[channels * i + 1] = level * 40;
                resampled[channels * i + 2] = 0;
                if (channels >= 4) {
                    resampled[channels * i + 3] = 255;
                }
            }
        }
#endif // DBG_COLORMIPMAPS

        if (channels == 4) {
            // burn alpha into RGB channels
            for (int i = 0; i < tex.width() * tex.height(); ++i) {
                tex.getData()[channels * i + 0] = (uint8_t) ((tex.getData()[channels * i + 0] * tex.getData()[channels * i + 3] + 128) >> 8);
                tex.getData()[channels * i + 1] = (uint8_t) ((tex.getData()[channels * i + 1] * tex.getData()[channels * i + 3] + 128) >> 8);
                tex.getData()[channels * i + 2] = (uint8_t) ((tex.getData()[channels * i + 2] * tex.getData()[channels * i + 3] + 128) >> 8);
            }
        }

        glTexImage2D(GL_TEXTURE_2D, level, tex.channels(), tex.width(), tex.height(), 0, getTexGLFormat(tex.channels()), GL_UNSIGNED_BYTE, tex.getData().get());
    }

    util::Packer m_packer;
    std::hash<std::string> m_hash;
    std::unordered_map<uint64_t, Data> m_texData;

    std::vector<std::string> m_tiles;
    static const std::regex FILE_EXTENSION_REGEX;
};

const std::regex Atlas::impl::FILE_EXTENSION_REGEX = std::regex("(.*)(\\.(jpg|png|gif))", std::regex::ECMAScript | std::regex::icase);

Atlas::Atlas(const std::string &name)
    : Texture(name),
      m_impl(std::make_unique<Atlas::impl>(util::file::list(util::file::path<util::file::type::texture>(name).c_str()))) {

}

void Atlas::load() {
    PROF_START(load);
    for (const std::string &tile : m_impl->m_tiles) {
        loadTile(tile);
    }

    m_impl->m_packer.pack();

    m_width = m_impl->m_packer.size().width();
    m_height = m_impl->m_packer.size().height();
    m_channels = 4;

#ifdef USES_MANUAL_MIPMAPS
    glGenTextures(1, &m_id);
    bindTexture();

    uint32_t mipmapLevels = (uint32_t) ceil(log(std::max(width(), height())) / log(2));

    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAX_LEVEL, mipmapLevels - 1);

    Log::debug("Preparing the OpenGL texture \"%s\" took %f sec.", m_path.c_str(), PROF_DURATION_PREV(load));

    for (uint8_t level = 0; level < mipmapLevels; ++level) {
#else
        uint8_t level = 0;
#endif

        uint32_t downsample = 1u << level;
        Data atlas(width() / downsample, height() / downsample, channels());

        Log::debug("Preparing the texture atlas \"%s\" took %f sec.", m_path.c_str(), PROF_DURATION_PREV(load));

        //TODO it's totally unreadable
        for (auto &el : m_impl->m_packer.elements()) {
            Data &tile = m_impl->m_texData.find(el.first)->second;
            auto resampled = texture::Resampler::downsample(tile.getData().get(), tile.width(), tile.height(), tile.channels(), downsample);
            util::Rectangle rect(el.second.x() / downsample, el.second.y() / downsample, el.second.width() / downsample, el.second.height() / downsample);

            Log::debug("Resampling of level %d of the texture atlas \"%s\" took %f sec.", level, m_path.c_str(), PROF_DURATION_PREV(load));

            uint32_t inWidth = tile.width() / downsample;
            uint32_t inHeight = tile.height() / downsample;
            uint32_t outWidth = m_impl->m_packer.size().width() / downsample;

            for (int yIn = 0; yIn < inHeight; ++yIn) {
                for (int xIn = 0; xIn < inWidth; ++xIn) {
                    uint32_t inPixelPos = xIn + yIn * inWidth;
                    uint32_t outPixelPos = rect.x() + xIn + (rect.y() + yIn) * outWidth;
                    for (int channel = 0; channel < channels(); ++channel) {
                        atlas.getData()[4 * outPixelPos + channel] = resampled[channels() * inPixelPos + channel];
                    }
                }
            }

            Log::debug("Preparing the texture atlas \"%s\" took %f sec.", m_path.c_str(), PROF_DURATION_PREV(load));
        }

#ifdef USES_MANUAL_MIPMAPS
        m_impl->writeTexToGPU(atlas, level);
    }
#else
    //TODO create additional writeTexToGPU overload (?)
    m_id = SOIL_create_OGL_texture(atlas.getData().get(), m_width, m_height, m_channels, 0, SOIL_FLAG_MULTIPLY_ALPHA);
    glGenerateMipmap(GL_TEXTURE_2D);
#endif

    Log::debug("Generating the texture atlas \"%s\" took %f sec.", m_path.c_str(), PROF_DURATION_START(load));
}

void Atlas::loadTile(const std::string &fileName) {
    Data tex(m_path + util::file::file_separator_str + fileName);
    uint64_t id = m_impl->m_hash(fileName.substr(0, fileName.find_last_of('.')));

    m_impl->m_packer.add({id, util::Rectangle(tex.width(), tex.height())});
    m_impl->m_texData.emplace(id, std::move(tex));
}

const util::Rectangle Atlas::element(const std::string &name) const {
    uint64_t id = m_impl->m_hash(name);
    auto elements = m_impl->m_packer.elements();

    auto it = std::find_if(elements.begin(), elements.end(), [id](const util::Packer::Element &element) {
        return element.first == id;
    });

    if (it == elements.end()) {
        char msg[100];
        snprintf(msg, sizeof(msg), "Trying to get inexistent element (\"%d\") from texture atlas.", name.c_str());
        throw invalid_texture_error(msg);
    }

    return it->second;
}

const uint64_t Atlas::getElementsNum() const {
    return m_impl->m_texData.size();
}

Atlas::~Atlas() {}<|endoftext|>


<commit_before>/****************************************************************************
**
** Copyright (C) 2009 Nokia Corporation and/or its subsidiary(-ies).
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the documentation of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:LGPL$
** Commercial Usage
** Licensees holding valid Qt Commercial licenses may use this file in
** accordance with the Qt Commercial License Agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and Nokia.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain
** additional rights. These rights are described in the Nokia Qt LGPL
** Exception version 1.0, included in the file LGPL_EXCEPTION.txt in this
** package.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU
** General Public License version 3.0 as published by the Free Software
** Foundation and appearing in the file LICENSE.GPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU General Public License version 3.0 requirements will be
** met: http://www.gnu.org/copyleft/gpl.html.
**
** If you are unsure which license is appropriate for your use, please
** contact the sales department at http://www.qtsoftware.com/contact.
** $QT_END_LICENSE$
**
****************************************************************************/

#include <QtGui>

QWidget *win
QWidget *panel

void snippet_ctor1()
{
//! [0]
    settings = QSettings("MySoft", "Star Runner")
//! [0]
}

void snippet_ctor2()
{
//! [1]
    QCoreApplication.setOrganizationName("MySoft")
//! [1] //! [2]
    QCoreApplication.setOrganizationDomain("mysoft.com")
//! [2] //! [3]
    QCoreApplication.setApplicationName("Star Runner")
//! [3]

//! [4]
    settings = QSettings()
//! [4]

//! [5]
    settings.setValue("editor/wrapMargin", 68)
//! [5] //! [6]
    margin = settings.value("editor/wrapMargin").toInt()
//! [6]
    {
//! [7]
    margin = settings.value("editor/wrapMargin", 80).toInt()
//! [7]
    }

//! [8]
    settings.setValue("mainwindow/size", win.size())
//! [8] //! [9]
    settings.setValue("mainwindow/fullScreen", win.isFullScreen())
//! [9] //! [10]
    settings.setValue("outputpanel/visible", panel.isVisible())
//! [10]

//! [11]
    settings.beginGroup("mainwindow")
    settings.setValue("size", win.size())
    settings.setValue("fullScreen", win.isFullScreen())
    settings.endGroup()
//! [11]

//! [12]
    settings.beginGroup("outputpanel")
    settings.setValue("visible", panel.isVisible())
    settings.endGroup()
//! [12]
}

void snippet_locations()
{
//! [13]
    obj1 = QSettings("MySoft", "Star Runner")
//! [13] //! [14]
    obj2 = QSettings("MySoft")
    obj3 = QSettings(QSettings.SystemScope, "MySoft", "Star Runner")
    obj4 = QSettings(QSettings.SystemScope, "MySoft")
//! [14]

    {
//! [15]
    settings = QSettings(QSettings.IniFormat, QSettings.UserScope,
                         "MySoft", "Star Runner")
//! [15]
    }

    {
    QSettings settings("starrunner.ini", QSettings.IniFormat)
    }

    {
    QSettings settings("HKEY_CURRENT_USER\\Software\\Microsoft",
                       QSettings.NativeFormat)
    }
}

class MainWindow : public QMainWindow
{
public:
    MainWindow()

    void writeSettings()
    void readSettings()

protected:
    void closeEvent(QCloseEvent *event)
}

//! [16]
class MainWindow(QMainWindow):
    ...
    def writeSettings(self):
        self.settings = QSettings("Moose Soft", "Clipper")
        self.settings.beginGroup("MainWindow")
        self.settings.setValue("size", self.size())
        self.settings.setValue("pos", self.pos())
        self.settings.endGroup()
//! [16]

//! [17]
    def readSettings(self):
        self.settings = QSettings("Moose Soft", "Clipper")
        self.settings.beginGroup("MainWindow")
        self.resize(settings.value("size", QSize(400, 400)).toSize())
        self.move(settings.value("pos", QPoint(200, 200)).toPoint())
        self.settings.endGroup()
//! [17]

//! [18]
    def __init__(self):
        self.settings = None
//! [18] //! [19]
        self.readSettings()
//! [19] //! [20]

//! [20]

bool userReallyWantsToQuit() { return true; }

//! [21]
    # event : QCloseEvent
    def closeEvent(self, event):
        if self.userReallyWantsToQuit():
            self.writeSettings()
            event.accept()
        else:
            event.ignore()
//! [21]
<commit_msg>Fix QSettings code snippet.<commit_after>/****************************************************************************
**
** Copyright (C) 2009 Nokia Corporation and/or its subsidiary(-ies).
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the documentation of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:LGPL$
** Commercial Usage
** Licensees holding valid Qt Commercial licenses may use this file in
** accordance with the Qt Commercial License Agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and Nokia.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain
** additional rights. These rights are described in the Nokia Qt LGPL
** Exception version 1.0, included in the file LGPL_EXCEPTION.txt in this
** package.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU
** General Public License version 3.0 as published by the Free Software
** Foundation and appearing in the file LICENSE.GPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU General Public License version 3.0 requirements will be
** met: http://www.gnu.org/copyleft/gpl.html.
**
** If you are unsure which license is appropriate for your use, please
** contact the sales department at http://www.qtsoftware.com/contact.
** $QT_END_LICENSE$
**
****************************************************************************/

#include <QtGui>

QWidget *win
QWidget *panel

void snippet_ctor1()
{
//! [0]
    settings = QSettings("MySoft", "Star Runner")
//! [0]
}

void snippet_ctor2()
{
//! [1]
    QCoreApplication.setOrganizationName("MySoft")
//! [1] //! [2]
    QCoreApplication.setOrganizationDomain("mysoft.com")
//! [2] //! [3]
    QCoreApplication.setApplicationName("Star Runner")
//! [3]

//! [4]
    settings = QSettings()
//! [4]

//! [5]
    settings.setValue("editor/wrapMargin", 68)
//! [5] //! [6]
    margin = int(settings.value("editor/wrapMargin"))
//! [6]
    {
//! [7]
    margin = int(settings.value("editor/wrapMargin", 80))
//! [7]
    }

//! [8]
    settings.setValue("mainwindow/size", win.size())
//! [8] //! [9]
    settings.setValue("mainwindow/fullScreen", win.isFullScreen())
//! [9] //! [10]
    settings.setValue("outputpanel/visible", panel.isVisible())
//! [10]

//! [11]
    settings.beginGroup("mainwindow")
    settings.setValue("size", win.size())
    settings.setValue("fullScreen", win.isFullScreen())
    settings.endGroup()
//! [11]

//! [12]
    settings.beginGroup("outputpanel")
    settings.setValue("visible", panel.isVisible())
    settings.endGroup()
//! [12]
}

void snippet_locations()
{
//! [13]
    obj1 = QSettings("MySoft", "Star Runner")
//! [13] //! [14]
    obj2 = QSettings("MySoft")
    obj3 = QSettings(QSettings.SystemScope, "MySoft", "Star Runner")
    obj4 = QSettings(QSettings.SystemScope, "MySoft")
//! [14]

    {
//! [15]
    settings = QSettings(QSettings.IniFormat, QSettings.UserScope,
                         "MySoft", "Star Runner")
//! [15]
    }

    {
    QSettings settings("starrunner.ini", QSettings.IniFormat)
    }

    {
    QSettings settings("HKEY_CURRENT_USER\\Software\\Microsoft",
                       QSettings.NativeFormat)
    }
}

class MainWindow : public QMainWindow
{
public:
    MainWindow()

    void writeSettings()
    void readSettings()

protected:
    void closeEvent(QCloseEvent *event)
}

//! [16]
class MainWindow(QMainWindow):
    ...
    def writeSettings(self):
        self.settings = QSettings("Moose Soft", "Clipper")
        self.settings.beginGroup("MainWindow")
        self.settings.setValue("size", self.size())
        self.settings.setValue("pos", self.pos())
        self.settings.endGroup()
//! [16]

//! [17]
    def readSettings(self):
        self.settings = QSettings("Moose Soft", "Clipper")
        self.settings.beginGroup("MainWindow")
        self.resize(settings.value("size", QSize(400, 400)).toSize())
        self.move(settings.value("pos", QPoint(200, 200)).toPoint())
        self.settings.endGroup()
//! [17]

//! [18]
    def __init__(self):
        self.settings = None
//! [18] //! [19]
        self.readSettings()
//! [19] //! [20]

//! [20]

bool userReallyWantsToQuit() { return true; }

//! [21]
    # event : QCloseEvent
    def closeEvent(self, event):
        if self.userReallyWantsToQuit():
            self.writeSettings()
            event.accept()
        else:
            event.ignore()
//! [21]
<|endoftext|>