<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: FieldDescriptions.hxx,v $
 *
 *  $Revision: 1.11 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-08 15:17:23 $
 *
 *  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 DBAUI_FIELDDESCRIPTIONS_HXX
#define DBAUI_FIELDDESCRIPTIONS_HXX

#ifndef DBAUI_ENUMTYPES_HXX
#include "QEnumTypes.hxx"
#endif
#ifndef _SVX_SVXENUM_HXX
#include <svx/svxenum.hxx>
#endif
#ifndef DBAUI_TYPEINFO_HXX
#include "TypeInfo.hxx"
#endif
#ifndef _OSL_DIAGNOSE_H_
#include <osl/diagnose.h>
#endif
#ifndef _COM_SUN_STAR_BEANS_XPROPERTYSET_HPP_
#include <com/sun/star/beans/XPropertySet.hpp>
#endif
#ifndef _COM_SUN_STAR_BEANS_XPROPERTYSETINFO_HPP_
#include <com/sun/star/beans/XPropertySetInfo.hpp>
#endif

namespace dbaui
{
    class OFieldDescription

    {
    private:
        ::com::sun::star::uno::Any      m_aDefaultValue;    // the default value from the database
        ::com::sun::star::uno::Any      m_aControlDefault;  // the value which the control inserts as default

        TOTypeInfoSP    m_pType;

        ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet >       m_xDest;
        ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySetInfo >   m_xDestInfo;

        ::rtl::OUString     m_sName;
        ::rtl::OUString     m_sTypeName;
        ::rtl::OUString     m_sDescription;

        ::rtl::OUString     m_sAutoIncrementValue;
        sal_Int32           m_nType;    // only used when m_pType is null
        sal_Int32           m_nPrecision;
        sal_Int32           m_nScale;
        sal_Int32           m_nIsNullable;
        sal_Int32           m_nFormatKey;
        SvxCellHorJustify   m_eHorJustify;
        sal_Bool            m_bIsAutoIncrement;
        sal_Bool            m_bIsPrimaryKey;
        sal_Bool            m_bIsCurrency;

    public:
        OFieldDescription();
        OFieldDescription(const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet >& _xAffectedCol
                         ,sal_Bool _bUseAsDest = sal_False);
        OFieldDescription(  const ::rtl::OUString&  _sName,
                            const ::rtl::OUString&  _sTypeName,
                            const ::rtl::OUString&  _sDescription,
                            const ::com::sun::star::uno::Any&   _aDefaultValue,
                            const ::com::sun::star::uno::Any&   _aControlDefault,
                            const ::rtl::OUString&  _sAutoIncrementValue,
                            const TOTypeInfoSP&     _pType,
                            sal_Int32               _nPrecision,
                            sal_Int32               _nScale,
                            sal_Int32               _nIsNullable,
                            sal_Int32               _nFormatKey,
                            SvxCellHorJustify       _eHorJustify,
                            sal_Bool                _bIsAutoIncrement,
                            sal_Bool                _bIsPrimaryKey,
                            sal_Bool                _bIsCurrency);
        OFieldDescription( const OFieldDescription& rDescr );
        ~OFieldDescription();

        void SetName(const ::rtl::OUString& _rName);
        void SetDescription(const ::rtl::OUString& _rDescription);
        void SetDefaultValue(const ::com::sun::star::uno::Any& _rDefaultValue);
        void SetControlDefault(const ::com::sun::star::uno::Any& _rControlDefault);
        void SetAutoIncrementValue(const ::rtl::OUString& _sAutoIncValue);
        void SetType(TOTypeInfoSP _pType);
        void SetTypeValue(sal_Int32 _nType);
        void SetTypeName(const ::rtl::OUString& _sTypeName);
        void SetPrecision(const sal_Int32& _rPrecision);
        void SetScale(const sal_Int32& _rScale);
        void SetIsNullable(const sal_Int32& _rIsNullable);
        void SetFormatKey(const sal_Int32& _rFormatKey);
        void SetHorJustify(const SvxCellHorJustify& _rHorJustify);
        void SetAutoIncrement(sal_Bool _bAuto);
        void SetPrimaryKey(sal_Bool _bPKey);
        void SetCurrency(sal_Bool _bIsCurrency);

        void FillFromTypeInfo(const TOTypeInfoSP& _pType,sal_Bool _bForce,sal_Bool _bReset);

        ::rtl::OUString             GetName()               const;
        ::rtl::OUString             GetDescription()        const;
        ::com::sun::star::uno::Any  GetDefaultValue()       const;
        ::com::sun::star::uno::Any  GetControlDefault()     const;
        ::rtl::OUString             GetAutoIncrementValue() const;
        sal_Int32                   GetType()               const;
        ::rtl::OUString             GetTypeName()           const;
        sal_Int32                   GetPrecision()          const;
        sal_Int32                   GetScale()              const;
        sal_Int32                   GetIsNullable()         const;
        sal_Int32                   GetFormatKey()          const;
        SvxCellHorJustify           GetHorJustify()         const;
        TOTypeInfoSP                getTypeInfo()           const;
        sal_Bool                    IsAutoIncrement()       const;
        sal_Bool                    IsPrimaryKey()          const;
        sal_Bool                    IsCurrency()            const;
        sal_Bool                    IsNullable()            const;
    };
}
#endif // DBAUI_FIELDDESCRIPTIONS_HXX

<commit_msg>INTEGRATION: CWS dba30 (1.11.14); FILE MERGED 2005/12/12 08:03:12 oj 1.11.14.1: #i18609# check type on target source<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: FieldDescriptions.hxx,v $
 *
 *  $Revision: 1.12 $
 *
 *  last change: $Author: hr $ $Date: 2006-08-15 10:52:23 $
 *
 *  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 DBAUI_FIELDDESCRIPTIONS_HXX
#define DBAUI_FIELDDESCRIPTIONS_HXX

#ifndef DBAUI_ENUMTYPES_HXX
#include "QEnumTypes.hxx"
#endif
#ifndef _SVX_SVXENUM_HXX
#include <svx/svxenum.hxx>
#endif
#ifndef DBAUI_TYPEINFO_HXX
#include "TypeInfo.hxx"
#endif
#ifndef _OSL_DIAGNOSE_H_
#include <osl/diagnose.h>
#endif
#ifndef _COM_SUN_STAR_BEANS_XPROPERTYSET_HPP_
#include <com/sun/star/beans/XPropertySet.hpp>
#endif
#ifndef _COM_SUN_STAR_BEANS_XPROPERTYSETINFO_HPP_
#include <com/sun/star/beans/XPropertySetInfo.hpp>
#endif

namespace dbaui
{
    class OFieldDescription

    {
    private:
        ::com::sun::star::uno::Any      m_aDefaultValue;    // the default value from the database
        ::com::sun::star::uno::Any      m_aControlDefault;  // the value which the control inserts as default
        ::com::sun::star::uno::Any      m_aWidth;               // sal_Int32 or void
        ::com::sun::star::uno::Any      m_aRelativePosition;    // sal_Int32 or void

        TOTypeInfoSP    m_pType;

        ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet >       m_xDest;
        ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySetInfo >   m_xDestInfo;

        ::rtl::OUString     m_sName;
        ::rtl::OUString     m_sTypeName;
        ::rtl::OUString     m_sDescription;

        ::rtl::OUString     m_sAutoIncrementValue;
        sal_Int32           m_nType;    // only used when m_pType is null
        sal_Int32           m_nPrecision;
        sal_Int32           m_nScale;
        sal_Int32           m_nIsNullable;
        sal_Int32           m_nFormatKey;
        SvxCellHorJustify   m_eHorJustify;
        sal_Bool            m_bIsAutoIncrement;
        sal_Bool            m_bIsPrimaryKey;
        sal_Bool            m_bIsCurrency;
        sal_Bool            m_bHidden;

    public:
        OFieldDescription();
        OFieldDescription(const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet >& _xAffectedCol
                         ,sal_Bool _bUseAsDest = sal_False);
        OFieldDescription(  const ::rtl::OUString&  _sName,
                            const ::rtl::OUString&  _sTypeName,
                            const ::rtl::OUString&  _sDescription,
                            const ::com::sun::star::uno::Any&   _aDefaultValue,
                            const ::com::sun::star::uno::Any&   _aControlDefault,
                            const ::rtl::OUString&  _sAutoIncrementValue,
                            const TOTypeInfoSP&     _pType,
                            sal_Int32               _nPrecision,
                            sal_Int32               _nScale,
                            sal_Int32               _nIsNullable,
                            sal_Int32               _nFormatKey,
                            SvxCellHorJustify       _eHorJustify,
                            sal_Bool                _bIsAutoIncrement,
                            sal_Bool                _bIsPrimaryKey,
                            sal_Bool                _bIsCurrency);
        OFieldDescription( const OFieldDescription& rDescr );
        ~OFieldDescription();

        void SetName(const ::rtl::OUString& _rName);
        void SetDescription(const ::rtl::OUString& _rDescription);
        void SetDefaultValue(const ::com::sun::star::uno::Any& _rDefaultValue);
        void SetControlDefault(const ::com::sun::star::uno::Any& _rControlDefault);
        void SetAutoIncrementValue(const ::rtl::OUString& _sAutoIncValue);
        void SetType(TOTypeInfoSP _pType);
        void SetTypeValue(sal_Int32 _nType);
        void SetTypeName(const ::rtl::OUString& _sTypeName);
        void SetPrecision(const sal_Int32& _rPrecision);
        void SetScale(const sal_Int32& _rScale);
        void SetIsNullable(const sal_Int32& _rIsNullable);
        void SetFormatKey(const sal_Int32& _rFormatKey);
        void SetHorJustify(const SvxCellHorJustify& _rHorJustify);
        void SetAutoIncrement(sal_Bool _bAuto);
        void SetPrimaryKey(sal_Bool _bPKey);
        void SetCurrency(sal_Bool _bIsCurrency);

        /** copies the content of the field eescription into the column
            @param  _rxColumn the dest
        */
        void copyColumnSettingsTo(const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet >& _rxColumn);

        void FillFromTypeInfo(const TOTypeInfoSP& _pType,sal_Bool _bForce,sal_Bool _bReset);

        ::rtl::OUString             GetName()               const;
        ::rtl::OUString             GetDescription()        const;
        ::com::sun::star::uno::Any  GetDefaultValue()       const;
        ::com::sun::star::uno::Any  GetControlDefault()     const;
        ::rtl::OUString             GetAutoIncrementValue() const;
        sal_Int32                   GetType()               const;
        ::rtl::OUString             GetTypeName()           const;
        sal_Int32                   GetPrecision()          const;
        sal_Int32                   GetScale()              const;
        sal_Int32                   GetIsNullable()         const;
        sal_Int32                   GetFormatKey()          const;
        SvxCellHorJustify           GetHorJustify()         const;
        TOTypeInfoSP                getTypeInfo()           const;
        sal_Bool                    IsAutoIncrement()       const;
        sal_Bool                    IsPrimaryKey()          const;
        sal_Bool                    IsCurrency()            const;
        sal_Bool                    IsNullable()            const;
    };
}
#endif // DBAUI_FIELDDESCRIPTIONS_HXX

<|endoftext|>


<commit_before><commit_msg>Questão da árvore por nível adicionada<commit_after><|endoftext|>


<commit_before>/*
*   Copyright (c) 2010-2013, MIT Probabilistic Computing Project
*
*   Lead Developers: Dan Lovell and Jay Baxter
*   Authors: Dan Lovell, Baxter Eaves, Jay Baxter, Vikash Mansinghka
*   Research Leads: Vikash Mansinghka, Patrick Shafto
*
*   Licensed under the Apache License, Version 2.0 (the "License");
*   you may not use this file except in compliance with the License.
*   You may obtain a copy of the License at
*
*       http://www.apache.org/licenses/LICENSE-2.0
*
*   Unless required by applicable law or agreed to in writing, software
*   distributed under the License is distributed on an "AS IS" BASIS,
*   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*   See the License for the specific language governing permissions and
*   limitations under the License.
*/
#include <iostream>
#include <map>
#include <vector>
#include <string>
#include "MultinomialComponentModel.h"
#include "RandomNumberGenerator.h"
#include "utils.h"

using namespace std;

typedef MultinomialComponentModel MCM;

void insert_elements(MCM &mcm, vector<double> elements) {
  vector<double>::iterator it;
  for(it=elements.begin(); it!=elements.end(); it++)
    mcm.insert_element(*it);
}

void remove_elements(MCM &mcm, vector<double> elements) {
  vector<double>::iterator it;
  for(it=elements.begin(); it!=elements.end(); it++)
    mcm.remove_element(*it);
}

int main() {
  cout << endl << "Begin:: test_multinomial_component_model" << endl;
  RandomNumberGenerator rng;

  // test settings
  int NUM_BUCKETS = 5;
  int num_values_to_test = 30;
  double precision = 1E-10;
  map<string, double> hypers;

  // generate all the random data to use
  //
  // initial parameters
  vector<double> dirichlet_alphas_to_test;
  dirichlet_alphas_to_test.push_back(0.5);
  dirichlet_alphas_to_test.push_back(1.0);
  dirichlet_alphas_to_test.push_back(10.0);
  //
  // elements to add
  vector<double> values_to_test;
  for(int i=0; i<num_values_to_test; i++) {
    int rand_i = rng.nexti(NUM_BUCKETS);
    values_to_test.push_back(rand_i);
  }
  //
  cout << "values_to_test: " << values_to_test << endl;
  vector<double> values_to_test_reversed = values_to_test;
  std::reverse(values_to_test_reversed.begin(), values_to_test_reversed.end());
  vector<double> values_to_test_shuffled = values_to_test;
  std::random_shuffle(values_to_test_shuffled.begin(), values_to_test_shuffled.end());

  // print generated values
  //
  cout << endl << "initial parameters: " << "\t";
  cout << "dirichlet_alphas_to_test: " << dirichlet_alphas_to_test << endl;
  cout << "values_to_test: " << values_to_test << endl;

  hypers["dirichlet_alpha"] = dirichlet_alphas_to_test[1];
  hypers["K"] = NUM_BUCKETS;
  MCM mcm(hypers);

  cout << "calc_marginal_logp() on empty MultinomialComponentModel: ";
  cout << mcm.calc_marginal_logp() << endl;
  assert(is_almost(mcm.calc_marginal_logp(), 0, precision));

  //
  cout << "test insertion and removal in same order" << endl;
  insert_elements(mcm, values_to_test);
  cout << mcm << endl;
  assert(is_almost(mcm.calc_marginal_logp(), -49.9364531937, precision));
  assert(is_almost(mcm.calc_element_predictive_logp(0), log(3.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(1), log(7.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(2), log(10.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(3), log(6.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(4), log(9.0/35), precision));
  remove_elements(mcm, values_to_test);
  cout << mcm << endl;
  //
  cout << "test insertion and removal in reversed order" << endl;
  insert_elements(mcm, values_to_test);
  cout << mcm << endl;
  assert(is_almost(mcm.calc_marginal_logp(), -49.9364531937, precision));
  assert(is_almost(mcm.calc_element_predictive_logp(0), log(3.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(1), log(7.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(2), log(10.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(3), log(6.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(4), log(9.0/35), precision));
  remove_elements(mcm, values_to_test_reversed);
  cout << mcm << endl;
  //
  cout << "test insertion and removal in shuffled order" << endl;
  insert_elements(mcm, values_to_test);
  cout << mcm << endl;
  assert(is_almost(mcm.calc_marginal_logp(), -49.9364531937, precision));
  assert(is_almost(mcm.calc_element_predictive_logp(0), log(3.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(1), log(7.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(2), log(10.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(3), log(6.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(4), log(9.0/35), precision));
  remove_elements(mcm, values_to_test_shuffled);
  cout << mcm << endl;

  cout << "test draws" << endl;
  cout << "inserting: " << values_to_test << endl;
  insert_elements(mcm, values_to_test);
  cout << mcm << endl;
  vector<double> draws;
  map<double, int> draw_counts;
  int num_draws = 10000;
  for(int i=0; i<num_draws; i++) {
    int rand_int = rng.nexti();
    double draw = mcm.get_draw(rand_int);
    draws.push_back(draw);
    if(in(draw_counts, draw)) {
      draw_counts[draw]++;
    } else {
      draw_counts[draw] = 1;
    }
  }
  // cout << "draws are: " << draws << endl;
  cout << "draw_counts is: " << draw_counts << endl;

  cout << endl << "End:: test_multinomial_component_model" << endl;
}
<commit_msg>add test for sparse constructor input<commit_after>/*
*   Copyright (c) 2010-2013, MIT Probabilistic Computing Project
*
*   Lead Developers: Dan Lovell and Jay Baxter
*   Authors: Dan Lovell, Baxter Eaves, Jay Baxter, Vikash Mansinghka
*   Research Leads: Vikash Mansinghka, Patrick Shafto
*
*   Licensed under the Apache License, Version 2.0 (the "License");
*   you may not use this file except in compliance with the License.
*   You may obtain a copy of the License at
*
*       http://www.apache.org/licenses/LICENSE-2.0
*
*   Unless required by applicable law or agreed to in writing, software
*   distributed under the License is distributed on an "AS IS" BASIS,
*   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*   See the License for the specific language governing permissions and
*   limitations under the License.
*/
#include <iostream>
#include <map>
#include <vector>
#include <string>
#include "MultinomialComponentModel.h"
#include "RandomNumberGenerator.h"
#include "utils.h"

using namespace std;

typedef MultinomialComponentModel MCM;

void insert_elements(MCM &mcm, vector<double> elements) {
  vector<double>::iterator it;
  for(it=elements.begin(); it!=elements.end(); it++)
    mcm.insert_element(*it);
}

void remove_elements(MCM &mcm, vector<double> elements) {
  vector<double>::iterator it;
  for(it=elements.begin(); it!=elements.end(); it++)
    mcm.remove_element(*it);
}

int main() {
  cout << endl << "Begin:: test_multinomial_component_model" << endl;
  RandomNumberGenerator rng;

  // test settings
  int NUM_BUCKETS = 5;
  int num_values_to_test = 30;
  double precision = 1E-10;
  map<string, double> hypers;

  // generate all the random data to use
  //
  // initial parameters
  vector<double> dirichlet_alphas_to_test;
  dirichlet_alphas_to_test.push_back(0.5);
  dirichlet_alphas_to_test.push_back(1.0);
  dirichlet_alphas_to_test.push_back(10.0);
  //
  // elements to add
  vector<double> values_to_test;
  for(int i=0; i<num_values_to_test; i++) {
    int rand_i = rng.nexti(NUM_BUCKETS);
    values_to_test.push_back(rand_i);
  }
  //
  cout << "values_to_test: " << values_to_test << endl;
  vector<double> values_to_test_reversed = values_to_test;
  std::reverse(values_to_test_reversed.begin(), values_to_test_reversed.end());
  vector<double> values_to_test_shuffled = values_to_test;
  std::random_shuffle(values_to_test_shuffled.begin(), values_to_test_shuffled.end());

  // print generated values
  //
  cout << endl << "initial parameters: " << "\t";
  cout << "dirichlet_alphas_to_test: " << dirichlet_alphas_to_test << endl;
  cout << "values_to_test: " << values_to_test << endl;

  hypers["dirichlet_alpha"] = dirichlet_alphas_to_test[1];
  hypers["K"] = NUM_BUCKETS;
  MCM mcm(hypers);

  cout << "calc_marginal_logp() on empty MultinomialComponentModel: ";
  cout << mcm.calc_marginal_logp() << endl;
  assert(is_almost(mcm.calc_marginal_logp(), 0, precision));

  //
  cout << "test insertion and removal in same order" << endl;
  insert_elements(mcm, values_to_test);
  cout << mcm << endl;
  assert(is_almost(mcm.calc_marginal_logp(), -49.9364531937, precision));
  assert(is_almost(mcm.calc_element_predictive_logp(0), log(3.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(1), log(7.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(2), log(10.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(3), log(6.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(4), log(9.0/35), precision));
  remove_elements(mcm, values_to_test);
  cout << mcm << endl;
  //
  cout << "test insertion and removal in reversed order" << endl;
  insert_elements(mcm, values_to_test);
  cout << mcm << endl;
  assert(is_almost(mcm.calc_marginal_logp(), -49.9364531937, precision));
  assert(is_almost(mcm.calc_element_predictive_logp(0), log(3.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(1), log(7.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(2), log(10.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(3), log(6.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(4), log(9.0/35), precision));
  remove_elements(mcm, values_to_test_reversed);
  cout << mcm << endl;
  //
  cout << "test insertion and removal in shuffled order" << endl;
  insert_elements(mcm, values_to_test);
  cout << mcm << endl;
  assert(is_almost(mcm.calc_marginal_logp(), -49.9364531937, precision));
  assert(is_almost(mcm.calc_element_predictive_logp(0), log(3.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(1), log(7.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(2), log(10.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(3), log(6.0/35), precision));
  assert(is_almost(mcm.calc_element_predictive_logp(4), log(9.0/35), precision));
  remove_elements(mcm, values_to_test_shuffled);
  cout << mcm << endl;

  cout << "test draws" << endl;
  cout << "inserting: " << values_to_test << endl;
  insert_elements(mcm, values_to_test);
  cout << mcm << endl;
  vector<double> draws;
  map<double, int> draw_counts;
  int num_draws = 10000;
  for(int i=0; i<num_draws; i++) {
    int rand_int = rng.nexti();
    double draw = mcm.get_draw(rand_int);
    draws.push_back(draw);
    if(in(draw_counts, draw)) {
      draw_counts[draw]++;
    } else {
      draw_counts[draw] = 1;
    }
  }
  // cout << "draws are: " << draws << endl;
  cout << "draw_counts is: " << draw_counts << endl;

  cout << endl << endl << "test constructor with sparse input" << endl;
  // elements to add
  values_to_test.clear();
  map<string, double> counts_to_use;
  for(int i=0; i<NUM_BUCKETS; i++) {
	  counts_to_use[stringify(i)] = 0.;
  }
  int ignore_value = 0;
  for(int i=0; i<num_values_to_test; i++) {
    int rand_i = rng.nexti(NUM_BUCKETS);
    if(rand_i==ignore_value) {
	    continue;
    }
    values_to_test.push_back(rand_i);
    counts_to_use[stringify(rand_i)]++;
  }
  counts_to_use.erase(stringify(ignore_value));
  //
  cout << "values_to_test: " << values_to_test << endl;
  cout << "counts_to_use: " << counts_to_use << endl;

  // print generated values
  //
  cout << endl << "initial parameters: " << endl;
  cout << "values_to_test: " << values_to_test << endl;

  hypers["dirichlet_alpha"] = 1.;
  hypers["K"] = NUM_BUCKETS;
  MCM mcm2(hypers, values_to_test.size(), counts_to_use);

  cout << "component model: " << mcm2 << endl;

  draws.clear();
  draw_counts.clear();
  num_draws = 10000;
  for(int i=0; i<num_draws; i++) {
    int rand_int = rng.nexti();
    double draw = mcm2.get_draw(rand_int);
    draws.push_back(draw);
    if(in(draw_counts, draw)) {
      draw_counts[draw]++;
    } else {
      draw_counts[draw] = 1;
    }
  }
  // cout << "draws are: " << draws << endl;
  cout << "draw_counts is: " << draw_counts << endl;

  double sum_p = 0;
  for(int element=0; element<NUM_BUCKETS; element++) {
	  double element_p = exp(mcm2.calc_element_predictive_logp(element));
  }

  cout << endl << "End:: test_multinomial_component_model" << endl;
}
<|endoftext|>


<commit_before>
#include "odrive_main.h"

Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
                Config_t& config) :
        hw_config_(hw_config),
        config_(config)
{
    // Calculate encoder pll gains
    // This calculation is currently identical to the PLL in SensorlessEstimator
    float pll_bandwidth = 100.0f;  // [rad/s]
    pll_kp_ = 2.0f * pll_bandwidth;

    // Critically damped
    pll_ki_ = 0.25f * (pll_kp_ * pll_kp_);

    if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL)) {
        offset_ = config.offset;
        is_ready_ = true;
    }
}

static void enc_index_cb_wrapper(void* ctx) {
    reinterpret_cast<Encoder*>(ctx)->enc_index_cb();
}

void Encoder::setup() {
    HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
    GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_NOPULL,
            enc_index_cb_wrapper, this);
}

void Encoder::set_error(Encoder::Error_t error) {
    error_ |= error;
    axis_->error_ |= Axis::ERROR_MOTOR_FAILED;
}

bool Encoder::do_checks(){
    return error_ == ERROR_NONE;
}

//--------------------
// Hardware Dependent
//--------------------

// Triggered when an encoder passes over the "Index" pin
// TODO: only arm index edge interrupt when we know encoder has powered up
// TODO: disable interrupt once we found the index
void Encoder::enc_index_cb() {
    if (config_.use_index && !index_found_) {
        set_circular_count(0);
        if (config_.pre_calibrated) {
            offset_ = config_.offset;
            is_ready_ = true;
        }
        index_found_ = true;
    }
}

// Function that sets the current encoder count to a desired 32-bit value.
void Encoder::set_linear_count(int32_t count) {
    // Disable interrupts to make a critical section to avoid race condition
    uint32_t prim = __get_PRIMASK();
    __disable_irq();

    // Update states
    shadow_count_ = count;
    pos_estimate_ = (float)count;
    //Write hardware last
    hw_config_.timer->Instance->CNT = count;

    __set_PRIMASK(prim);
}

// Function that sets the CPR circular tracking encoder count to a desired 32-bit value.
// Note that this will get mod'ed down to [0, cpr)
void Encoder::set_circular_count(int32_t count) {
    // Disable interrupts to make a critical section to avoid race condition
    uint32_t prim = __get_PRIMASK();
    __disable_irq();

    // Offset and state must be shifted by the same amount
    offset_ += count - count_in_cpr_;
    offset_ = mod(offset_, config_.cpr);
    // Update states
    count_in_cpr_ = mod(count, config_.cpr);
    pos_cpr_ = (float)count_in_cpr_;

    __set_PRIMASK(prim);
}

// @brief Slowly turns the motor in one direction until the
// encoder index is found.
// TODO: Do the scan with current, not voltage!
bool Encoder::run_index_search() {
    float voltage_magnitude;
    if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT)
        voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
    else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL)
        voltage_magnitude = axis_->motor_.config_.calibration_current;
    else
        return false;

    float omega = (float)(axis_->motor_.config_.direction) * config_.idx_search_speed;

    index_found_ = false;
    float phase = 0.0f;
    axis_->run_control_loop([&](){
        phase = wrap_pm_pi(phase + omega * current_meas_period);

        float v_alpha = voltage_magnitude * arm_cos_f32(phase);
        float v_beta = voltage_magnitude * arm_sin_f32(phase);
        if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH);

        // continue until the index is found
        return !index_found_;
    });
    return true;
}

// @brief Turns the motor in one direction for a bit and then in the other
// direction in order to find the offset between the electrical phase 0
// and the encoder state 0.
// TODO: Do the scan with current, not voltage!
bool Encoder::run_offset_calibration() {
    static const float start_lock_duration = 1.0f;
    static const float scan_omega = 4.0f * M_PI;
    static const float scan_distance = 16.0f * M_PI;
    static const int num_steps = scan_distance / scan_omega * current_meas_hz;

    // Temporarily disable index search so it doesn't mess
    // with the offset calibration
    bool old_use_index = config_.use_index;
    config_.use_index = false;

    float voltage_magnitude;
    if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT)
        voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
    else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL)
        voltage_magnitude = axis_->motor_.config_.calibration_current;
    else
        return false;

    // go to motor zero phase for start_lock_duration to get ready to scan
    int i = 0;
    axis_->run_control_loop([&](){
        if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f))
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
        return ++i < start_lock_duration * current_meas_hz;
    });
    if (axis_->error_ != Axis::ERROR_NONE)
        return false;

    int32_t init_enc_val = shadow_count_;
    int64_t encvaluesum = 0;

    // scan forward
    i = 0;
    axis_->run_control_loop([&](){
        float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f);
        float v_alpha = voltage_magnitude * arm_cos_f32(phase);
        float v_beta = voltage_magnitude * arm_sin_f32(phase);
        if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);

        encvaluesum += shadow_count_;

        return ++i < num_steps;
    });
    if (axis_->error_ != Axis::ERROR_NONE)
        return false;

    //TODO avoid recomputing elec_rad_per_enc every time
    float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
    float expected_encoder_delta = scan_distance / elec_rad_per_enc;
    float actual_encoder_delta_abs = fabsf(shadow_count_-init_enc_val);
    if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range)
    {
        set_error(ERROR_CPR_OUT_OF_RANGE);
        return false;
    }
    // check direction
    if (shadow_count_ > init_enc_val + 8) {
        // motor same dir as encoder
        axis_->motor_.config_.direction = 1;
    } else if (shadow_count_ < init_enc_val - 8) {
        // motor opposite dir as encoder
        axis_->motor_.config_.direction = -1;
    } else {
        // Encoder response error
        set_error(ERROR_RESPONSE);
        return false;
    }

    // scan backwards
    i = 0;
    axis_->run_control_loop([&](){
        float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f);
        float v_alpha = voltage_magnitude * arm_cos_f32(phase);
        float v_beta = voltage_magnitude * arm_sin_f32(phase);
        if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);

        encvaluesum += shadow_count_;

        return ++i < num_steps;
    });
    if (axis_->error_ != Axis::ERROR_NONE)
        return false;

    offset_ = encvaluesum / (num_steps * 2);
    config_.offset = offset_;
    int32_t residual = encvaluesum - ((int64_t)offset_ * (int64_t)(num_steps * 2));
    config_.offset_float = (float)residual / (float)(num_steps * 2);
    is_ready_ = true;
    config_.use_index = old_use_index;
    return true;
}

static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) {
    switch (hall_state) {
        case 0b001: *hall_cnt = 0; return true;
        case 0b011: *hall_cnt = 1; return true;
        case 0b010: *hall_cnt = 2; return true;
        case 0b110: *hall_cnt = 3; return true;
        case 0b100: *hall_cnt = 4; return true;
        case 0b101: *hall_cnt = 5; return true;
        default: return false;
    }
}

bool Encoder::update() {
    // Check that we don't get problems with discrete time approximation
    if (!(current_meas_period * pll_kp_ < 1.0f)) {
        set_error(ERROR_UNSTABLE_GAIN);
        return false;
    }

    // update internal encoder state.
    int32_t delta_enc = 0;
    switch (config_.mode) {
        case MODE_INCREMENTAL: {
            //TODO: use count_in_cpr_ instead as shadow_count_ can overflow
            //or use 64 bit
            int16_t delta_enc_16 = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)shadow_count_;
            delta_enc = (int32_t)delta_enc_16; //sign extend
        } break;

        case MODE_HALL: {
            int32_t hall_cnt;
            if (decode_hall(hall_state_, &hall_cnt)) {
                delta_enc = hall_cnt - count_in_cpr_;
                delta_enc = mod(delta_enc, 6);
                if (delta_enc > 3)
                    delta_enc -= 6;
            } else {
                set_error(ERROR_ILLEGAL_HALL_STATE);
                return false;
            }
        } break;

        default: {
           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
           return false;
        } break;
    }

    shadow_count_ += delta_enc;
    count_in_cpr_ += delta_enc;
    count_in_cpr_ = mod(count_in_cpr_, config_.cpr);

    //// run encoder count interpolation
    int32_t corrected_enc = count_in_cpr_ - offset_;
    // reset interpolation if encoder edge comes
    if (delta_enc > 0) {
        interpolation_ = 0.0f;
    } else if (delta_enc < 0) {
        interpolation_ = 1.0f;
    } else {
        // Interpolate (predict) between encoder counts using pll_vel,
        interpolation_ += current_meas_period * pll_vel_;
        // don't allow interpolation indicated position outside of [enc, enc+1)
        if (interpolation_ > 1.0f) interpolation_ = 1.0f;
        if (interpolation_ < 0.0f) interpolation_ = 0.0f;
    }
    float interpolated_enc = corrected_enc + interpolation_;

    //// compute electrical phase
    //TODO avoid recomputing elec_rad_per_enc every time
    float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
    float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
    // ph = fmodf(ph, 2*M_PI);
    phase_ = wrap_pm_pi(ph);

    //// run pll (for now pll is in units of encoder counts)
    // Predict current pos
    pos_estimate_ += current_meas_period * pll_vel_;
    pos_cpr_      += current_meas_period * pll_vel_;
    // discrete phase detector
    float delta_pos     = (float)(shadow_count_ - (int32_t)floorf(pos_estimate_));
    float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr_));
    delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr));
    // pll feedback
    pos_estimate_ += current_meas_period * pll_kp_ * delta_pos;
    pos_cpr_      += current_meas_period * pll_kp_ * delta_pos_cpr;
    pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr));
    pll_vel_      += current_meas_period * pll_ki_ * delta_pos_cpr;
    if (fabsf(pll_vel_) < 0.5f * current_meas_period * pll_ki_)
        pll_vel_ = 0.0f; //align delta-sigma on zero to prevent jitter

    return true;
}
<commit_msg>reset encoder bandwidth back up to 1000 per s<commit_after>
#include "odrive_main.h"

Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
                Config_t& config) :
        hw_config_(hw_config),
        config_(config)
{
    // Calculate encoder pll gains
    // This calculation is currently identical to the PLL in SensorlessEstimator
    float pll_bandwidth = 1000.0f;  // [rad/s]
    pll_kp_ = 2.0f * pll_bandwidth;

    // Critically damped
    pll_ki_ = 0.25f * (pll_kp_ * pll_kp_);

    if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL)) {
        offset_ = config.offset;
        is_ready_ = true;
    }
}

static void enc_index_cb_wrapper(void* ctx) {
    reinterpret_cast<Encoder*>(ctx)->enc_index_cb();
}

void Encoder::setup() {
    HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
    GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_NOPULL,
            enc_index_cb_wrapper, this);
}

void Encoder::set_error(Encoder::Error_t error) {
    error_ |= error;
    axis_->error_ |= Axis::ERROR_MOTOR_FAILED;
}

bool Encoder::do_checks(){
    return error_ == ERROR_NONE;
}

//--------------------
// Hardware Dependent
//--------------------

// Triggered when an encoder passes over the "Index" pin
// TODO: only arm index edge interrupt when we know encoder has powered up
// TODO: disable interrupt once we found the index
void Encoder::enc_index_cb() {
    if (config_.use_index && !index_found_) {
        set_circular_count(0);
        if (config_.pre_calibrated) {
            offset_ = config_.offset;
            is_ready_ = true;
        }
        index_found_ = true;
    }
}

// Function that sets the current encoder count to a desired 32-bit value.
void Encoder::set_linear_count(int32_t count) {
    // Disable interrupts to make a critical section to avoid race condition
    uint32_t prim = __get_PRIMASK();
    __disable_irq();

    // Update states
    shadow_count_ = count;
    pos_estimate_ = (float)count;
    //Write hardware last
    hw_config_.timer->Instance->CNT = count;

    __set_PRIMASK(prim);
}

// Function that sets the CPR circular tracking encoder count to a desired 32-bit value.
// Note that this will get mod'ed down to [0, cpr)
void Encoder::set_circular_count(int32_t count) {
    // Disable interrupts to make a critical section to avoid race condition
    uint32_t prim = __get_PRIMASK();
    __disable_irq();

    // Offset and state must be shifted by the same amount
    offset_ += count - count_in_cpr_;
    offset_ = mod(offset_, config_.cpr);
    // Update states
    count_in_cpr_ = mod(count, config_.cpr);
    pos_cpr_ = (float)count_in_cpr_;

    __set_PRIMASK(prim);
}

// @brief Slowly turns the motor in one direction until the
// encoder index is found.
// TODO: Do the scan with current, not voltage!
bool Encoder::run_index_search() {
    float voltage_magnitude;
    if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT)
        voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
    else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL)
        voltage_magnitude = axis_->motor_.config_.calibration_current;
    else
        return false;

    float omega = (float)(axis_->motor_.config_.direction) * config_.idx_search_speed;

    index_found_ = false;
    float phase = 0.0f;
    axis_->run_control_loop([&](){
        phase = wrap_pm_pi(phase + omega * current_meas_period);

        float v_alpha = voltage_magnitude * arm_cos_f32(phase);
        float v_beta = voltage_magnitude * arm_sin_f32(phase);
        if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH);

        // continue until the index is found
        return !index_found_;
    });
    return true;
}

// @brief Turns the motor in one direction for a bit and then in the other
// direction in order to find the offset between the electrical phase 0
// and the encoder state 0.
// TODO: Do the scan with current, not voltage!
bool Encoder::run_offset_calibration() {
    static const float start_lock_duration = 1.0f;
    static const float scan_omega = 4.0f * M_PI;
    static const float scan_distance = 16.0f * M_PI;
    static const int num_steps = scan_distance / scan_omega * current_meas_hz;

    // Temporarily disable index search so it doesn't mess
    // with the offset calibration
    bool old_use_index = config_.use_index;
    config_.use_index = false;

    float voltage_magnitude;
    if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT)
        voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
    else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL)
        voltage_magnitude = axis_->motor_.config_.calibration_current;
    else
        return false;

    // go to motor zero phase for start_lock_duration to get ready to scan
    int i = 0;
    axis_->run_control_loop([&](){
        if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f))
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
        return ++i < start_lock_duration * current_meas_hz;
    });
    if (axis_->error_ != Axis::ERROR_NONE)
        return false;

    int32_t init_enc_val = shadow_count_;
    int64_t encvaluesum = 0;

    // scan forward
    i = 0;
    axis_->run_control_loop([&](){
        float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f);
        float v_alpha = voltage_magnitude * arm_cos_f32(phase);
        float v_beta = voltage_magnitude * arm_sin_f32(phase);
        if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);

        encvaluesum += shadow_count_;

        return ++i < num_steps;
    });
    if (axis_->error_ != Axis::ERROR_NONE)
        return false;

    //TODO avoid recomputing elec_rad_per_enc every time
    float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
    float expected_encoder_delta = scan_distance / elec_rad_per_enc;
    float actual_encoder_delta_abs = fabsf(shadow_count_-init_enc_val);
    if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range)
    {
        set_error(ERROR_CPR_OUT_OF_RANGE);
        return false;
    }
    // check direction
    if (shadow_count_ > init_enc_val + 8) {
        // motor same dir as encoder
        axis_->motor_.config_.direction = 1;
    } else if (shadow_count_ < init_enc_val - 8) {
        // motor opposite dir as encoder
        axis_->motor_.config_.direction = -1;
    } else {
        // Encoder response error
        set_error(ERROR_RESPONSE);
        return false;
    }

    // scan backwards
    i = 0;
    axis_->run_control_loop([&](){
        float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f);
        float v_alpha = voltage_magnitude * arm_cos_f32(phase);
        float v_beta = voltage_magnitude * arm_sin_f32(phase);
        if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);

        encvaluesum += shadow_count_;

        return ++i < num_steps;
    });
    if (axis_->error_ != Axis::ERROR_NONE)
        return false;

    offset_ = encvaluesum / (num_steps * 2);
    config_.offset = offset_;
    int32_t residual = encvaluesum - ((int64_t)offset_ * (int64_t)(num_steps * 2));
    config_.offset_float = (float)residual / (float)(num_steps * 2);
    is_ready_ = true;
    config_.use_index = old_use_index;
    return true;
}

static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) {
    switch (hall_state) {
        case 0b001: *hall_cnt = 0; return true;
        case 0b011: *hall_cnt = 1; return true;
        case 0b010: *hall_cnt = 2; return true;
        case 0b110: *hall_cnt = 3; return true;
        case 0b100: *hall_cnt = 4; return true;
        case 0b101: *hall_cnt = 5; return true;
        default: return false;
    }
}

bool Encoder::update() {
    // Check that we don't get problems with discrete time approximation
    if (!(current_meas_period * pll_kp_ < 1.0f)) {
        set_error(ERROR_UNSTABLE_GAIN);
        return false;
    }

    // update internal encoder state.
    int32_t delta_enc = 0;
    switch (config_.mode) {
        case MODE_INCREMENTAL: {
            //TODO: use count_in_cpr_ instead as shadow_count_ can overflow
            //or use 64 bit
            int16_t delta_enc_16 = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)shadow_count_;
            delta_enc = (int32_t)delta_enc_16; //sign extend
        } break;

        case MODE_HALL: {
            int32_t hall_cnt;
            if (decode_hall(hall_state_, &hall_cnt)) {
                delta_enc = hall_cnt - count_in_cpr_;
                delta_enc = mod(delta_enc, 6);
                if (delta_enc > 3)
                    delta_enc -= 6;
            } else {
                set_error(ERROR_ILLEGAL_HALL_STATE);
                return false;
            }
        } break;

        default: {
           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
           return false;
        } break;
    }

    shadow_count_ += delta_enc;
    count_in_cpr_ += delta_enc;
    count_in_cpr_ = mod(count_in_cpr_, config_.cpr);

    //// run encoder count interpolation
    int32_t corrected_enc = count_in_cpr_ - offset_;
    // reset interpolation if encoder edge comes
    if (delta_enc > 0) {
        interpolation_ = 0.0f;
    } else if (delta_enc < 0) {
        interpolation_ = 1.0f;
    } else {
        // Interpolate (predict) between encoder counts using pll_vel,
        interpolation_ += current_meas_period * pll_vel_;
        // don't allow interpolation indicated position outside of [enc, enc+1)
        if (interpolation_ > 1.0f) interpolation_ = 1.0f;
        if (interpolation_ < 0.0f) interpolation_ = 0.0f;
    }
    float interpolated_enc = corrected_enc + interpolation_;

    //// compute electrical phase
    //TODO avoid recomputing elec_rad_per_enc every time
    float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
    float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
    // ph = fmodf(ph, 2*M_PI);
    phase_ = wrap_pm_pi(ph);

    //// run pll (for now pll is in units of encoder counts)
    // Predict current pos
    pos_estimate_ += current_meas_period * pll_vel_;
    pos_cpr_      += current_meas_period * pll_vel_;
    // discrete phase detector
    float delta_pos     = (float)(shadow_count_ - (int32_t)floorf(pos_estimate_));
    float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr_));
    delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr));
    // pll feedback
    pos_estimate_ += current_meas_period * pll_kp_ * delta_pos;
    pos_cpr_      += current_meas_period * pll_kp_ * delta_pos_cpr;
    pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr));
    pll_vel_      += current_meas_period * pll_ki_ * delta_pos_cpr;
    if (fabsf(pll_vel_) < 0.5f * current_meas_period * pll_ki_)
        pll_vel_ = 0.0f; //align delta-sigma on zero to prevent jitter

    return true;
}
<|endoftext|>


<commit_before>/*
 * RoadStore.cpp
 *
 *  Created on: 10 Aug 2015
 *      Author: makrai
 */

#include "RoadStore.h"

#ifdef _DEBUG_
#include <iostream>
using namespace std;
#endif

RoadStore::RoadStore(int initialStoreSize) {
	edgeId = new int[initialStoreSize];
	startNode = new int[initialStoreSize];
	endNode = new int[initialStoreSize];
	length = new double[initialStoreSize];
	speedLimit = new double[initialStoreSize];
	size = 0;
	storeSize = initialStoreSize;
}

void RoadStore::dispose() {
	delete [] edgeId;
	delete [] startNode;
	delete [] endNode;
	delete [] length;
	delete [] speedLimit;
}

void RoadStore::addRoad(int edgeId, int startNode, int endNode, double length, double speedLimit) {
	if (size == storeSize) {
		int newSize = size * 2;
		int* newEdgeId = new int[newSize];
		int* newStartNode = new int[newSize];
		int* newEndNode = new int[newSize];
		double* newLength = new double[newSize];
		double* newSpeedLimit = new double[newSize];

		for (int i = 0; i < size; ++i) {
			newEdgeId[i] = this->edgeId[i];
			newStartNode[i] = this->startNode[i];
			newEndNode[i] = this->endNode[i];
			newLength[i] = this->length[i];
			newSpeedLimit[i] = this->speedLimit[i];
		}

		delete [] this->edgeId;
		delete [] this->startNode;
		delete [] this->endNode;
		delete [] this->length;
		delete [] this->speedLimit;

		this->edgeId = newEdgeId;
		this->startNode = newStartNode;
		this->endNode = newEndNode;
		this->length = newLength;
		this->speedLimit = newSpeedLimit;

		storeSize = newSize;
	}

	this->edgeId[size] = edgeId;
	this->startNode[size] = startNode;
	this->endNode[size] = endNode;
	this->length[size] = length;
	this->speedLimit[size] = speedLimit;

	++size;
}

void RoadStore::reassignNodeIds(NodeStore* nodeStore) {
	for (int i = 0; i < this->size; ++i) {

#ifdef _DEBUG_
		cout << "RoadStore: reassignNodeIds: Road(index:" << i << ",id" << this->edgeId[i] << ",sNode" << this->startNode[i] << ",eNode:" << this->endNode[i] << ") has new ids: ";
#endif

		int startNodeIndex = nodeStore->getIndex(this->startNode[i]);

#ifdef _DEBUG_
		cout << "sNode(" << startNodeIndex << "),";
#endif

		this->startNode[i] = startNodeIndex;

		int endNodeIndex = nodeStore->getIndex(this->endNode[i]);

#ifdef _DEBUG_
		cout << "eNode(" << endNodeIndex << ")" << endl;
#endif

		this->endNode[i] = endNodeIndex;
#ifdef _DEBUG_
		if (startNodeIndex == -1) {
			cout << "problem...";
		}
		if (endNodeIndex == -1) {
			cout << "poroblem...";
		}
#endif
	}
}

<commit_msg>fixing spelling error<commit_after>/*
 * RoadStore.cpp
 *
 *  Created on: 10 Aug 2015
 *      Author: makrai
 */

#include "RoadStore.h"

#ifdef _DEBUG_
#include <iostream>
using namespace std;
#endif

RoadStore::RoadStore(int initialStoreSize) {
	edgeId = new int[initialStoreSize];
	startNode = new int[initialStoreSize];
	endNode = new int[initialStoreSize];
	length = new double[initialStoreSize];
	speedLimit = new double[initialStoreSize];
	size = 0;
	storeSize = initialStoreSize;
}

void RoadStore::dispose() {
	delete [] edgeId;
	delete [] startNode;
	delete [] endNode;
	delete [] length;
	delete [] speedLimit;
}

void RoadStore::addRoad(int edgeId, int startNode, int endNode, double length, double speedLimit) {
	if (size == storeSize) {
		int newSize = size * 2;
		int* newEdgeId = new int[newSize];
		int* newStartNode = new int[newSize];
		int* newEndNode = new int[newSize];
		double* newLength = new double[newSize];
		double* newSpeedLimit = new double[newSize];

		for (int i = 0; i < size; ++i) {
			newEdgeId[i] = this->edgeId[i];
			newStartNode[i] = this->startNode[i];
			newEndNode[i] = this->endNode[i];
			newLength[i] = this->length[i];
			newSpeedLimit[i] = this->speedLimit[i];
		}

		delete [] this->edgeId;
		delete [] this->startNode;
		delete [] this->endNode;
		delete [] this->length;
		delete [] this->speedLimit;

		this->edgeId = newEdgeId;
		this->startNode = newStartNode;
		this->endNode = newEndNode;
		this->length = newLength;
		this->speedLimit = newSpeedLimit;

		storeSize = newSize;
	}

	this->edgeId[size] = edgeId;
	this->startNode[size] = startNode;
	this->endNode[size] = endNode;
	this->length[size] = length;
	this->speedLimit[size] = speedLimit;

	++size;
}

void RoadStore::reassignNodeIds(NodeStore* nodeStore) {
	for (int i = 0; i < this->size; ++i) {

#ifdef _DEBUG_
		cout << "RoadStore: reassignNodeIds: Road(index:" << i << ",id" << this->edgeId[i] << ",sNode" << this->startNode[i] << ",eNode:" << this->endNode[i] << ") has new ids: ";
#endif

		int startNodeIndex = nodeStore->getIndex(this->startNode[i]);

#ifdef _DEBUG_
		cout << "sNode(" << startNodeIndex << "),";
#endif

		this->startNode[i] = startNodeIndex;

		int endNodeIndex = nodeStore->getIndex(this->endNode[i]);

#ifdef _DEBUG_
		cout << "eNode(" << endNodeIndex << ")" << endl;
#endif

		this->endNode[i] = endNodeIndex;
#ifdef _DEBUG_
		if (startNodeIndex == -1) {
			cout << "problem...";
		}
		if (endNodeIndex == -1) {
			cout << "problem...";
		}
#endif
	}
}

<|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 qp_spline_st_graph.cc
 **/

#include "modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h"

#include <algorithm>
#include <string>
#include <utility>

#include "modules/common/log.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::VehicleParam;

QpSplineStGraph::QpSplineStGraph(
    const QpSplineStSpeedConfig& qp_spline_st_speed_config,
    const VehicleParam& veh_param)
    : qp_spline_st_speed_config_(qp_spline_st_speed_config),
      t_knots_resolution_(
          qp_spline_st_speed_config_.total_time() /
          qp_spline_st_speed_config_.number_of_discrete_graph_t()),
      t_evaluated_resolution_(
          qp_spline_st_speed_config_.total_time() /
          qp_spline_st_speed_config_.number_of_evaluated_graph_t())

{}

void QpSplineStGraph::Init() {
  // init knots
  double curr_t = 0.0;
  for (uint32_t i = 0;
       i <= qp_spline_st_speed_config_.number_of_discrete_graph_t(); ++i) {
    t_knots_.push_back(curr_t);
    curr_t += t_knots_resolution_;
  }

  // init evaluated t positions
  curr_t = 0.0;
  for (uint32_t i = 0;
       i <= qp_spline_st_speed_config_.number_of_evaluated_graph_t(); ++i) {
    t_evaluated_.push_back(curr_t);
    curr_t += t_evaluated_resolution_;
  }

  // init spline generator
  spline_generator_.reset(new Spline1dGenerator(
      t_knots_, qp_spline_st_speed_config_.spline_order()));
}

Status QpSplineStGraph::Search(const StGraphData& st_graph_data,
                               const PathData& path_data,
                               SpeedData* const speed_data) {
  init_point_ = st_graph_data.init_point();
  if (st_graph_data.path_data_length() <
      qp_spline_st_speed_config_.total_path_length()) {
    qp_spline_st_speed_config_.set_total_path_length(
        st_graph_data.path_data_length());
  }

  // TODO(all): update config through veh physical limit here generate knots

  // initialize time resolution and
  Init();

  if (!ApplyConstraint(st_graph_data.init_point(), st_graph_data.speed_limit(),
                       st_graph_data.st_graph_boundaries())
           .ok()) {
    const std::string msg = "Apply constraint failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  if (!ApplyKernel(st_graph_data.st_graph_boundaries(),
                   st_graph_data.speed_limit())
           .ok()) {
    const std::string msg = "Apply kernel failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  if (!Solve().ok()) {
    const std::string msg = "Solve qp problem failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  // extract output
  speed_data->Clear();
  const Spline1d& spline = spline_generator_->spline();

  double t_output_resolution =
      qp_spline_st_speed_config_.output_time_resolution();
  double time = 0.0;
  while (time < qp_spline_st_speed_config_.total_time() + t_output_resolution) {
    double s = spline(time);
    double v = spline.derivative(time);
    double a = spline.second_order_derivative(time);
    double da = spline.third_order_derivative(time);
    speed_data->add_speed_point(s, time, v, a, da);
    time += t_output_resolution;
  }

  return Status::OK();
}

Status QpSplineStGraph::ApplyConstraint(
    const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
    const std::vector<StGraphBoundary>& boundaries) {
  Spline1dConstraint* constraint =
      spline_generator_->mutable_spline_constraint();
  // position, velocity, acceleration

  if (!constraint->add_point_fx_constraint(0.0, 0.0)) {
    const std::string msg = "add st start point constraint failed";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  if (!constraint->add_point_derivative_constraint(0.0, init_point_.v())) {
    const std::string msg = "add st start point velocity constraint failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  if (!constraint->add_point_second_derivative_constraint(0.0,
                                                          init_point_.a())) {
    const std::string msg =
        "add st start point acceleration constraint failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  if (!constraint->add_point_second_derivative_constraint(
          spline_generator_->spline().x_knots().back(), 0.0)) {
    const std::string msg = "add st end point acceleration constraint failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  // monotone constraint
  if (!constraint->add_monotone_fx_inequality_constraint_at_knots()) {
    const std::string msg = "add monotonicity constraint failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  // smoothness constraint
  if (!constraint->add_third_derivative_smooth_constraint()) {
    const std::string msg = "add smoothness joint constraint failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  // boundary constraint
  std::vector<double> s_upper_bound;
  std::vector<double> s_lower_bound;

  for (const double curr_t : t_evaluated_) {
    double lower_s = 0.0;
    double upper_s = 0.0;
    GetSConstraintByTime(boundaries, curr_t,
                         qp_spline_st_speed_config_.total_path_length(),
                         &upper_s, &lower_s);
    s_upper_bound.push_back(upper_s);
    s_lower_bound.push_back(lower_s);
  }
  if (!constraint->add_fx_boundary(t_evaluated_, s_lower_bound,
                                   s_upper_bound)) {
    const std::string msg = "Fail to apply distance constraints.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  std::vector<double> speed_upper_bound;
  if (!EstimateSpeedUpperBound(init_point, speed_limit, &speed_upper_bound)
           .ok()) {
    std::string msg = "Fail to estimate speed upper constraints.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  std::vector<double> speed_lower_bound(t_evaluated_.size(), 0.0);
  if (!constraint->add_derivative_boundary(t_evaluated_, speed_lower_bound,
                                   speed_upper_bound)) {
    const std::string msg = "Fail to apply speed constraints.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  return Status::OK();
}

Status QpSplineStGraph::ApplyKernel(
    const std::vector<StGraphBoundary>& boundaries,
    const SpeedLimit& speed_limit) {
  Spline1dKernel* spline_kernel = spline_generator_->mutable_spline_kernel();

  if (qp_spline_st_speed_config_.speed_kernel_weight() > 0) {
    spline_kernel->add_derivative_kernel_matrix(
        qp_spline_st_speed_config_.speed_kernel_weight());
  }

  if (qp_spline_st_speed_config_.accel_kernel_weight() > 0) {
    spline_kernel->add_second_order_derivative_matrix(
        qp_spline_st_speed_config_.accel_kernel_weight());
  }

  if (qp_spline_st_speed_config_.jerk_kernel_weight() > 0) {
    spline_kernel->add_third_order_derivative_matrix(
        qp_spline_st_speed_config_.jerk_kernel_weight());
  }

  if (AddCruiseReferenceLineKernel(t_evaluated_, speed_limit) != Status::OK()) {
    return Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::ApplyKernel");
  }

  if (AddFollowReferenceLineKernel(t_evaluated_, boundaries, 1.0) != Status::OK()) {
    return Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::ApplyKernel");
  }
  return Status::OK();
}

Status QpSplineStGraph::Solve() {
  return spline_generator_->solve()
             ? Status::OK()
             : Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::solve");
}

Status QpSplineStGraph::AddCruiseReferenceLineKernel(
      const std::vector<double>& evaluate_t,
      const SpeedLimit& speed_limit) {
  auto* spline_kernel = spline_generator_->mutable_spline_kernel();
  std::vector<double> s_vec;
  if (speed_limit.speed_points().size() == 0) {
    std::string msg = "Fail to apply_kernel due to empty speed limits.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  double dist_ref = 0.0;
  for (uint32_t i = 1; i < evaluate_t.size(); ++i) {
    s_vec.push_back(dist_ref);
    dist_ref += (evaluate_t[i] - evaluate_t[i - 1]) *
      speed_limit.get_speed_limit_by_s(dist_ref);
  }
  spline_kernel->add_reference_line_kernel_matrix(
    t_knots_, s_vec,
    qp_spline_st_speed_config_.reference_line_kernel_weight());

  return Status::OK();
}

Status QpSplineStGraph::AddFollowReferenceLineKernel(
    const std::vector<double>& evaluate_t,
    const std::vector<StGraphBoundary>& boundaries,
    const double weight) {
  auto* spline_kernel = spline_generator_->mutable_spline_kernel();
  std::vector<double> ref_s;
  std::vector<double> filtered_evaluate_t;
  for (const double curr_t : evaluate_t) {
    double s_upper = 0.0;
    double s_lower = 0.0;
    double s_ref_min = std::numeric_limits<double>::infinity();
    bool success = false;
    for (const auto& boundary : boundaries) {
      if (boundary.boundary_type() == StGraphBoundary::BoundaryType::FOLLOW &&
        boundary.GetUnblockSRange(curr_t, &s_upper, &s_lower)) {
        success = true;
        s_ref_min = std::min(s_ref_min,
          s_upper - boundary.characteristic_length());
      }
    }
    if (success) {
      filtered_evaluate_t.push_back(curr_t);
      ref_s.push_back(s_ref_min);
    }
  }
  spline_kernel->add_reference_line_kernel_matrix(
    filtered_evaluate_t, ref_s, weight);
  return Status::OK();
}

Status QpSplineStGraph::GetSConstraintByTime(
    const std::vector<StGraphBoundary>& boundaries, const double time,
    const double total_path_s, double* const s_upper_bound,
    double* const s_lower_bound) const {
  *s_upper_bound =
      std::min(total_path_s, time * qp_spline_st_speed_config_.max_speed());

  for (const StGraphBoundary& boundary : boundaries) {
    double s_upper = 0.0;
    double s_lower = 0.0;

    if (!boundary.GetUnblockSRange(time, &s_upper, &s_lower)) {
      continue;
    }

    if (boundary.boundary_type() == StGraphBoundary::BoundaryType::STOP ||
        boundary.boundary_type() == StGraphBoundary::BoundaryType::FOLLOW ||
        boundary.boundary_type() == StGraphBoundary::BoundaryType::YIELD) {
      *s_upper_bound = std::fmin(*s_upper_bound, s_upper);
    } else {
      *s_lower_bound = std::fmax(*s_lower_bound, s_lower);
    }
  }

  return Status::OK();
}

Status QpSplineStGraph::EstimateSpeedUpperBound(
    const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
    std::vector<double>* speed_upper_bound) const {
  DCHECK_NOTNULL(speed_upper_bound);
  speed_upper_bound->clear();

  // use v to estimate position: not accurate, but feasible in cyclic
  // processing. We can do the following process multiple times and use
  // previous cycle's results for better estimation.
  const double v = init_point.v();

  uint32_t i = 0;
  uint32_t j = 0;
  double distance = 0.0;
  const double kDistanceEpsilon = 1e-6;
  while (i < t_evaluated_.size() && j + 1 < speed_limit.speed_points().size()) {
    distance = v * t_evaluated_[i];
    if (fabs(distance - speed_limit.speed_points()[j].s()) < kDistanceEpsilon) {
      speed_upper_bound->push_back(speed_limit.speed_points()[j].v());
      ++i;
    } else if (speed_limit.speed_points()[j + 1].s() < distance) {
      ++j;
    } else {
      speed_upper_bound->push_back(speed_limit.get_speed_limit_by_s(distance));
      ++i;
    }
  }

  for (uint32_t k = speed_upper_bound->size() - 1; k < t_evaluated_.size();
       ++k) {
    speed_upper_bound->push_back(qp_spline_st_speed_config_.max_speed());
  }
  return Status::OK();
}

}  // namespace planning
}  // namespace apollo
<commit_msg>Planning: quick bug fix<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 qp_spline_st_graph.cc
 **/

#include "modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h"

#include <algorithm>
#include <string>
#include <utility>

#include "modules/common/log.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::VehicleParam;

QpSplineStGraph::QpSplineStGraph(
    const QpSplineStSpeedConfig& qp_spline_st_speed_config,
    const VehicleParam& veh_param)
    : qp_spline_st_speed_config_(qp_spline_st_speed_config),
      t_knots_resolution_(
          qp_spline_st_speed_config_.total_time() /
          qp_spline_st_speed_config_.number_of_discrete_graph_t()),
      t_evaluated_resolution_(
          qp_spline_st_speed_config_.total_time() /
          qp_spline_st_speed_config_.number_of_evaluated_graph_t())

{}

void QpSplineStGraph::Init() {
  // init knots
  double curr_t = 0.0;
  for (uint32_t i = 0;
       i <= qp_spline_st_speed_config_.number_of_discrete_graph_t(); ++i) {
    t_knots_.push_back(curr_t);
    curr_t += t_knots_resolution_;
  }

  // init evaluated t positions
  curr_t = 0.0;
  for (uint32_t i = 0;
       i <= qp_spline_st_speed_config_.number_of_evaluated_graph_t(); ++i) {
    t_evaluated_.push_back(curr_t);
    curr_t += t_evaluated_resolution_;
  }

  // init spline generator
  spline_generator_.reset(new Spline1dGenerator(
      t_knots_, qp_spline_st_speed_config_.spline_order()));
}

Status QpSplineStGraph::Search(const StGraphData& st_graph_data,
                               const PathData& path_data,
                               SpeedData* const speed_data) {
  init_point_ = st_graph_data.init_point();
  if (st_graph_data.path_data_length() <
      qp_spline_st_speed_config_.total_path_length()) {
    qp_spline_st_speed_config_.set_total_path_length(
        st_graph_data.path_data_length());
  }

  // TODO(all): update config through veh physical limit here generate knots

  // initialize time resolution and
  Init();

  if (!ApplyConstraint(st_graph_data.init_point(), st_graph_data.speed_limit(),
                       st_graph_data.st_graph_boundaries())
           .ok()) {
    const std::string msg = "Apply constraint failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  if (!ApplyKernel(st_graph_data.st_graph_boundaries(),
                   st_graph_data.speed_limit())
           .ok()) {
    const std::string msg = "Apply kernel failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  if (!Solve().ok()) {
    const std::string msg = "Solve qp problem failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  // extract output
  speed_data->Clear();
  const Spline1d& spline = spline_generator_->spline();

  double t_output_resolution =
      qp_spline_st_speed_config_.output_time_resolution();
  double time = 0.0;
  while (time < qp_spline_st_speed_config_.total_time() + t_output_resolution) {
    double s = spline(time);
    double v = spline.derivative(time);
    double a = spline.second_order_derivative(time);
    double da = spline.third_order_derivative(time);
    speed_data->add_speed_point(s, time, v, a, da);
    time += t_output_resolution;
  }

  return Status::OK();
}

Status QpSplineStGraph::ApplyConstraint(
    const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
    const std::vector<StGraphBoundary>& boundaries) {
  Spline1dConstraint* constraint =
      spline_generator_->mutable_spline_constraint();
  // position, velocity, acceleration

  if (!constraint->add_point_fx_constraint(0.0, 0.0)) {
    const std::string msg = "add st start point constraint failed";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  if (!constraint->add_point_derivative_constraint(0.0, init_point_.v())) {
    const std::string msg = "add st start point velocity constraint failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  if (!constraint->add_point_second_derivative_constraint(0.0,
                                                          init_point_.a())) {
    const std::string msg =
        "add st start point acceleration constraint failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  if (!constraint->add_point_second_derivative_constraint(
          spline_generator_->spline().x_knots().back(), 0.0)) {
    const std::string msg = "add st end point acceleration constraint failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  // monotone constraint
  if (!constraint->add_monotone_fx_inequality_constraint_at_knots()) {
    const std::string msg = "add monotonicity constraint failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  // smoothness constraint
  if (!constraint->add_third_derivative_smooth_constraint()) {
    const std::string msg = "add smoothness joint constraint failed!";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  // boundary constraint
  std::vector<double> s_upper_bound;
  std::vector<double> s_lower_bound;

  for (const double curr_t : t_evaluated_) {
    double lower_s = 0.0;
    double upper_s = 0.0;
    GetSConstraintByTime(boundaries, curr_t,
                         qp_spline_st_speed_config_.total_path_length(),
                         &upper_s, &lower_s);
    s_upper_bound.push_back(upper_s);
    s_lower_bound.push_back(lower_s);
  }
  if (!constraint->add_fx_boundary(t_evaluated_, s_lower_bound,
                                   s_upper_bound)) {
    const std::string msg = "Fail to apply distance constraints.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  std::vector<double> speed_upper_bound;
  if (!EstimateSpeedUpperBound(init_point, speed_limit, &speed_upper_bound)
           .ok()) {
    std::string msg = "Fail to estimate speed upper constraints.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  std::vector<double> speed_lower_bound(t_evaluated_.size(), 0.0);
  if (!constraint->add_derivative_boundary(t_evaluated_, speed_lower_bound,
                                           speed_upper_bound)) {
    const std::string msg = "Fail to apply speed constraints.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  return Status::OK();
}

Status QpSplineStGraph::ApplyKernel(
    const std::vector<StGraphBoundary>& boundaries,
    const SpeedLimit& speed_limit) {
  Spline1dKernel* spline_kernel = spline_generator_->mutable_spline_kernel();

  if (qp_spline_st_speed_config_.speed_kernel_weight() > 0) {
    spline_kernel->add_derivative_kernel_matrix(
        qp_spline_st_speed_config_.speed_kernel_weight());
  }

  if (qp_spline_st_speed_config_.accel_kernel_weight() > 0) {
    spline_kernel->add_second_order_derivative_matrix(
        qp_spline_st_speed_config_.accel_kernel_weight());
  }

  if (qp_spline_st_speed_config_.jerk_kernel_weight() > 0) {
    spline_kernel->add_third_order_derivative_matrix(
        qp_spline_st_speed_config_.jerk_kernel_weight());
  }

  if (AddCruiseReferenceLineKernel(t_evaluated_, speed_limit) != Status::OK()) {
    return Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::ApplyKernel");
  }

  if (AddFollowReferenceLineKernel(t_evaluated_, boundaries, 1.0) !=
      Status::OK()) {
    return Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::ApplyKernel");
  }
  return Status::OK();
}

Status QpSplineStGraph::Solve() {
  return spline_generator_->solve()
             ? Status::OK()
             : Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::solve");
}

Status QpSplineStGraph::AddCruiseReferenceLineKernel(
    const std::vector<double>& evaluate_t, const SpeedLimit& speed_limit) {
  auto* spline_kernel = spline_generator_->mutable_spline_kernel();
  std::vector<double> s_vec;
  if (speed_limit.speed_points().size() == 0) {
    std::string msg = "Fail to apply_kernel due to empty speed limits.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  double dist_ref = 0.0;
  for (uint32_t i = 1; i < evaluate_t.size(); ++i) {
    s_vec.push_back(dist_ref);
    dist_ref += (evaluate_t[i] - evaluate_t[i - 1]) *
                speed_limit.get_speed_limit_by_s(dist_ref);
  }
  spline_kernel->add_reference_line_kernel_matrix(
      evaluate_t, s_vec,
      qp_spline_st_speed_config_.reference_line_kernel_weight());

  return Status::OK();
}

Status QpSplineStGraph::AddFollowReferenceLineKernel(
    const std::vector<double>& evaluate_t,
    const std::vector<StGraphBoundary>& boundaries, const double weight) {
  auto* spline_kernel = spline_generator_->mutable_spline_kernel();
  std::vector<double> ref_s;
  std::vector<double> filtered_evaluate_t;
  for (const double curr_t : evaluate_t) {
    double s_upper = 0.0;
    double s_lower = 0.0;
    double s_ref_min = std::numeric_limits<double>::infinity();
    bool success = false;
    for (const auto& boundary : boundaries) {
      if (boundary.boundary_type() == StGraphBoundary::BoundaryType::FOLLOW &&
          boundary.GetUnblockSRange(curr_t, &s_upper, &s_lower)) {
        success = true;
        s_ref_min =
            std::min(s_ref_min, s_upper - boundary.characteristic_length());
      }
    }
    if (success) {
      filtered_evaluate_t.push_back(curr_t);
      ref_s.push_back(s_ref_min);
    }
  }
  spline_kernel->add_reference_line_kernel_matrix(filtered_evaluate_t, ref_s,
                                                  weight);
  return Status::OK();
}

Status QpSplineStGraph::GetSConstraintByTime(
    const std::vector<StGraphBoundary>& boundaries, const double time,
    const double total_path_s, double* const s_upper_bound,
    double* const s_lower_bound) const {
  *s_upper_bound =
      std::min(total_path_s, time * qp_spline_st_speed_config_.max_speed());

  for (const StGraphBoundary& boundary : boundaries) {
    double s_upper = 0.0;
    double s_lower = 0.0;

    if (!boundary.GetUnblockSRange(time, &s_upper, &s_lower)) {
      continue;
    }

    if (boundary.boundary_type() == StGraphBoundary::BoundaryType::STOP ||
        boundary.boundary_type() == StGraphBoundary::BoundaryType::FOLLOW ||
        boundary.boundary_type() == StGraphBoundary::BoundaryType::YIELD) {
      *s_upper_bound = std::fmin(*s_upper_bound, s_upper);
    } else {
      *s_lower_bound = std::fmax(*s_lower_bound, s_lower);
    }
  }

  return Status::OK();
}

Status QpSplineStGraph::EstimateSpeedUpperBound(
    const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
    std::vector<double>* speed_upper_bound) const {
  DCHECK_NOTNULL(speed_upper_bound);
  speed_upper_bound->clear();

  // use v to estimate position: not accurate, but feasible in cyclic
  // processing. We can do the following process multiple times and use
  // previous cycle's results for better estimation.
  const double v = init_point.v();

  uint32_t i = 0;
  uint32_t j = 0;
  double distance = 0.0;
  const double kDistanceEpsilon = 1e-6;
  while (i < t_evaluated_.size() && j + 1 < speed_limit.speed_points().size()) {
    distance = v * t_evaluated_[i];
    if (fabs(distance - speed_limit.speed_points()[j].s()) < kDistanceEpsilon) {
      speed_upper_bound->push_back(speed_limit.speed_points()[j].v());
      ++i;
    } else if (speed_limit.speed_points()[j + 1].s() < distance) {
      ++j;
    } else {
      speed_upper_bound->push_back(speed_limit.get_speed_limit_by_s(distance));
      ++i;
    }
  }

  for (uint32_t k = speed_upper_bound->size() - 1; k < t_evaluated_.size();
       ++k) {
    speed_upper_bound->push_back(qp_spline_st_speed_config_.max_speed());
  }
  return Status::OK();
}

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


<commit_before>
#include "../HuboJmc.hpp"
#include "../HuboCanId.hpp"
#include "HuboState/State.hpp"

using namespace HuboCan;

void Hubo2PlusBasicJmc::update()
{
    if(NULL == _pump)
        return;

    _request_encoder_readings();
    _send_reference_commands();
}

void Hubo2PlusBasicJmc::_request_encoder_readings()
{
    _frame.can_id   = CMD_BYTE;

    _frame.data[0]  = info.hardware_index;
    _frame.data[1]  = GET_ENCODER;
    _frame.data[2] = 0;

    _frame.can_dlc = 3;

    _pump->addFrame(_frame, info.can_channel, 1);
}

void Hubo2PlusBasicJmc::_send_reference_commands()
{
    // TODO
}

bool Hubo2PlusBasicJmc::decode(const can_frame_t &frame, size_t channel)
{
    if( channel != info.can_channel )
        return false;

    if( frame.can_id - ENCODER_REPLY == info.hardware_index )
    {
        if(frame.can_dlc == 8)
        {
            std::cout << "jmc name: " << info.name << " | joint count: " << joints.size() << std::endl;

            for(size_t i=0; i < joints.size(); ++i)
            {
                int32_t encoder = 0;
                for(size_t j=3; j >= 0; --j)
                {
                    encoder = (encoder << 8) + frame.data[j + i*4];
                }

                std::cout << "checking joint index..." << std::endl;
                size_t joint_index = joints[i]->info.software_index;
                std::cout << "joint_index: " << joint_index << std::endl;
                _state->joints[joint_index].position =
                            joints[i]->encoder2radian(encoder);

                // TODO: Decide if velocity should be computed here
            }
            return true;
        }
        return false;
    }

    return false;
}
<commit_msg>debugging<commit_after>
#include "../HuboJmc.hpp"
#include "../HuboCanId.hpp"
#include "HuboState/State.hpp"

using namespace HuboCan;

void Hubo2PlusBasicJmc::update()
{
    if(NULL == _pump)
        return;

    _request_encoder_readings();
    _send_reference_commands();
}

void Hubo2PlusBasicJmc::_request_encoder_readings()
{
    _frame.can_id   = CMD_BYTE;

    _frame.data[0]  = info.hardware_index;
    _frame.data[1]  = GET_ENCODER;
    _frame.data[2] = 0;

    _frame.can_dlc = 3;

    _pump->addFrame(_frame, info.can_channel, 1);
}

void Hubo2PlusBasicJmc::_send_reference_commands()
{
    // TODO
}

bool Hubo2PlusBasicJmc::decode(const can_frame_t &frame, size_t channel)
{
    if( channel != info.can_channel )
        return false;

    if( frame.can_id - ENCODER_REPLY == info.hardware_index )
    {
        if(frame.can_dlc == 8)
        {
            std::cout << "jmc name: " << info.name << " | joint count: " << joints.size() << std::endl;

            for(size_t i=0; i < joints.size(); ++i)
            {
                int32_t encoder = 0;
                for(size_t j=3; j >= 0; --j)
                {
                    std::cout << "frame index: " << j + i*4 << std::endl;
                    encoder = (encoder << 8) + frame.data[j + i*4];
                }

                std::cout << "checking joint index..." << std::endl;
                size_t joint_index = joints[i]->info.software_index;
                std::cout << "joint_index: " << joint_index << std::endl;
                _state->joints[joint_index].position =
                            joints[i]->encoder2radian(encoder);

                // TODO: Decide if velocity should be computed here
            }
            return true;
        }
        return false;
    }

    return false;
}
<|endoftext|>


<commit_before>// Copyright (c) 2018 Chris Ohk, Youngjoong Kim, SeungHyun Jeon

// We are making my contributions/submissions to this project solely in our
// personal capacity and are not conveying any rights to any intellectual
// property of any third parties.

#ifndef HEARTHSTONEPP_IO_POLICY_HPP
#define HEARTHSTONEPP_IO_POLICY_HPP

#include <hspp/Policy/BasicPolicy.hpp>

#include <iostream>

namespace Hearthstonepp
{
class IoPolicy : public BasicPolicy
{
 public:
    IoPolicy(std::ostream& out, std::istream& in);

 private:
    TaskMeta RequireMulligan(Player& player) override;
    TaskMeta RequirePlaySpell(Player& player) override;
    TaskMeta RequirePlayCard(Player& player) override;
    TaskMeta RequirePlayMinion(Player& player) override;
    TaskMeta RequireCombat(Player& player) override;

    void NotifyOverDraw(const TaskMeta& meta) override;

    std::ostream& m_out;
    std::istream& m_in;
};
}  // namespace Hearthstonepp

#endif<commit_msg>docs: Add doxygen documentation<commit_after>// Copyright (c) 2018 Chris Ohk, Youngjoong Kim, SeungHyun Jeon

// We are making my contributions/submissions to this project solely in our
// personal capacity and are not conveying any rights to any intellectual
// property of any third parties.

#ifndef HEARTHSTONEPP_IO_POLICY_HPP
#define HEARTHSTONEPP_IO_POLICY_HPP

#include <hspp/Policy/BasicPolicy.hpp>

#include <iostream>

namespace Hearthstonepp
{
//!
//! \brief IoPolicy class.
//!
//! This class implement IPolicy for user input policy.
//!
class IoPolicy : public BasicPolicy
{
 public:
    //! Constructs IO policy with given \p out and \p in.
    //! \param out Basic output stream.
    //! \param in Basic input stream.
    IoPolicy(std::ostream& out, std::istream& in);

 private:
    //! Method for MulliganTask requirement.
    TaskMeta RequireMulligan(Player& player) override;

    //! Method for PlaySpellTask requirement.
    TaskMeta RequirePlaySpell(Player& player) override;

    //! Method for PlayCardTask requirement.
    TaskMeta RequirePlayCard(Player& player) override;

    //! Method for PlayMinionTask requirement.
    TaskMeta RequirePlayMinion(Player& player) override;

    //! Method for CombatTask requirement.
    TaskMeta RequireCombat(Player& player) override;

    //! Method for OverDraw notifying.
    void NotifyOverDraw(const TaskMeta& meta) override;

    std::ostream& m_out;
    std::istream& m_in;
};
}  // namespace Hearthstonepp

#endif<|endoftext|>


<commit_before>#include "PhysicsList.hh"

#include "G4EmDNAPhysics.hh"
#include "G4EmStandardPhysics.hh"
#include "G4EmStandardPhysics_option4.hh"
#include "G4EmLivermorePhysics.hh"
#include "G4OpticalPhysics.hh"
#include "HadronPhysicsQGSP_BERT_HP.hh"
#include "G4IonPhysics.hh"
/**
 * PhysicsList
 *
 * Constructs the physics of the simulation
 *
 * The physics list relies on modular physics. Currently used modules are:
 *  - Hadronic physics (High Precision)
 *  - IonPhysics
 *  - OpticalPhysics
 *  - EM Standard Physics
 */
PhysicsList::PhysicsList() : G4VModularPhysicsList() {
    currentDefaultCut   = 10*nm;

    // SetVerboseLevel(1);

    // Adding Physics List
    RegisterPhysics( new HadronPhysicsQGSP_BERT_HP());
    RegisterPhysics( new G4IonPhysics());
    RegisterPhysics( new G4OpticalPhysics());
    RegisterPhysics( new G4EmStandardPhysics_option4());
   // RegisterPhysics( new G4EmDNAPhysics());
}

/**
 * Deconstructor
 */
PhysicsList::~PhysicsList(){
    // Nothing to be done
}
/**
 * Sets the physic list cuts
 */
void PhysicsList::SetCuts(){
    SetDefaultCutValue(1*um);
}
<commit_msg>Removed hadrons from physics list<commit_after>#include "PhysicsList.hh"

#include "G4EmDNAPhysics.hh"
#include "G4EmStandardPhysics.hh"
#include "G4EmStandardPhysics_option4.hh"
#include "G4EmLivermorePhysics.hh"
#include "G4OpticalPhysics.hh"
#include "G4IonPhysics.hh"
/**
 * PhysicsList
 *
 * Constructs the physics of the simulation
 *
 * The physics list relies on modular physics. Currently used modules are:
 *  - IonPhysics
 *  - OpticalPhysics
 *  - EM Standard Physics
 */
PhysicsList::PhysicsList() : G4VModularPhysicsList() {
    currentDefaultCut   = 10*nm;

    // SetVerboseLevel(1);

    // Adding Physics List
    RegisterPhysics( new G4IonPhysics());
    RegisterPhysics( new G4OpticalPhysics());
    RegisterPhysics( new G4EmStandardPhysics_option4());
}

/**
 * Deconstructor
 */
PhysicsList::~PhysicsList(){
    // Nothing to be done
}
/**
 * Sets the physic list cuts
 */
void PhysicsList::SetCuts(){
    SetDefaultCutValue(1*um);
}
<|endoftext|>


<commit_before>#include "Skin2Mesh.h"
#include "PackCoords.h"
#include "PackArray.h"
#include "to_int.h"

#include <bimp/bimp_typedef.h>
#include <simp/SIMP_Skin2Mesh.h>
#include <polymesh/Skin2Triangles.h>
#include <polymesh/MeshType.h>

namespace esprpacker
{

Skin2Mesh::Skin2Mesh(const pm::Skin2Triangles* mesh)
	: m_mesh(mesh)
{
}

int Skin2Mesh::Type() const
{
	return pm::MESH_SKIN2;
}

void Skin2Mesh::PackToLuaString(ebuilder::CodeGenerator& gen) const
{
	// joints
	lua::assign_with_end(gen, "joints_num", m_mesh->joint_num);
	{
		lua::TableAssign ta(gen, "joints", true);
		for (int i = 0; i < m_mesh->joint_num; ++i)
		{
			const pm::Skin2Joint& joint = m_mesh->joints[i];
			lua::connect(gen, 6,
				lua::assign("joint", joint.joint),
				lua::assign("x", joint.vertex.x),
				lua::assign("y", joint.vertex.y),
				lua::assign("dx", joint.offset.x),
				lua::assign("dy", joint.offset.y),
				lua::assign("weight", joint.weight));
		}
	}
	// vertices
	lua::assign_with_end(gen, "vertices_num", m_mesh->vert_num);
	{
		lua::TableAssign ta(gen, "vertices", true);
		for (int i = 0; i < m_mesh->vert_num; ++i)
		{
			const pm::Skin2Vertex& vertex = m_mesh->vertices[i];
			lua::connect(gen, 5,
				lua::assign("joint_count", m_mesh->vertices_joint[i]),
				lua::assign("x", vertex.xy.x),
				lua::assign("y", vertex.xy.y),
				lua::assign("u", vertex.uv.x),
				lua::assign("v", vertex.uv.y));
		}
	}
	// triangles
	std::vector<int> triangles;
	triangles.reserve(m_mesh->tri_num);
	for (int i = 0; i < m_mesh->tri_num; ++i) {
		triangles.push_back(m_mesh->triangles[i]);
	}
	PackArray<int, uint16_t, uint16_t>::PackToLua(gen, triangles, "triangles");
}

int Skin2Mesh::SizeOfUnpackFromBin() const
{
	int sz = simp::Skin2Mesh::Size();

	// joints_n
	sz += ALIGN_4BYTE(sizeof(uint8_t) * m_mesh->vert_num);
	// joints
	sz += simp::Skin2Mesh::JointSize() * m_mesh->joint_num;
	// texcoords
	sz += ALIGN_4BYTE(sizeof(int16_t) * 2 * m_mesh->vert_num);
	// triangles
	sz += ALIGN_4BYTE(sizeof(uint16_t) * m_mesh->tri_num);

	return sz;
}

int Skin2Mesh::SizeOfPackToBin() const
{
	int sz = 0;

	// vertices num
	sz += sizeof(uint16_t);
	// joints num
	sz += sizeof(uint8_t) * m_mesh->vert_num;
	// joints
	sz += sizeof(uint16_t) * 4 * m_mesh->joint_num;
	// texcoords
	sz += sizeof(uint16_t);
	sz += sizeof(int16_t) * 2 * m_mesh->vert_num;
	// triangles
	sz += sizeof(uint16_t);
	sz += sizeof(uint16_t) * m_mesh->tri_num;

	return sz;
}

void Skin2Mesh::PackToBin(uint8_t** ptr) const
{
	// vertices num
	uint16_t v_num = m_mesh->vert_num;
	pack(v_num, ptr);
	// joints num
	for (int i = 0; i < v_num; ++i) {
		uint8_t n = m_mesh->vertices_joint[i];
		pack(n, ptr);
	}
	// joints
	for (int i = 0; i < m_mesh->joint_num; ++i)
	{
		const pm::Skin2Joint& joint = m_mesh->joints[i];
		uint16_t j = joint.joint;
		pack(j, ptr);
		uint16_t vx = float2int(joint.vertex.x, 64);
		pack(vx, ptr);
		uint16_t vy = float2int(joint.vertex.y, 64);
		pack(vy, ptr);
		int16_t weight = float2int(joint.weight - 0.5f, 2048);
		pack(weight, ptr);
	}
	// texcoords
	std::vector<sm::vec2> texcoords;
	texcoords.reserve(m_mesh->vert_num);
	for (int i = 0; i < m_mesh->vert_num; ++i) {
		texcoords.push_back(m_mesh->vertices[i].uv);
	}
	PackCoords::PackToBin(texcoords, ptr, 8192);
	// triangles
	std::vector<int> triangles;
	triangles.reserve(m_mesh->tri_num);
	for (int i = 0; i < m_mesh->tri_num; ++i) {
		triangles.push_back(m_mesh->triangles[i]);
	}
	PackArray<int, uint16_t, uint16_t>::PackToBin(triangles, ptr);
}

}<commit_msg>[FIXED] skin2 mesh pack<commit_after>#include "Skin2Mesh.h"
#include "PackCoords.h"
#include "PackArray.h"
#include "to_int.h"

#include <bimp/bimp_typedef.h>
#include <simp/SIMP_Skin2Mesh.h>
#include <polymesh/Skin2Triangles.h>
#include <polymesh/MeshType.h>

namespace esprpacker
{

Skin2Mesh::Skin2Mesh(const pm::Skin2Triangles* mesh)
	: m_mesh(mesh)
{
}

int Skin2Mesh::Type() const
{
	return pm::MESH_SKIN2;
}

void Skin2Mesh::PackToLuaString(ebuilder::CodeGenerator& gen) const
{
	// joints
	lua::assign_with_end(gen, "joints_num", m_mesh->joint_num);
	{
		lua::TableAssign ta(gen, "joints", true);
		for (int i = 0; i < m_mesh->joint_num; ++i)
		{
			const pm::Skin2Joint& joint = m_mesh->joints[i];
			lua::connect(gen, 6,
				lua::assign("joint", joint.joint),
				lua::assign("x", joint.vertex.x),
				lua::assign("y", joint.vertex.y),
				lua::assign("dx", joint.offset.x),
				lua::assign("dy", joint.offset.y),
				lua::assign("weight", joint.weight));
		}
	}
	// vertices
	lua::assign_with_end(gen, "vertices_num", m_mesh->vert_num);
	{
		lua::TableAssign ta(gen, "vertices", true);
		for (int i = 0; i < m_mesh->vert_num; ++i)
		{
			const pm::Skin2Vertex& vertex = m_mesh->vertices[i];
			lua::connect(gen, 5,
				lua::assign("joint_count", m_mesh->vertices_joint[i]),
				lua::assign("x", vertex.xy.x),
				lua::assign("y", vertex.xy.y),
				lua::assign("u", vertex.uv.x),
				lua::assign("v", vertex.uv.y));
		}
	}
	// triangles
	std::vector<int> triangles;
	triangles.reserve(m_mesh->tri_num);
	for (int i = 0; i < m_mesh->tri_num; ++i) {
		triangles.push_back(m_mesh->triangles[i]);
	}
	PackArray<int, uint16_t, uint16_t>::PackToLua(gen, triangles, "triangles");
}

int Skin2Mesh::SizeOfUnpackFromBin() const
{
	int sz = simp::Skin2Mesh::Size();

	// joints_n
	sz += ALIGN_4BYTE(sizeof(uint8_t) * m_mesh->vert_num);
	// joints
	sz += simp::Skin2Mesh::JointSize() * m_mesh->joint_num;
	// texcoords
	sz += ALIGN_4BYTE(sizeof(int16_t) * 2 * m_mesh->vert_num);
	// triangles
	sz += ALIGN_4BYTE(sizeof(uint16_t) * m_mesh->tri_num);

	return sz;
}

int Skin2Mesh::SizeOfPackToBin() const
{
	int sz = 0;

	// vertices num
	sz += sizeof(uint16_t);
	// joints num
	sz += sizeof(uint8_t) * m_mesh->vert_num;
	// joints
	sz += sizeof(uint16_t) * 4 * m_mesh->joint_num;
	// texcoords
	sz += sizeof(uint16_t);
	sz += sizeof(int16_t) * 2 * m_mesh->vert_num;
	// triangles
	sz += sizeof(uint16_t);
	sz += sizeof(uint16_t) * m_mesh->tri_num;

	return sz;
}

void Skin2Mesh::PackToBin(uint8_t** ptr) const
{
	// vertices num
	uint16_t v_num = m_mesh->vert_num;
	pack(v_num, ptr);
	// joints num
	for (int i = 0; i < v_num; ++i) {
		uint8_t n = m_mesh->vertices_joint[i];
		pack(n, ptr);
	}
	// joints
	for (int i = 0; i < m_mesh->joint_num; ++i)
	{
		const pm::Skin2Joint& joint = m_mesh->joints[i];
		uint16_t j = joint.joint;
		pack(j, ptr);
		uint16_t vx = float2int(joint.vertex.x, 16);
		pack(vx, ptr);
		uint16_t vy = float2int(joint.vertex.y, 16);
		pack(vy, ptr);
		int16_t weight = float2int(joint.weight - 0.5f, 2048);
		pack(weight, ptr);
	}
	// texcoords
	std::vector<sm::vec2> texcoords;
	texcoords.reserve(m_mesh->vert_num);
	for (int i = 0; i < m_mesh->vert_num; ++i) {
		texcoords.push_back(m_mesh->vertices[i].uv);
	}
	PackCoords::PackToBin(texcoords, ptr, 8192);
	// triangles
	std::vector<int> triangles;
	triangles.reserve(m_mesh->tri_num);
	for (int i = 0; i < m_mesh->tri_num; ++i) {
		triangles.push_back(m_mesh->triangles[i]);
	}
	PackArray<int, uint16_t, uint16_t>::PackToBin(triangles, ptr);
}

}<|endoftext|>