<commit_before>/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2018 INRIA.
 *
 * 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 NonSmoothDynamicalSystem.hpp
 * \brief container for DynamicalSystem and Interaction
 */
#ifndef NSDS_H
#define NSDS_H

#include "SiconosPointers.hpp"
#include "Topology.hpp"
#include "DynamicalSystem.hpp"

/** the NonSmoothDynamicalSystem consists in Dynamical Systems and Interactions
    structured into a graph defined in a Topology.
    In the DynamicalSystem graph, DynamicalSystem objects are nodes and Interaction objects
    are edges.

    To add a DynamicalSystem, use insertDynamicalSystem method.
    To add a new Interaction, use link method.

    A dual graph is also contructed, where Interactions are vertices and DynamicalSystems
    are edges.

*/
class NonSmoothDynamicalSystem
{
public:
  typedef enum
  {
    addDynamicalSystem, rmDynamicalSystem, addInteraction, rmInteraction, clearTopology
  } ChangeType;

  class Change
  {
  private:
    ACCEPT_SERIALIZATION(NonSmoothDynamicalSystem::Change);
    Change(){};
  public:
    ChangeType typeOfChange;
    SP::DynamicalSystem ds;
    SP::Interaction i;

    Change(ChangeType t, SP::DynamicalSystem dsnew ):typeOfChange(t),ds(dsnew){};
    Change(ChangeType t, SP::Interaction inew):typeOfChange(t),i(inew){};
    Change(ChangeType t):typeOfChange(t){};
    void display() const;
  };

  typedef std::list<Change> ChangeLog;
  class ChangeLogIter
  {
    ACCEPT_SERIALIZATION(NonSmoothDynamicalSystem::Change);
  public:
    ChangeLogIter(){};
    ChangeLogIter(const ChangeLog& log,
                  const ChangeLog::const_iterator& i)
      : _log(&log), it(i) {};
    const ChangeLog *_log;
    ChangeLog::const_iterator it;
  };

private:
  /** serialization hooks
  */
  ACCEPT_SERIALIZATION(NonSmoothDynamicalSystem);

  /** current time of the simulation
      Warning FP : it corresponds to the time
      at the end of the integration step.
      It means that _t corresponds to tkp1 of the
      simulation or nextTime().
   */
  double _t;

  /** initial time of the simulation */
  double _t0;

  /** final time of the simulation */
  double _T;

  /** information concerning the Model */
  std::string _title, _author, _description, _date;

  /** TRUE if the NonSmoothDynamicalSystem is a boundary value problem*/
  bool _BVP;

  /** log list of the modifications of the nsds */
  std::list<Change> _changeLog;

  /** the topology of the system */
  SP::Topology _topology;

  NonSmoothDynamicalSystem(const NonSmoothDynamicalSystem& nsds);

  /** False is one of the interaction is non-linear.
   */
  bool _mIsLinear;

public:

  /** default constructor
   */
  NonSmoothDynamicalSystem();

  /** constructor with t0 and T
   * \param t0 initial time
   * \param T final time
   */
  NonSmoothDynamicalSystem(double t0, double T);

  /** destructor
   */
  ~NonSmoothDynamicalSystem();

  // --- GETTERS/SETTERS ---
/** get the current time
   *  \return a double
   */
  inline double currentTime() const
  {
    return _t;
  }

  /** set the current time
   *  \param newValue the new time
   */
  inline void setCurrentTime(double newValue)
  {
    _t = newValue;
  }

  /** get initial time
   *  \return a double
   */
  inline double t0() const
  {
    return _t0;
  }

  /** set initial time of the time discretisation
   *  \param newT0
   */
  inline void sett0(double newT0)
  {
    _t0 = newT0;
  };

  /** get final time
   *  \return a double
   */
  inline double finalT() const
  {
    return _T;
  }

  /** set final time
   *  \param newValue the new final time for the Simulatiom
   */
  void setT(double newValue)
  {
    _T = newValue;
  };

  /** get the title of the simulation
   *  \return std::string : the title
   */
  inline const std::string  title() const
  {
    return _title;
  }

  /** set the title of the simulation
   *  \param s : the title
   */
  inline void setTitle(const std::string & s)
  {
    _title = s;
  }

  /** get the author of the simulation
   *  \return std::string : the author
   */
  inline const std::string  author() const
  {
    return _author;
  }

  /** set the author of the simulation
   *  \param s std::string : the author
   */
  inline void setAuthor(const std::string & s)
  {
    _author = s;
  }

  /** allows to get the description of the simulation
   *  \return std::string : the description
   */
  inline const std::string  description() const
  {
    return _description;
  }

  /** set the author of the simulation
   *  \param s std::string : the author
   */
  inline void setDescription(const std::string & s)
  {
    _description = s;
  }

  /** allows to get the date of the simulation
   *  \return std::string : the date
   */
  inline const std::string  date() const
  {
    return _date;
  }

  /** set the date of the simulation
   *  \param s std::string : the date
   */
  inline void setDate(const std::string & s)
  {
    _date = s;
  }

  /** get problem type (true if BVP)
   *  \return a bool
   */
  inline bool isBVP() const
  {
    return _BVP;
  }

  /** get problem type (true if IVP)
   *  \return a bool
   */
  inline bool isIVP() const
  {
    return !_BVP;
  }

  /** set the NonSmoothDynamicalSystem to BVP, else it is IVP
   *  \param newBvp true if BVP, false otherwise
   */
  inline void setBVP(const bool& newBvp)
  {
    _BVP = newBvp;
  }

  /** get a reference to the changelog for an NSDS.
   * \return a reference to the changelog.
   */
  inline const ChangeLog& changeLog()
  {
    return _changeLog;
  };

  /** get an iterator to the last item in the changelog.
   * \return an iterator pointing at the last item in the changelog.
   */
  inline ChangeLogIter changeLogPosition()
  {
    ChangeLogIter it(_changeLog, _changeLog.end());
    // return iterator to last item, i.e. one less than end
    --it.it;
    return it;
  };

  /** get an iterator to the beginning of the changelog.
   * \return an iterator pointing at the beginning of the changelog.
   */
  inline ChangeLogIter changeLogBegin()
  {
    ChangeLogIter it(_changeLog, _changeLog.begin());
    return it;
  };

  /** clear the changelog up to a given position.
   *  \param it  This iterator must point to somewhere in the changelog
   *             for this NSDS.
   */
  void clearChangeLogTo(const ChangeLogIter& it);

  // === DynamicalSystems management ===

  /** get the number of Dynamical Systems present in the NSDS
      \return an unsigned int
   */
  inline unsigned int getNumberOfDS() const
  {
    return _topology->dSG(0)->size();
  }

  /** get all the dynamical systems declared in the NonSmoothDynamicalSystem.
   * \return a SP::DynamicalSystemsGraph
   */
  inline const SP::DynamicalSystemsGraph dynamicalSystems() const
  {
    return _topology->dSG(0);
  }

  /** add a dynamical system into the DS graph (as a vertex)
   * \param ds a pointer to the system to add
   */
  void insertDynamicalSystem(SP::DynamicalSystem ds);

  /** get Dynamical system number I
   * \param nb the identifier of the DynamicalSystem to get
   * \return a pointer on DynamicalSystem
   */
  inline SP::DynamicalSystem dynamicalSystems(int nb) const
  {
    return _topology->getDynamicalSystems(nb);
  }
  inline void displayDynamicalSystem() const
  {
    _topology->displayDynamicalSystem();
  }

  /** remove a dynamical system
   * \param ds a pointer to the dynamical system to remove
   */
  void removeDynamicalSystem(SP::DynamicalSystem ds);

  // === Interactions management ===

  /** get the number of Interactions present in the NSDS.
   *  \return an unsigned int
   */
  inline unsigned int getNumberOfInteractions() const
  {
    return _topology->indexSet0()->size();
  };

  /** return the graph of  Interactions present in the NSDS.
   *  \return SP::InteractionGraph
   */
  inline const SP::InteractionsGraph  interactions() const
  {
    return _topology->indexSet0();
  };

  /** remove an interaction to the system
   * \param inter a pointer to the interaction to remove
   */
  void removeInteraction(SP::Interaction inter);

  /** get Interaction number I
   * \param nb the identifier of the Interaction to get
   * \return a pointer to an Interaction
   */
  inline SP::Interaction interaction(int nb) const
  {
    return _topology->getInteraction(nb);
  }

  /** get Interaction named name
   * \param name of the Interaction to get
   * \return a pointer to an Interaction
   */
  inline SP::Interaction interaction(std::string name) const
  {
    return _topology->getInteraction(name);
  }

  /** link an interaction to two dynamical systems
   * \param inter the interaction
   * \param ds1 a DynamicalSystem
   * \param ds2 a DynamicalSystem (optional)
   */
  void link(SP::Interaction inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2 = SP::DynamicalSystem());

  /** set the name for this Dynamical System
   * \param ds a pointer to the system
   * \param name the name of the DynamicalSystem
   */
  inline void setName(SP::DynamicalSystem ds, const std::string& name)
  {
    _topology->setName(ds, name);
  };

  /** get the name for this Dynamical System
   * \param ds a pointer to the system
   * \return name the name of the DynamicalSystem, or empty string if not found.
   */
  std::string name(SP::DynamicalSystem ds)
  {
    return _topology->name(ds);
  }

  /** set the name for this Interaction
   * \param interaction a pointer to the Interaction
   * \param name the name of the Interaction
   */
  inline void setName(SP::Interaction interaction, const std::string& name)
  {
    _topology->setName(interaction, name);
  };

  /** get the name for this Interaction
   * \param inter a pointer to the Interaction
   * \return name the name of the Interaction, or empty string if not found.
   */
  std::string name(SP::Interaction inter)
  {
    return _topology->name(inter);
  }

  /** specify id the given Interaction is for controlling the DS
   * \param inter the Interaction
   * \param isControlInteraction true if the Interaction is used for
   * control purposes
   **/
  void setControlProperty(SP::Interaction inter, const bool isControlInteraction)
  {
    _topology->setControlProperty(inter, isControlInteraction);
  }

  /** get the topology of the system
   *  \return a pointer on Topology
   */
  inline SP::Topology topology() const
  {
    return _topology;
  }

  /** display the data of the Non Smooth Dynamical System
   */
  void display() const;

  /** return false is one of the interations is not linear.  else
   *  return true.
   *  \return a bool
   */
  inline bool isLinear() const
  {
    return _mIsLinear;
  };

  void clear();

  /** set symmetry in the blocks computation
   * \param val a bool
   */
  void setSymmetric(bool val);

  /** Set all DS non-smooth part to zero.
   */
  void reset();

  /** Set all DS non-smooth part to zero for a given level.
   * \param level the level to will be zeroed
   */
  void reset(unsigned int level);

  /** save DynamicalSystems and Interactions states in Memories
   */
  void swapInMemory();

  /** save interaction states in memories. Applied to all interactions
   of the connected topology
  */
  void pushInteractionsInMemory();

  /** compute r thanks to lambda[level] for all Interactions
    * \param time
    * \param level lambda level
   */
  void updateInput(double time, unsigned int level);

  /** compute output for all the interactions for a given level
   * \param time
   * \param level y order to be computed
   */
  void updateOutput(double time, unsigned int level = 0);

  /** compute output for all the interactions and for a level range
   * \param time
   * \param level_min y min order to be computed
   * \param level_max y max order to be computed
   */
  void updateOutput(double time, unsigned int level_min, unsigned int level_max);

  /** compute Jacobians for all the interactions (in indexSet0)
   * \param time
   */
  void computeInteractionJacobians(double time);

  /** compute Jacobians for all the interactions of a given index set.
   \param time
   \param indexSet InteractionsGraph of interest
   */
  void computeInteractionJacobians(double time, InteractionsGraph& indexSet);

  /** visit all dynamical systems in this system.
   * \param visitor an SP::SiconosVisitor that can visit classes derived from DS
   */
  void visitDynamicalSystems(SP::SiconosVisitor visitor);
};

#endif
<commit_msg>[kernel] fix typos<commit_after>/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2018 INRIA.
 *
 * 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 NonSmoothDynamicalSystem.hpp
 * \brief container for DynamicalSystem and Interaction
 */
#ifndef NSDS_H
#define NSDS_H

#include "SiconosPointers.hpp"
#include "Topology.hpp"
#include "DynamicalSystem.hpp"

/** the NonSmoothDynamicalSystem consists in Dynamical Systems and Interactions
    structured into a graph defined in a Topology.
    In the DynamicalSystem graph, DynamicalSystem objects are nodes and Interaction objects
    are edges.

    To add a DynamicalSystem, use insertDynamicalSystem method.
    To add a new Interaction, use link method.

    A dual graph is also contructed, where Interactions are vertices and DynamicalSystems
    are edges.

*/
class NonSmoothDynamicalSystem
{
public:
  typedef enum
  {
    addDynamicalSystem, rmDynamicalSystem, addInteraction, rmInteraction, clearTopology
  } ChangeType;

  class Change
  {
  private:
    ACCEPT_SERIALIZATION(NonSmoothDynamicalSystem::Change);
    Change(){};
  public:
    ChangeType typeOfChange;
    SP::DynamicalSystem ds;
    SP::Interaction i;

    Change(ChangeType t, SP::DynamicalSystem dsnew ):typeOfChange(t),ds(dsnew){};
    Change(ChangeType t, SP::Interaction inew):typeOfChange(t),i(inew){};
    Change(ChangeType t):typeOfChange(t){};
    void display() const;
  };

  typedef std::list<Change> ChangeLog;
  class ChangeLogIter
  {
    ACCEPT_SERIALIZATION(NonSmoothDynamicalSystem::Change);
  public:
    ChangeLogIter(){};
    ChangeLogIter(const ChangeLog& log,
                  const ChangeLog::const_iterator& i)
      : _log(&log), it(i) {};
    const ChangeLog *_log;
    ChangeLog::const_iterator it;
  };

private:
  /** serialization hooks
  */
  ACCEPT_SERIALIZATION(NonSmoothDynamicalSystem);

  /** current time of the simulation
      Warning FP : it corresponds to the time
      at the end of the integration step.
      It means that _t corresponds to tkp1 of the
      simulation or nextTime().
   */
  double _t;

  /** initial time of the simulation */
  double _t0;

  /** final time of the simulation */
  double _T;

  /** information concerning the Model */
  std::string _title, _author, _description, _date;

  /** TRUE if the NonSmoothDynamicalSystem is a boundary value problem*/
  bool _BVP;

  /** log list of the modifications of the nsds */
  std::list<Change> _changeLog;

  /** the topology of the system */
  SP::Topology _topology;

  NonSmoothDynamicalSystem(const NonSmoothDynamicalSystem& nsds);

  /** False is one of the interaction is non-linear.
   */
  bool _mIsLinear;

public:

  /** default constructor
   */
  NonSmoothDynamicalSystem();

  /** constructor with t0 and T
   * \param t0 initial time
   * \param T final time
   */
  NonSmoothDynamicalSystem(double t0, double T);

  /** destructor
   */
  ~NonSmoothDynamicalSystem();

  // --- GETTERS/SETTERS ---
/** get the current time
   *  \return a double
   */
  inline double currentTime() const
  {
    return _t;
  }

  /** set the current time
   *  \param newValue the new time
   */
  inline void setCurrentTime(double newValue)
  {
    _t = newValue;
  }

  /** get initial time
   *  \return a double
   */
  inline double t0() const
  {
    return _t0;
  }

  /** set initial time of the time discretisation
   *  \param newT0
   */
  inline void sett0(double newT0)
  {
    _t0 = newT0;
  };

  /** get final time
   *  \return a double
   */
  inline double finalT() const
  {
    return _T;
  }

  /** set final time
   *  \param newValue the new final time for the Simulatiom
   */
  void setT(double newValue)
  {
    _T = newValue;
  };

  /** get the title of the simulation
   *  \return std::string : the title
   */
  inline const std::string  title() const
  {
    return _title;
  }

  /** set the title of the simulation
   *  \param s : the title
   */
  inline void setTitle(const std::string & s)
  {
    _title = s;
  }

  /** get the author of the simulation
   *  \return std::string : the author
   */
  inline const std::string  author() const
  {
    return _author;
  }

  /** set the author of the simulation
   *  \param s std::string : the author
   */
  inline void setAuthor(const std::string & s)
  {
    _author = s;
  }

  /** allows to get the description of the simulation
   *  \return std::string : the description
   */
  inline const std::string  description() const
  {
    return _description;
  }

  /** set the author of the simulation
   *  \param s std::string : the author
   */
  inline void setDescription(const std::string & s)
  {
    _description = s;
  }

  /** allows to get the date of the simulation
   *  \return std::string : the date
   */
  inline const std::string  date() const
  {
    return _date;
  }

  /** set the date of the simulation
   *  \param s std::string : the date
   */
  inline void setDate(const std::string & s)
  {
    _date = s;
  }

  /** get problem type (true if BVP)
   *  \return a bool
   */
  inline bool isBVP() const
  {
    return _BVP;
  }

  /** get problem type (true if IVP)
   *  \return a bool
   */
  inline bool isIVP() const
  {
    return !_BVP;
  }

  /** set the NonSmoothDynamicalSystem to BVP, else it is IVP
   *  \param newBvp true if BVP, false otherwise
   */
  inline void setBVP(const bool& newBvp)
  {
    _BVP = newBvp;
  }

  /** get a reference to the changelog for an NSDS.
   * \return a reference to the changelog.
   */
  inline const ChangeLog& changeLog()
  {
    return _changeLog;
  };

  /** get an iterator to the last item in the changelog.
   * \return an iterator pointing at the last item in the changelog.
   */
  inline ChangeLogIter changeLogPosition()
  {
    ChangeLogIter it(_changeLog, _changeLog.end());
    // return iterator to last item, i.e. one less than end
    --it.it;
    return it;
  };

  /** get an iterator to the beginning of the changelog.
   * \return an iterator pointing at the beginning of the changelog.
   */
  inline ChangeLogIter changeLogBegin()
  {
    ChangeLogIter it(_changeLog, _changeLog.begin());
    return it;
  };

  /** clear the changelog up to a given position.
   *  \param it  This iterator must point to somewhere in the changelog
   *             for this NSDS.
   */
  void clearChangeLogTo(const ChangeLogIter& it);

  // === DynamicalSystems management ===

  /** get the number of Dynamical Systems present in the NSDS
      \return an unsigned int
   */
  inline unsigned int getNumberOfDS() const
  {
    return _topology->dSG(0)->size();
  }

  /** get all the dynamical systems declared in the NonSmoothDynamicalSystem.
   * \return a SP::DynamicalSystemsGraph
   */
  inline const SP::DynamicalSystemsGraph dynamicalSystems() const
  {
    return _topology->dSG(0);
  }

  /** add a dynamical system into the DS graph (as a vertex)
   * \param ds a pointer to the system to add
   */
  void insertDynamicalSystem(SP::DynamicalSystem ds);

  /** get Dynamical system number I
   * \param nb the identifier of the DynamicalSystem to get
   * \return a pointer on DynamicalSystem
   */
  inline SP::DynamicalSystem dynamicalSystem(int nb) const
  {
    return _topology->getDynamicalSystem(nb);
  }
  inline void displayDynamicalSystems() const
  {
    _topology->displayDynamicalSystems();
  }

  /** remove a dynamical system
   * \param ds a pointer to the dynamical system to remove
   */
  void removeDynamicalSystem(SP::DynamicalSystem ds);

  // === Interactions management ===

  /** get the number of Interactions present in the NSDS.
   *  \return an unsigned int
   */
  inline unsigned int getNumberOfInteractions() const
  {
    return _topology->indexSet0()->size();
  };

  /** return the graph of  Interactions present in the NSDS.
   *  \return SP::InteractionGraph
   */
  inline const SP::InteractionsGraph  interactions() const
  {
    return _topology->indexSet0();
  };

  /** remove an interaction to the system
   * \param inter a pointer to the interaction to remove
   */
  void removeInteraction(SP::Interaction inter);

  /** get Interaction number I
   * \param nb the identifier of the Interaction to get
   * \return a pointer to an Interaction
   */
  inline SP::Interaction interaction(int nb) const
  {
    return _topology->getInteraction(nb);
  }

  /** get Interaction named name
   * \param name of the Interaction to get
   * \return a pointer to an Interaction
   */
  inline SP::Interaction interaction(std::string name) const
  {
    return _topology->getInteraction(name);
  }

  /** link an interaction to two dynamical systems
   * \param inter the interaction
   * \param ds1 a DynamicalSystem
   * \param ds2 a DynamicalSystem (optional)
   */
  void link(SP::Interaction inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2 = SP::DynamicalSystem());

  /** set the name for this Dynamical System
   * \param ds a pointer to the system
   * \param name the name of the DynamicalSystem
   */
  inline void setName(SP::DynamicalSystem ds, const std::string& name)
  {
    _topology->setName(ds, name);
  };

  /** get the name for this Dynamical System
   * \param ds a pointer to the system
   * \return name the name of the DynamicalSystem, or empty string if not found.
   */
  std::string name(SP::DynamicalSystem ds)
  {
    return _topology->name(ds);
  }

  /** set the name for this Interaction
   * \param interaction a pointer to the Interaction
   * \param name the name of the Interaction
   */
  inline void setName(SP::Interaction interaction, const std::string& name)
  {
    _topology->setName(interaction, name);
  };

  /** get the name for this Interaction
   * \param inter a pointer to the Interaction
   * \return name the name of the Interaction, or empty string if not found.
   */
  std::string name(SP::Interaction inter)
  {
    return _topology->name(inter);
  }

  /** specify id the given Interaction is for controlling the DS
   * \param inter the Interaction
   * \param isControlInteraction true if the Interaction is used for
   * control purposes
   **/
  void setControlProperty(SP::Interaction inter, const bool isControlInteraction)
  {
    _topology->setControlProperty(inter, isControlInteraction);
  }

  /** get the topology of the system
   *  \return a pointer on Topology
   */
  inline SP::Topology topology() const
  {
    return _topology;
  }

  /** display the data of the Non Smooth Dynamical System
   */
  void display() const;

  /** return false is one of the interations is not linear.  else
   *  return true.
   *  \return a bool
   */
  inline bool isLinear() const
  {
    return _mIsLinear;
  };

  void clear();

  /** set symmetry in the blocks computation
   * \param val a bool
   */
  void setSymmetric(bool val);

  /** Set all DS non-smooth part to zero.
   */
  void reset();

  /** Set all DS non-smooth part to zero for a given level.
   * \param level the level to will be zeroed
   */
  void reset(unsigned int level);

  /** save DynamicalSystems and Interactions states in Memories
   */
  void swapInMemory();

  /** save interaction states in memories. Applied to all interactions
   of the connected topology
  */
  void pushInteractionsInMemory();

  /** compute r thanks to lambda[level] for all Interactions
    * \param time
    * \param level lambda level
   */
  void updateInput(double time, unsigned int level);

  /** compute output for all the interactions for a given level
   * \param time
   * \param level y order to be computed
   */
  void updateOutput(double time, unsigned int level = 0);

  /** compute output for all the interactions and for a level range
   * \param time
   * \param level_min y min order to be computed
   * \param level_max y max order to be computed
   */
  void updateOutput(double time, unsigned int level_min, unsigned int level_max);

  /** compute Jacobians for all the interactions (in indexSet0)
   * \param time
   */
  void computeInteractionJacobians(double time);

  /** compute Jacobians for all the interactions of a given index set.
   \param time
   \param indexSet InteractionsGraph of interest
   */
  void computeInteractionJacobians(double time, InteractionsGraph& indexSet);

  /** visit all dynamical systems in this system.
   * \param visitor an SP::SiconosVisitor that can visit classes derived from DS
   */
  void visitDynamicalSystems(SP::SiconosVisitor visitor);
};

#endif
<|endoftext|>


<commit_before>/*
Copyright (c) 2013 Microsoft Corporation. All rights reserved.
Released under Apache 2.0 license as described in the file LICENSE.

Author: Leonardo de Moura
*/
#include "util/flet.h"
#include "util/scoped_map.h"
#include "util/interrupt.h"
#include "kernel/environment.h"
#include "kernel/normalizer.h"
#include "kernel/builtin.h"
#include "kernel/kernel_exception.h"
#include "kernel/type_checker_justification.h"
#include "kernel/instantiate.h"
#include "kernel/free_vars.h"
#include "kernel/metavar.h"
#include "library/kernel_bindings.h"
#include "library/type_inferer.h"

namespace lean {
static name g_x_name("x");
class type_inferer::imp {
    typedef scoped_map<expr, expr, expr_hash_alloc, expr_eqp> cache;
    typedef buffer<unification_constraint> unification_constraints;

    ro_environment            m_env;
    context                   m_ctx;
    cached_metavar_env        m_menv;
    unification_constraints * m_uc;
    normalizer                m_normalizer;
    cache                     m_cache;

    expr normalize(expr const & e, context const & ctx) {
        return m_normalizer(e, ctx, m_menv.to_some_menv());
    }
    expr lift_free_vars(expr const & e, unsigned s, unsigned d) {
        return ::lean::lift_free_vars(e, s, d, m_menv.to_some_menv());
    }

    expr lift_free_vars(expr const & e, unsigned d) {
        return ::lean::lift_free_vars(e, d, m_menv.to_some_menv());
    }

    expr instantiate(expr const & e, unsigned n, expr const * s) {
        return ::lean::instantiate(e, n, s, m_menv.to_some_menv());
    }

    expr check_type(expr const & e, expr const & s, context const & ctx) {
        if (is_type(e))
            return e;
        if (is_bool(e))
            return Type();
        expr u = normalize(e, ctx);
        if (is_type(u))
            return u;
        if (is_bool(u))
            return Type();
        if (has_metavar(u) && m_menv && m_uc) {
            justification jst = mk_type_expected_justification(ctx, s);
            m_uc->push_back(mk_convertible_constraint(ctx, u, TypeU, jst));
            return u;
        }
        throw type_expected_exception(m_env, ctx, s);
    }

    expr get_range(expr t, expr const & e, context const & ctx) {
        unsigned num = num_args(e) - 1;
        while (num > 0) {
            --num;
            if (is_pi(t)) {
                t = abst_body(t);
            } else {
                t = m_normalizer(t, ctx);
                if (is_pi(t)) {
                    t = abst_body(t);
                } else if (has_metavar(t) && m_menv && m_uc) {
                    // Create two fresh variables A and B,
                    // and assign r == (Pi(x : A), B)
                    expr A   = m_menv->mk_metavar(ctx);
                    expr B   = m_menv->mk_metavar(extend(ctx, g_x_name, A));
                    expr p   = mk_pi(g_x_name, A, B);
                    justification jst = mk_function_expected_justification(ctx, e);
                    m_uc->push_back(mk_eq_constraint(ctx, t, p, jst));
                    t        = abst_body(p);
                } else {
                    throw function_expected_exception(m_env, ctx, e);
                }
            }
        }
        if (closed(t))
            return t;
        else
            return instantiate(t, num_args(e)-1, &arg(e, 1));
    }

    expr infer_type(expr const & e, context const & ctx) {
        // cheap cases, we do not cache results
        switch (e.kind()) {
        case expr_kind::MetaVar:
            if (m_menv) {
                if (m_menv->is_assigned(e))
                    return infer_type(*(m_menv->get_subst(e)), ctx);
                else
                    return m_menv->get_type(e);
            } else {
                throw unexpected_metavar_occurrence(m_env, e);
            }
        case expr_kind::Constant: {
            if (const_type(e)) {
                return *const_type(e);
            } else {
                object const & obj = m_env->get_object(const_name(e));
                if (obj.has_type())
                    return obj.get_type();
                else
                    throw has_no_type_exception(m_env, e);
            }
            break;
        }
        case expr_kind::Var: {
            auto p = lookup_ext(ctx, var_idx(e));
            context_entry const & ce = p.first;
            if (ce.get_domain()) {
                context const & ce_ctx   = p.second;
                return lift_free_vars(*(ce.get_domain()), ctx.size() - ce_ctx.size());
            }
            // Remark: the case where ce.get_domain() is not
            // available is not considered cheap.
            break;
        }
        case expr_kind::Eq:
            return mk_bool_type();
        case expr_kind::Value:
            return to_value(e).get_type();
        case expr_kind::Type:
            return mk_type(ty_level(e) + 1);
        case expr_kind::App: case expr_kind::Lambda:
        case expr_kind::Pi:  case expr_kind::Let:
            break; // expensive cases
        }

        check_system("type inference");
        bool shared = false;
        if (is_shared(e)) {
            shared = true;
            auto it = m_cache.find(e);
            if (it != m_cache.end())
                return it->second;
        }

        expr r;
        switch (e.kind()) {
        case expr_kind::Constant: case expr_kind::Eq:
        case expr_kind::Value:    case expr_kind::Type:
        case expr_kind::MetaVar:
            lean_unreachable(); // LCOV_EXCL_LINE
        case expr_kind::Var: {
            auto p = lookup_ext(ctx, var_idx(e));
            context_entry const & ce = p.first;
            context const & ce_ctx   = p.second;
            lean_assert(!ce.get_domain());
            r = lift_free_vars(infer_type(*(ce.get_body()), ce_ctx), ctx.size() - ce_ctx.size());
            break;
        }
        case expr_kind::App: {
            expr const & f = arg(e, 0);
            expr f_t = infer_type(f, ctx);
            r = get_range(f_t, e, ctx);
            break;
        }
        case expr_kind::Lambda: {
            cache::mk_scope sc(m_cache);
            r = mk_pi(abst_name(e), abst_domain(e), infer_type(abst_body(e), extend(ctx, abst_name(e), abst_domain(e))));
            break;
        }
        case expr_kind::Pi: {
            expr t1  = check_type(infer_type(abst_domain(e), ctx), abst_domain(e), ctx);
            expr t2;
            context new_ctx = extend(ctx, abst_name(e), abst_domain(e));
            {
                cache::mk_scope sc(m_cache);
                t2 = check_type(infer_type(abst_body(e), new_ctx), abst_body(e), new_ctx);
            }
            if (is_type(t1) && is_type(t2)) {
                r = mk_type(max(ty_level(t1), ty_level(t2)));
            } else {
                lean_assert(m_uc);
                justification jst = mk_max_type_justification(ctx, e);
                r = m_menv->mk_metavar(ctx);
                m_uc->push_back(mk_max_constraint(new_ctx, lift_free_vars(t1, 0, 1), t2, r, jst));
            }
            break;
        }
        case expr_kind::Let: {
            cache::mk_scope sc(m_cache);
            r = infer_type(let_body(e), extend(ctx, let_name(e), let_type(e), let_value(e)));
            break;
        }}

        if (shared) {
            m_cache.insert(e, r);
        }
        return r;
    }

    void set_ctx(context const & ctx) {
        if (!is_eqp(m_ctx, ctx)) {
            clear();
            m_ctx = ctx;
        }
    }

public:
    imp(ro_environment const & env):
        m_env(env),
        m_normalizer(env) {
        m_uc = nullptr;
    }

    expr operator()(expr const & e, context const & ctx, optional<metavar_env> const & menv, buffer<unification_constraint> * uc) {
        set_ctx(ctx);
        if (m_menv.update(menv))
            clear_cache();
        flet<unification_constraints*> set(m_uc, uc);
        return infer_type(e, ctx);
    }

    void clear_cache() {
        m_cache.clear();
        m_normalizer.clear();
    }

    void clear() {
        clear_cache();
        m_menv.clear();
        m_ctx = context();
    }

    bool is_proposition(expr const & e, context const & ctx, optional<metavar_env> const & menv) {
        // Catch easy cases
        switch (e.kind()) {
        case expr_kind::Lambda: case expr_kind::Pi: case expr_kind::Type: return false;
        case expr_kind::Eq: return true;
        default: break;
        }
        expr t = operator()(e, ctx, menv, nullptr);
        if (is_bool(t))
            return true;
        else
            return is_bool(normalize(t, ctx));
    }
};
type_inferer::type_inferer(ro_environment const & env):m_ptr(new imp(env)) {}
type_inferer::~type_inferer() {}
expr type_inferer::operator()(expr const & e, context const & ctx, optional<metavar_env> const & menv, buffer<unification_constraint> * uc) {
    return m_ptr->operator()(e, ctx, menv, uc);
}
expr type_inferer::operator()(expr const & e, context const & ctx, metavar_env const & menv, buffer<unification_constraint> & uc) {
    return m_ptr->operator()(e, ctx, some_menv(menv), &uc);
}
expr type_inferer::operator()(expr const & e, context const & ctx) {
    return operator()(e, ctx, none_menv(), nullptr);
}
bool type_inferer::is_proposition(expr const & e, context const & ctx, optional<metavar_env> const & menv) {
    return m_ptr->is_proposition(e, ctx, menv);
}
bool type_inferer::is_proposition(expr const & e, context const & ctx) {
    return is_proposition(e, ctx, none_menv());
}
bool type_inferer::is_proposition(expr const & e, context const & ctx, metavar_env const & menv) {
    return is_proposition(e, ctx, some_menv(menv));
}
void type_inferer::clear() { m_ptr->clear(); }

constexpr char const * type_inferer_mt = "type_inferer";
type_inferer & to_type_inferer(lua_State * L, int i) { return *static_cast<type_inferer*>(luaL_checkudata(L, i, type_inferer_mt)); }
DECL_PRED(type_inferer)
DECL_GC(type_inferer)

static int type_inferer_call(lua_State * L) {
    int nargs = lua_gettop(L);
    type_inferer & inferer = to_type_inferer(L, 1);
    if (nargs == 2)
        return push_expr(L, inferer(to_expr(L, 2)));
    else
        return push_expr(L, inferer(to_expr(L, 2), to_context(L, 3)));
}

static int type_inferer_clear(lua_State * L) {
    to_type_inferer(L, 1).clear();
    return 0;
}

static int mk_type_inferer(lua_State * L) {
    void * mem = lua_newuserdata(L, sizeof(type_inferer));
    new (mem) type_inferer(to_environment(L, 1));
    luaL_getmetatable(L, type_inferer_mt);
    lua_setmetatable(L, -2);
    return 1;
}

static const struct luaL_Reg type_inferer_m[] = {
    {"__gc",            type_inferer_gc}, // never throws
    {"__call",          safe_function<type_inferer_call>},
    {"clear",           safe_function<type_inferer_clear>},
    {0, 0}
};

void open_type_inferer(lua_State * L) {
    luaL_newmetatable(L, type_inferer_mt);
    lua_pushvalue(L, -1);
    lua_setfield(L, -2, "__index");
    setfuncs(L, type_inferer_m, 0);

    SET_GLOBAL_FUN(mk_type_inferer,          "type_inferer");
    SET_GLOBAL_FUN(type_inferer_pred,        "is_type_inferer");
}
}
<commit_msg>fix(library/type_inferer): another incorrect use of scoped_map<commit_after>/*
Copyright (c) 2013 Microsoft Corporation. All rights reserved.
Released under Apache 2.0 license as described in the file LICENSE.

Author: Leonardo de Moura
*/
#include <unordered_map>
#include "util/flet.h"
#include "util/freset.h"
#include "util/interrupt.h"
#include "kernel/environment.h"
#include "kernel/normalizer.h"
#include "kernel/builtin.h"
#include "kernel/kernel_exception.h"
#include "kernel/type_checker_justification.h"
#include "kernel/instantiate.h"
#include "kernel/free_vars.h"
#include "kernel/metavar.h"
#include "library/kernel_bindings.h"
#include "library/type_inferer.h"

namespace lean {
static name g_x_name("x");
class type_inferer::imp {
    typedef std::unordered_map<expr, expr, expr_hash_alloc, expr_eqp> cache;
    typedef buffer<unification_constraint> unification_constraints;

    ro_environment            m_env;
    context                   m_ctx;
    cached_metavar_env        m_menv;
    unification_constraints * m_uc;
    normalizer                m_normalizer;
    cache                     m_cache;

    expr normalize(expr const & e, context const & ctx) {
        return m_normalizer(e, ctx, m_menv.to_some_menv());
    }
    expr lift_free_vars(expr const & e, unsigned s, unsigned d) {
        return ::lean::lift_free_vars(e, s, d, m_menv.to_some_menv());
    }

    expr lift_free_vars(expr const & e, unsigned d) {
        return ::lean::lift_free_vars(e, d, m_menv.to_some_menv());
    }

    expr instantiate(expr const & e, unsigned n, expr const * s) {
        return ::lean::instantiate(e, n, s, m_menv.to_some_menv());
    }

    expr check_type(expr const & e, expr const & s, context const & ctx) {
        if (is_type(e))
            return e;
        if (is_bool(e))
            return Type();
        expr u = normalize(e, ctx);
        if (is_type(u))
            return u;
        if (is_bool(u))
            return Type();
        if (has_metavar(u) && m_menv && m_uc) {
            justification jst = mk_type_expected_justification(ctx, s);
            m_uc->push_back(mk_convertible_constraint(ctx, u, TypeU, jst));
            return u;
        }
        throw type_expected_exception(m_env, ctx, s);
    }

    expr get_range(expr t, expr const & e, context const & ctx) {
        unsigned num = num_args(e) - 1;
        while (num > 0) {
            --num;
            if (is_pi(t)) {
                t = abst_body(t);
            } else {
                t = m_normalizer(t, ctx);
                if (is_pi(t)) {
                    t = abst_body(t);
                } else if (has_metavar(t) && m_menv && m_uc) {
                    // Create two fresh variables A and B,
                    // and assign r == (Pi(x : A), B)
                    expr A   = m_menv->mk_metavar(ctx);
                    expr B   = m_menv->mk_metavar(extend(ctx, g_x_name, A));
                    expr p   = mk_pi(g_x_name, A, B);
                    justification jst = mk_function_expected_justification(ctx, e);
                    m_uc->push_back(mk_eq_constraint(ctx, t, p, jst));
                    t        = abst_body(p);
                } else {
                    throw function_expected_exception(m_env, ctx, e);
                }
            }
        }
        if (closed(t))
            return t;
        else
            return instantiate(t, num_args(e)-1, &arg(e, 1));
    }

    expr infer_type(expr const & e, context const & ctx) {
        // cheap cases, we do not cache results
        switch (e.kind()) {
        case expr_kind::MetaVar:
            if (m_menv) {
                if (m_menv->is_assigned(e))
                    return infer_type(*(m_menv->get_subst(e)), ctx);
                else
                    return m_menv->get_type(e);
            } else {
                throw unexpected_metavar_occurrence(m_env, e);
            }
        case expr_kind::Constant: {
            if (const_type(e)) {
                return *const_type(e);
            } else {
                object const & obj = m_env->get_object(const_name(e));
                if (obj.has_type())
                    return obj.get_type();
                else
                    throw has_no_type_exception(m_env, e);
            }
            break;
        }
        case expr_kind::Var: {
            auto p = lookup_ext(ctx, var_idx(e));
            context_entry const & ce = p.first;
            if (ce.get_domain()) {
                context const & ce_ctx   = p.second;
                return lift_free_vars(*(ce.get_domain()), ctx.size() - ce_ctx.size());
            }
            // Remark: the case where ce.get_domain() is not
            // available is not considered cheap.
            break;
        }
        case expr_kind::Eq:
            return mk_bool_type();
        case expr_kind::Value:
            return to_value(e).get_type();
        case expr_kind::Type:
            return mk_type(ty_level(e) + 1);
        case expr_kind::App: case expr_kind::Lambda:
        case expr_kind::Pi:  case expr_kind::Let:
            break; // expensive cases
        }

        check_system("type inference");
        bool shared = false;
        if (is_shared(e)) {
            shared = true;
            auto it = m_cache.find(e);
            if (it != m_cache.end())
                return it->second;
        }

        expr r;
        switch (e.kind()) {
        case expr_kind::Constant: case expr_kind::Eq:
        case expr_kind::Value:    case expr_kind::Type:
        case expr_kind::MetaVar:
            lean_unreachable(); // LCOV_EXCL_LINE
        case expr_kind::Var: {
            auto p = lookup_ext(ctx, var_idx(e));
            context_entry const & ce = p.first;
            context const & ce_ctx   = p.second;
            lean_assert(!ce.get_domain());
            r = lift_free_vars(infer_type(*(ce.get_body()), ce_ctx), ctx.size() - ce_ctx.size());
            break;
        }
        case expr_kind::App: {
            expr const & f = arg(e, 0);
            expr f_t = infer_type(f, ctx);
            r = get_range(f_t, e, ctx);
            break;
        }
        case expr_kind::Lambda: {
            freset<cache> reset(m_cache);
            r = mk_pi(abst_name(e), abst_domain(e), infer_type(abst_body(e), extend(ctx, abst_name(e), abst_domain(e))));
            break;
        }
        case expr_kind::Pi: {
            expr t1  = check_type(infer_type(abst_domain(e), ctx), abst_domain(e), ctx);
            expr t2;
            context new_ctx = extend(ctx, abst_name(e), abst_domain(e));
            {
                freset<cache> reset(m_cache);
                t2 = check_type(infer_type(abst_body(e), new_ctx), abst_body(e), new_ctx);
            }
            if (is_type(t1) && is_type(t2)) {
                r = mk_type(max(ty_level(t1), ty_level(t2)));
            } else {
                lean_assert(m_uc);
                justification jst = mk_max_type_justification(ctx, e);
                r = m_menv->mk_metavar(ctx);
                m_uc->push_back(mk_max_constraint(new_ctx, lift_free_vars(t1, 0, 1), t2, r, jst));
            }
            break;
        }
        case expr_kind::Let: {
            freset<cache> reset(m_cache);
            r = infer_type(let_body(e), extend(ctx, let_name(e), let_type(e), let_value(e)));
            break;
        }}

        if (shared) {
            m_cache[e] = r;
        }
        return r;
    }

    void set_ctx(context const & ctx) {
        if (!is_eqp(m_ctx, ctx)) {
            clear();
            m_ctx = ctx;
        }
    }

public:
    imp(ro_environment const & env):
        m_env(env),
        m_normalizer(env) {
        m_uc = nullptr;
    }

    expr operator()(expr const & e, context const & ctx, optional<metavar_env> const & menv, buffer<unification_constraint> * uc) {
        set_ctx(ctx);
        if (m_menv.update(menv))
            clear_cache();
        flet<unification_constraints*> set(m_uc, uc);
        return infer_type(e, ctx);
    }

    void clear_cache() {
        m_cache.clear();
        m_normalizer.clear();
    }

    void clear() {
        clear_cache();
        m_menv.clear();
        m_ctx = context();
    }

    bool is_proposition(expr const & e, context const & ctx, optional<metavar_env> const & menv) {
        // Catch easy cases
        switch (e.kind()) {
        case expr_kind::Lambda: case expr_kind::Pi: case expr_kind::Type: return false;
        case expr_kind::Eq: return true;
        default: break;
        }
        expr t = operator()(e, ctx, menv, nullptr);
        if (is_bool(t))
            return true;
        else
            return is_bool(normalize(t, ctx));
    }
};
type_inferer::type_inferer(ro_environment const & env):m_ptr(new imp(env)) {}
type_inferer::~type_inferer() {}
expr type_inferer::operator()(expr const & e, context const & ctx, optional<metavar_env> const & menv, buffer<unification_constraint> * uc) {
    return m_ptr->operator()(e, ctx, menv, uc);
}
expr type_inferer::operator()(expr const & e, context const & ctx, metavar_env const & menv, buffer<unification_constraint> & uc) {
    return m_ptr->operator()(e, ctx, some_menv(menv), &uc);
}
expr type_inferer::operator()(expr const & e, context const & ctx) {
    return operator()(e, ctx, none_menv(), nullptr);
}
bool type_inferer::is_proposition(expr const & e, context const & ctx, optional<metavar_env> const & menv) {
    return m_ptr->is_proposition(e, ctx, menv);
}
bool type_inferer::is_proposition(expr const & e, context const & ctx) {
    return is_proposition(e, ctx, none_menv());
}
bool type_inferer::is_proposition(expr const & e, context const & ctx, metavar_env const & menv) {
    return is_proposition(e, ctx, some_menv(menv));
}
void type_inferer::clear() { m_ptr->clear(); }

constexpr char const * type_inferer_mt = "type_inferer";
type_inferer & to_type_inferer(lua_State * L, int i) { return *static_cast<type_inferer*>(luaL_checkudata(L, i, type_inferer_mt)); }
DECL_PRED(type_inferer)
DECL_GC(type_inferer)

static int type_inferer_call(lua_State * L) {
    int nargs = lua_gettop(L);
    type_inferer & inferer = to_type_inferer(L, 1);
    if (nargs == 2)
        return push_expr(L, inferer(to_expr(L, 2)));
    else
        return push_expr(L, inferer(to_expr(L, 2), to_context(L, 3)));
}

static int type_inferer_clear(lua_State * L) {
    to_type_inferer(L, 1).clear();
    return 0;
}

static int mk_type_inferer(lua_State * L) {
    void * mem = lua_newuserdata(L, sizeof(type_inferer));
    new (mem) type_inferer(to_environment(L, 1));
    luaL_getmetatable(L, type_inferer_mt);
    lua_setmetatable(L, -2);
    return 1;
}

static const struct luaL_Reg type_inferer_m[] = {
    {"__gc",            type_inferer_gc}, // never throws
    {"__call",          safe_function<type_inferer_call>},
    {"clear",           safe_function<type_inferer_clear>},
    {0, 0}
};

void open_type_inferer(lua_State * L) {
    luaL_newmetatable(L, type_inferer_mt);
    lua_pushvalue(L, -1);
    lua_setfield(L, -2, "__index");
    setfuncs(L, type_inferer_m, 0);

    SET_GLOBAL_FUN(mk_type_inferer,          "type_inferer");
    SET_GLOBAL_FUN(type_inferer_pred,        "is_type_inferer");
}
}
<|endoftext|>


<commit_before>//
// This file is part of the Marble Desktop Globe.
//
// This program is free software licensed under the GNU LGPL. You can
// find a copy of this license in LICENSE.txt in the top directory of
// the source code.
//
// Copyright 2006-2007 Torsten Rahn <tackat@kde.org>"
// Copyright 2007      Inge Wallin  <ingwa@kde.org>"
// Copyright 2008      Carlos Licea <carlos.licea@kdemail.net>
//

#include "TextureColorizer.h"

#include <cmath>

#include <QtCore/QDebug>
#include <QtCore/QFile>
#include <QtCore/QString>
#include <QtCore/QTime>
#include <QtGui/QColor>
#include <QtGui/QImage>
#include <QtGui/QPainter>

#include "global.h"
#include "GeoSceneDocument.h"
#include "GeoSceneSettings.h"
#include "ViewParams.h"
#include "ViewportParams.h"
#include "AbstractProjection.h"
#include "MathHelper.h"

using namespace Marble;

uint TextureColorizer::texturepalette[16][512];

TextureColorizer::TextureColorizer( const QString& seafile,
                                    const QString& landfile )
{
    QTime t;
    t.start();
    generatePalette(seafile, landfile);
    qDebug("TextureColorizer: Time elapsed: %d ms", t.elapsed());
}

QString TextureColorizer::seafile() const
{
    return m_seafile;
}

QString TextureColorizer::landfile() const
{
    return m_landfile;
}

// This function takes two images, both in viewParams:
//  - The coast image, which has a number of colors where each color
//    represents a sort of terrain (ex: land/sea)
//  - The canvas image, which has a gray scale image, often
//    representing a height field.
//
// It then uses the values of the pixels in the coast image to select
// a color map.  The value of the pixel in the canvas image is used as
// an index into the selected color map and the resulting color is
// written back to the canvas image.  This way we can have different
// color schemes for land and water.
//
// In addition to this, a simple form of bump mapping is performed to
// increase the illusion of height differences (see the variable
// showRelief).
//

void TextureColorizer::colorize(ViewParams *viewParams)
{
    QImage        *origimg  = viewParams->canvasImage();
    const QImage  *coastimg = viewParams->coastImage();
    const qint64   radius   = viewParams->radius();

    const int  imgheight = origimg->height();
    const int  imgwidth  = origimg->width();
    const int  imgrx     = imgwidth / 2;
    const int  imgry     = imgheight / 2;
    // This variable is not used anywhere..
    const int  imgradius = imgrx * imgrx + imgry * imgry;

    const uint landoffscreen = qRgb(255,0,0);
    // const uint seaoffscreen = qRgb(0,0,0);
    const uint lakeoffscreen = qRgb(0,0,0);
    // const uint glaciercolor = qRgb(200,200,200);

    int     bump = 8;
    GpFifo  emboss;
    emboss.buffer = 0;

    bool showRelief;
    viewParams->mapTheme()->settings()->propertyValue( "relief", showRelief );

    if ( radius * radius > imgradius
         || viewParams->projection() == Equirectangular
         || viewParams->projection() == Mercator )
    {
        int yTop = 0;
        int yBottom = imgheight;

        if( viewParams->projection() == Equirectangular
            || viewParams->projection() == Mercator )
        {
            // Calculate translation of center point
            qreal  centerLon;
            qreal  centerLat;
            viewParams->centerCoordinates( centerLon, centerLat );

            const float rad2Pixel = (qreal)( 2 * radius ) / M_PI;
            if ( viewParams->projection() == Equirectangular ) {
                int yCenterOffset = (int)( centerLat * rad2Pixel );
                yTop = ( imgry - radius + yCenterOffset < 0)? 0 : imgry - radius + yCenterOffset;
                yBottom = ( imgry + yCenterOffset + radius > imgheight )? imgheight : imgry + yCenterOffset + radius;
            }
            else if ( viewParams->projection() == Mercator ) {
                int yCenterOffset = (int)( asinh( tan( centerLat ) ) * rad2Pixel  );
                yTop = ( imgry - 2 * radius + yCenterOffset < 0 ) ? 0 : imgry - 2 * radius + yCenterOffset;
                yBottom = ( imgry + 2 * radius + yCenterOffset > imgheight )? imgheight : imgry + 2 * radius + yCenterOffset;
            }
        }

        const int itEnd = yBottom;

        for (int y = yTop; y < itEnd; ++y) {

            QRgb  *writeData         = (QRgb*)( origimg->scanLine( y ) );
            const QRgb  *coastData   = (QRgb*)( coastimg->scanLine( y ) );

            uchar *readDataStart     = origimg->scanLine( y );
            const uchar *readDataEnd = readDataStart + imgwidth*4;

            emboss.buffer = 0;

            for ( uchar* readData = readDataStart;
                  readData < readDataEnd;
                  readData += 4, ++writeData, ++coastData )
            {

                // Cheap Emboss / Bumpmapping
                uchar&  grey = *readData; // qBlue(*data);

                if ( showRelief ) {
                    emboss.gpuint.x4 = grey;
                    emboss.buffer = emboss.buffer >> 8;
                    bump = ( emboss.gpuint.x1 + 8 - grey );
                    if ( bump  < 0 )  bump = 0;
                    if ( bump  > 15 ) bump = 15;
                }

                int alpha = qRed( *coastData );
                if ( alpha == 255 || alpha == 0 ) {
                    if ( *coastData == landoffscreen )
                        *writeData = texturepalette[bump][grey + 0x100];
                    else {
                        if (*coastData == lakeoffscreen)
                            *writeData = texturepalette[bump][0x055];
                        else {
                            *writeData = texturepalette[bump][grey];
                        }
                    }
                }
                else {
                    qreal c = 1.0 / 255.0;

                    if ( qRed( *coastData ) != 0 && qGreen( *coastData ) == 0) {

                        QRgb landcolor  = (QRgb)(texturepalette[bump][grey + 0x100]);
                        QRgb watercolor = (QRgb)(texturepalette[bump][grey]);

                        *writeData = qRgb(
                            (int) ( c * ( alpha * qRed( landcolor )
                            + ( 255 - alpha ) * qRed( watercolor ) ) ),
                            (int) ( c * ( alpha * qGreen( landcolor )
                            + ( 255 - alpha ) * qGreen( watercolor ) ) ),
                            (int) ( c * ( alpha * qBlue( landcolor )
                            + ( 255 - alpha ) * qBlue( watercolor ) ) )
                        );
                    }
                    else {

                        if ( qGreen( *coastData ) != 0 ) {

                            QRgb landcolor  = (QRgb)(texturepalette[bump][grey + 0x100]);
                            QRgb glaciercolor = (QRgb)(texturepalette[bump][grey]);

                            *writeData = qRgb(
                                (int) ( c * ( alpha * qRed( glaciercolor )
                                + ( 255 - alpha ) * qRed( landcolor ) ) ),
                                (int) ( c * ( alpha * qGreen( glaciercolor )
                                + ( 255 - alpha ) * qGreen( landcolor ) ) ),
                                (int) ( c * ( alpha * qBlue( glaciercolor )
                                + ( 255 - alpha ) * qBlue( landcolor ) ) )
                            );
                        }
                    }
                }
            }
        }
    }
    else {
        int yTop    = ( imgry-radius < 0 ) ? 0 : imgry-radius;
        const int yBottom = ( yTop == 0 ) ? imgheight : imgry + radius;

        for ( int y = yTop; y < yBottom; ++y ) {
            const int  dy = imgry - y;
            int  rx = (int)sqrt( (qreal)( radius * radius - dy * dy ) );
            int  xLeft  = 0;
            int  xRight = imgwidth;

            if ( imgrx-rx > 0 ) {
                xLeft  = imgrx - rx;
                xRight = imgrx + rx;
            }

            QRgb  *writeData         = (QRgb*)( origimg->scanLine( y ) )  + xLeft;
            const QRgb *coastData    = (QRgb*)( coastimg->scanLine( y ) ) + xLeft;

            uchar *readDataStart     = origimg->scanLine( y ) + xLeft * 4;
            const uchar *readDataEnd = origimg->scanLine( y ) + xRight * 4;

            for ( uchar* readData = readDataStart;
                  readData < readDataEnd;
                  readData += 4, ++writeData, ++coastData )
            {
                // Cheap Embosss / Bumpmapping

                uchar& grey = *readData; // qBlue(*data);

                if ( showRelief == true ) {
                    emboss.buffer = emboss.buffer >> 8;
                    emboss.gpuint.x4 = grey;
                    bump = ( emboss.gpuint.x1 + 16 - grey ) >> 1;
                    if ( bump > 15 ) bump = 15;
                    if ( bump < 0 )  bump = 0;
                }

                int alpha = qRed( *coastData );
                if ( alpha == 255 || alpha == 0 ) {
                    if ( *coastData == landoffscreen )
                        *writeData = texturepalette[bump][grey + 0x100];
                    else {
                        if (*coastData == lakeoffscreen)
                            *writeData = texturepalette[bump][0x055];
                        else {
                            *writeData = texturepalette[bump][grey];
                        }
                    }
                }
                else {
                    qreal c = 1.0 / 255.0;

                    if ( qRed( *coastData ) != 0 && qGreen( *coastData ) == 0) {

                        QRgb landcolor  = (QRgb)(texturepalette[bump][grey + 0x100]);
                        QRgb watercolor = (QRgb)(texturepalette[bump][grey]);

                        *writeData = qRgb(
                            (int) ( c * ( alpha * qRed( landcolor )
                            + ( 255 - alpha ) * qRed( watercolor ) ) ),
                            (int) ( c * ( alpha * qGreen( landcolor )
                            + ( 255 - alpha ) * qGreen( watercolor ) ) ),
                            (int) ( c * ( alpha * qBlue( landcolor )
                            + ( 255 - alpha ) * qBlue( watercolor ) ) )
                        );
                    }
                    else {

                        if ( qGreen( *coastData ) != 0 ) {

                            QRgb landcolor  = (QRgb)(texturepalette[bump][grey + 0x100]);
                            QRgb glaciercolor = (QRgb)(texturepalette[bump][grey]);

                            *writeData = qRgb(
                                (int) ( c * ( alpha * qRed( glaciercolor )
                                + ( 255 - alpha ) * qRed( landcolor ) ) ),
                                (int) ( c * ( alpha * qGreen( glaciercolor )
                                + ( 255 - alpha ) * qGreen( landcolor ) ) ),
                                (int) ( c * ( alpha * qBlue( glaciercolor )
                                + ( 255 - alpha ) * qBlue( landcolor ) ) )
                            );
                        }
                    }
                }
            }
        }
    }
}

void TextureColorizer::generatePalette(const QString& seafile,
                                       const QString& landfile)
{
    QImage   gradientImage ( 256, 3, QImage::Format_RGB32 );
    QPainter  gradientPainter;
    gradientPainter.begin( &gradientImage );
    gradientPainter.setPen( Qt::NoPen );

    int shadingStart = 120;
    QImage    shadingImage ( 16, 3, QImage::Format_RGB32 );
    QPainter  shadingPainter;
    shadingPainter.begin( &shadingImage );
    shadingPainter.setPen( Qt::NoPen );

    int offset = 0;

    QStringList  filelist;
    filelist << seafile << landfile;

    foreach ( const QString &filename, filelist ) {

        QLinearGradient  gradient( 0, 0, 256, 0 );

        QFile  file( filename );
        file.open( QIODevice::ReadOnly );
        QTextStream  stream( &file );  // read the data from the file

        QString      evalstrg;

        while ( !stream.atEnd() ) {
            stream >> evalstrg;
            if ( !evalstrg.isEmpty() && evalstrg.contains( '=' ) ) {
                QString  colorValue = evalstrg.left( evalstrg.indexOf( '=' ) );
                QString  colorPosition = evalstrg.mid( evalstrg.indexOf( '=' ) + 1 );
                gradient.setColorAt( colorPosition.toDouble(),
                                     QColor( colorValue ) );
            }
        }
        gradientPainter.setBrush( gradient );
        gradientPainter.drawRect( 0, 0, 256, 3 );

        QLinearGradient  shadeGradient( - shadingStart, 0, 256 - shadingStart, 0 );

        shadeGradient.setColorAt(0.00, QColor(Qt::white));
        shadeGradient.setColorAt(0.15, QColor(Qt::white));
        shadeGradient.setColorAt(0.75, QColor(Qt::black));
        shadeGradient.setColorAt(1.00, QColor(Qt::black));

        const QRgb * gradientScanLine  = (QRgb*)( gradientImage.scanLine( 1 ) );
        const QRgb * shadingScanLine   = (QRgb*)( shadingImage.scanLine( 1 ) );

        for ( int i = 0; i < 256; ++i ) {

            QRgb  shadeColor = *(gradientScanLine + i );
            shadeGradient.setColorAt(0.496, shadeColor);
            shadeGradient.setColorAt(0.504, shadeColor);
            shadingPainter.setBrush( shadeGradient );
            shadingPainter.drawRect( 0, 0, 16, 3 );

            // populate texturepalette[][]
            for ( int j = 0; j < 16; ++j ) {
                texturepalette[j][offset + i] = *(shadingScanLine + j );
            }
        }

        offset += 256;
    }
    shadingPainter.end();  // Need to explicitly tell painter lifetime to avoid crash
    gradientPainter.end(); // on some systems.

    m_seafile = seafile;
    m_landfile = landfile;
}
<commit_msg>Simplify bool expressions, do not compare bool values with "true", just use the value.<commit_after>//
// This file is part of the Marble Desktop Globe.
//
// This program is free software licensed under the GNU LGPL. You can
// find a copy of this license in LICENSE.txt in the top directory of
// the source code.
//
// Copyright 2006-2007 Torsten Rahn <tackat@kde.org>"
// Copyright 2007      Inge Wallin  <ingwa@kde.org>"
// Copyright 2008      Carlos Licea <carlos.licea@kdemail.net>
//

#include "TextureColorizer.h"

#include <cmath>

#include <QtCore/QDebug>
#include <QtCore/QFile>
#include <QtCore/QString>
#include <QtCore/QTime>
#include <QtGui/QColor>
#include <QtGui/QImage>
#include <QtGui/QPainter>

#include "global.h"
#include "GeoSceneDocument.h"
#include "GeoSceneSettings.h"
#include "ViewParams.h"
#include "ViewportParams.h"
#include "AbstractProjection.h"
#include "MathHelper.h"

using namespace Marble;

uint TextureColorizer::texturepalette[16][512];

TextureColorizer::TextureColorizer( const QString& seafile,
                                    const QString& landfile )
{
    QTime t;
    t.start();
    generatePalette(seafile, landfile);
    qDebug("TextureColorizer: Time elapsed: %d ms", t.elapsed());
}

QString TextureColorizer::seafile() const
{
    return m_seafile;
}

QString TextureColorizer::landfile() const
{
    return m_landfile;
}

// This function takes two images, both in viewParams:
//  - The coast image, which has a number of colors where each color
//    represents a sort of terrain (ex: land/sea)
//  - The canvas image, which has a gray scale image, often
//    representing a height field.
//
// It then uses the values of the pixels in the coast image to select
// a color map.  The value of the pixel in the canvas image is used as
// an index into the selected color map and the resulting color is
// written back to the canvas image.  This way we can have different
// color schemes for land and water.
//
// In addition to this, a simple form of bump mapping is performed to
// increase the illusion of height differences (see the variable
// showRelief).
//

void TextureColorizer::colorize(ViewParams *viewParams)
{
    QImage        *origimg  = viewParams->canvasImage();
    const QImage  *coastimg = viewParams->coastImage();
    const qint64   radius   = viewParams->radius();

    const int  imgheight = origimg->height();
    const int  imgwidth  = origimg->width();
    const int  imgrx     = imgwidth / 2;
    const int  imgry     = imgheight / 2;
    // This variable is not used anywhere..
    const int  imgradius = imgrx * imgrx + imgry * imgry;

    const uint landoffscreen = qRgb(255,0,0);
    // const uint seaoffscreen = qRgb(0,0,0);
    const uint lakeoffscreen = qRgb(0,0,0);
    // const uint glaciercolor = qRgb(200,200,200);

    int     bump = 8;
    GpFifo  emboss;
    emboss.buffer = 0;

    bool showRelief;
    viewParams->mapTheme()->settings()->propertyValue( "relief", showRelief );

    if ( radius * radius > imgradius
         || viewParams->projection() == Equirectangular
         || viewParams->projection() == Mercator )
    {
        int yTop = 0;
        int yBottom = imgheight;

        if( viewParams->projection() == Equirectangular
            || viewParams->projection() == Mercator )
        {
            // Calculate translation of center point
            qreal  centerLon;
            qreal  centerLat;
            viewParams->centerCoordinates( centerLon, centerLat );

            const float rad2Pixel = (qreal)( 2 * radius ) / M_PI;
            if ( viewParams->projection() == Equirectangular ) {
                int yCenterOffset = (int)( centerLat * rad2Pixel );
                yTop = ( imgry - radius + yCenterOffset < 0)? 0 : imgry - radius + yCenterOffset;
                yBottom = ( imgry + yCenterOffset + radius > imgheight )? imgheight : imgry + yCenterOffset + radius;
            }
            else if ( viewParams->projection() == Mercator ) {
                int yCenterOffset = (int)( asinh( tan( centerLat ) ) * rad2Pixel  );
                yTop = ( imgry - 2 * radius + yCenterOffset < 0 ) ? 0 : imgry - 2 * radius + yCenterOffset;
                yBottom = ( imgry + 2 * radius + yCenterOffset > imgheight )? imgheight : imgry + 2 * radius + yCenterOffset;
            }
        }

        const int itEnd = yBottom;

        for (int y = yTop; y < itEnd; ++y) {

            QRgb  *writeData         = (QRgb*)( origimg->scanLine( y ) );
            const QRgb  *coastData   = (QRgb*)( coastimg->scanLine( y ) );

            uchar *readDataStart     = origimg->scanLine( y );
            const uchar *readDataEnd = readDataStart + imgwidth*4;

            emboss.buffer = 0;

            for ( uchar* readData = readDataStart;
                  readData < readDataEnd;
                  readData += 4, ++writeData, ++coastData )
            {

                // Cheap Emboss / Bumpmapping
                uchar&  grey = *readData; // qBlue(*data);

                if ( showRelief ) {
                    emboss.gpuint.x4 = grey;
                    emboss.buffer = emboss.buffer >> 8;
                    bump = ( emboss.gpuint.x1 + 8 - grey );
                    if ( bump  < 0 )  bump = 0;
                    if ( bump  > 15 ) bump = 15;
                }

                int alpha = qRed( *coastData );
                if ( alpha == 255 || alpha == 0 ) {
                    if ( *coastData == landoffscreen )
                        *writeData = texturepalette[bump][grey + 0x100];
                    else {
                        if (*coastData == lakeoffscreen)
                            *writeData = texturepalette[bump][0x055];
                        else {
                            *writeData = texturepalette[bump][grey];
                        }
                    }
                }
                else {
                    qreal c = 1.0 / 255.0;

                    if ( qRed( *coastData ) != 0 && qGreen( *coastData ) == 0) {

                        QRgb landcolor  = (QRgb)(texturepalette[bump][grey + 0x100]);
                        QRgb watercolor = (QRgb)(texturepalette[bump][grey]);

                        *writeData = qRgb(
                            (int) ( c * ( alpha * qRed( landcolor )
                            + ( 255 - alpha ) * qRed( watercolor ) ) ),
                            (int) ( c * ( alpha * qGreen( landcolor )
                            + ( 255 - alpha ) * qGreen( watercolor ) ) ),
                            (int) ( c * ( alpha * qBlue( landcolor )
                            + ( 255 - alpha ) * qBlue( watercolor ) ) )
                        );
                    }
                    else {

                        if ( qGreen( *coastData ) != 0 ) {

                            QRgb landcolor  = (QRgb)(texturepalette[bump][grey + 0x100]);
                            QRgb glaciercolor = (QRgb)(texturepalette[bump][grey]);

                            *writeData = qRgb(
                                (int) ( c * ( alpha * qRed( glaciercolor )
                                + ( 255 - alpha ) * qRed( landcolor ) ) ),
                                (int) ( c * ( alpha * qGreen( glaciercolor )
                                + ( 255 - alpha ) * qGreen( landcolor ) ) ),
                                (int) ( c * ( alpha * qBlue( glaciercolor )
                                + ( 255 - alpha ) * qBlue( landcolor ) ) )
                            );
                        }
                    }
                }
            }
        }
    }
    else {
        int yTop    = ( imgry-radius < 0 ) ? 0 : imgry-radius;
        const int yBottom = ( yTop == 0 ) ? imgheight : imgry + radius;

        for ( int y = yTop; y < yBottom; ++y ) {
            const int  dy = imgry - y;
            int  rx = (int)sqrt( (qreal)( radius * radius - dy * dy ) );
            int  xLeft  = 0;
            int  xRight = imgwidth;

            if ( imgrx-rx > 0 ) {
                xLeft  = imgrx - rx;
                xRight = imgrx + rx;
            }

            QRgb  *writeData         = (QRgb*)( origimg->scanLine( y ) )  + xLeft;
            const QRgb *coastData    = (QRgb*)( coastimg->scanLine( y ) ) + xLeft;

            uchar *readDataStart     = origimg->scanLine( y ) + xLeft * 4;
            const uchar *readDataEnd = origimg->scanLine( y ) + xRight * 4;

            for ( uchar* readData = readDataStart;
                  readData < readDataEnd;
                  readData += 4, ++writeData, ++coastData )
            {
                // Cheap Embosss / Bumpmapping

                uchar& grey = *readData; // qBlue(*data);

                if ( showRelief ) {
                    emboss.buffer = emboss.buffer >> 8;
                    emboss.gpuint.x4 = grey;
                    bump = ( emboss.gpuint.x1 + 16 - grey ) >> 1;
                    if ( bump > 15 ) bump = 15;
                    if ( bump < 0 )  bump = 0;
                }

                int alpha = qRed( *coastData );
                if ( alpha == 255 || alpha == 0 ) {
                    if ( *coastData == landoffscreen )
                        *writeData = texturepalette[bump][grey + 0x100];
                    else {
                        if (*coastData == lakeoffscreen)
                            *writeData = texturepalette[bump][0x055];
                        else {
                            *writeData = texturepalette[bump][grey];
                        }
                    }
                }
                else {
                    qreal c = 1.0 / 255.0;

                    if ( qRed( *coastData ) != 0 && qGreen( *coastData ) == 0) {

                        QRgb landcolor  = (QRgb)(texturepalette[bump][grey + 0x100]);
                        QRgb watercolor = (QRgb)(texturepalette[bump][grey]);

                        *writeData = qRgb(
                            (int) ( c * ( alpha * qRed( landcolor )
                            + ( 255 - alpha ) * qRed( watercolor ) ) ),
                            (int) ( c * ( alpha * qGreen( landcolor )
                            + ( 255 - alpha ) * qGreen( watercolor ) ) ),
                            (int) ( c * ( alpha * qBlue( landcolor )
                            + ( 255 - alpha ) * qBlue( watercolor ) ) )
                        );
                    }
                    else {

                        if ( qGreen( *coastData ) != 0 ) {

                            QRgb landcolor  = (QRgb)(texturepalette[bump][grey + 0x100]);
                            QRgb glaciercolor = (QRgb)(texturepalette[bump][grey]);

                            *writeData = qRgb(
                                (int) ( c * ( alpha * qRed( glaciercolor )
                                + ( 255 - alpha ) * qRed( landcolor ) ) ),
                                (int) ( c * ( alpha * qGreen( glaciercolor )
                                + ( 255 - alpha ) * qGreen( landcolor ) ) ),
                                (int) ( c * ( alpha * qBlue( glaciercolor )
                                + ( 255 - alpha ) * qBlue( landcolor ) ) )
                            );
                        }
                    }
                }
            }
        }
    }
}

void TextureColorizer::generatePalette(const QString& seafile,
                                       const QString& landfile)
{
    QImage   gradientImage ( 256, 3, QImage::Format_RGB32 );
    QPainter  gradientPainter;
    gradientPainter.begin( &gradientImage );
    gradientPainter.setPen( Qt::NoPen );

    int shadingStart = 120;
    QImage    shadingImage ( 16, 3, QImage::Format_RGB32 );
    QPainter  shadingPainter;
    shadingPainter.begin( &shadingImage );
    shadingPainter.setPen( Qt::NoPen );

    int offset = 0;

    QStringList  filelist;
    filelist << seafile << landfile;

    foreach ( const QString &filename, filelist ) {

        QLinearGradient  gradient( 0, 0, 256, 0 );

        QFile  file( filename );
        file.open( QIODevice::ReadOnly );
        QTextStream  stream( &file );  // read the data from the file

        QString      evalstrg;

        while ( !stream.atEnd() ) {
            stream >> evalstrg;
            if ( !evalstrg.isEmpty() && evalstrg.contains( '=' ) ) {
                QString  colorValue = evalstrg.left( evalstrg.indexOf( '=' ) );
                QString  colorPosition = evalstrg.mid( evalstrg.indexOf( '=' ) + 1 );
                gradient.setColorAt( colorPosition.toDouble(),
                                     QColor( colorValue ) );
            }
        }
        gradientPainter.setBrush( gradient );
        gradientPainter.drawRect( 0, 0, 256, 3 );

        QLinearGradient  shadeGradient( - shadingStart, 0, 256 - shadingStart, 0 );

        shadeGradient.setColorAt(0.00, QColor(Qt::white));
        shadeGradient.setColorAt(0.15, QColor(Qt::white));
        shadeGradient.setColorAt(0.75, QColor(Qt::black));
        shadeGradient.setColorAt(1.00, QColor(Qt::black));

        const QRgb * gradientScanLine  = (QRgb*)( gradientImage.scanLine( 1 ) );
        const QRgb * shadingScanLine   = (QRgb*)( shadingImage.scanLine( 1 ) );

        for ( int i = 0; i < 256; ++i ) {

            QRgb  shadeColor = *(gradientScanLine + i );
            shadeGradient.setColorAt(0.496, shadeColor);
            shadeGradient.setColorAt(0.504, shadeColor);
            shadingPainter.setBrush( shadeGradient );
            shadingPainter.drawRect( 0, 0, 16, 3 );

            // populate texturepalette[][]
            for ( int j = 0; j < 16; ++j ) {
                texturepalette[j][offset + i] = *(shadingScanLine + j );
            }
        }

        offset += 256;
    }
    shadingPainter.end();  // Need to explicitly tell painter lifetime to avoid crash
    gradientPainter.end(); // on some systems.

    m_seafile = seafile;
    m_landfile = landfile;
}
<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Int32.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>

#define KEYCODE_RA 0x43
#define KEYCODE_LA 0x44
#define KEYCODE_UA 0x41
#define KEYCODE_DA 0x42
#define KEYCODE_Q 0x71
#define KEYCODE_W 0x77
#define KEYCODE_S 0x73
#define KEYCODE_A 0x61
#define KEYCODE_D 0x64
#define KEYCODE_T 0x74
#define KEYCODE_L 0x6C
#define KEYCODE_P 0x70
#define KEYCODE_O 0x6F
#define KEYCODE_I 0x69
#define KEYCODE_C 0x63

#define POSITION_MODE 0
#define ORIENTATION_MODE 1
#define PID_TUNING_MODE 2

#define WAITING 0
#define TAKE_OFF 1
#define POS_CTRL 2
#define LAND 3
#define EMERGENCY 4
#define Z_TUNE 5
#define Y_TUNE 6
#define X_TUNE 7
#define CALIBRATE 8

#define KP_Z_INIT 250000.0
#define KI_Z_INIT 0.0
#define KD_Z_INIT 200000.0

#define KP_Z_OFFSET 1000.0
#define KI_Z_OFFSET 100.0
#define KD_Z_OFFSET 1000.0

#define KP_XY_INIT 0.0
#define KI_XY_INIT 0.0
#define KD_XY_INIT 0.0

#define KP_XY_OFFSET 1.0
#define KI_XY_OFFSET 1.0
#define KD_XY_OFFSET 1.0

using namespace geometry_msgs;
using namespace std;

class TeleopCrazyflie
 {

 public:
   TeleopCrazyflie();
   void keyLoop();

 private:

   ros::NodeHandle nh_;
   double roll, pitch, yawrate, thrust, goal_x, goal_y, goal_z, kp, ki, kd;
   int mode, state, tune_param, prev_tune_param;
   ros::Publisher vel_pub_, cmd_pub_, state_pub_;

 };

TeleopCrazyflie::TeleopCrazyflie():
  roll(0.0),
  pitch(0.0),
  yawrate(0.0),
  thrust(32767.0),
  mode(ORIENTATION_MODE),
  state(WAITING),
  goal_x(0.0),
  goal_y(0.0),
  goal_z(0.15),
  kp(KP_Z_INIT),
  kd(KD_Z_INIT),
  ki(KI_Z_INIT),
  tune_param(Z_TUNE),
  prev_tune_param(Z_TUNE)
{
  vel_pub_ =  nh_.advertise<geometry_msgs::Twist>("crazyflie/deep_learning/cmd_vel", 1);
  state_pub_ =  nh_.advertise<std_msgs::Int32>("crazyflie/deep_learning/cmd_state", 1);
  cmd_pub_ =  nh_.advertise<geometry_msgs::Twist>("crazyflie/deep_learning/cmd_pos", 1);

}

int kfd = 0;
struct termios cooked, raw;

void quit(int sig)
{
   tcsetattr(kfd, TCSANOW, &cooked);
   ros::shutdown();
   exit(0);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "crazyflie_teleop_node");
  TeleopCrazyflie teleop_crazyflie;

  signal(SIGINT,quit);

  teleop_crazyflie.keyLoop();

  return(0);
}

void TeleopCrazyflie::keyLoop()
{
  char c;
  bool dirty=false;

  // get the console in raw mode
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);
  double thrust_offset,roll_offset,pitch_offset,yawrate_offset;
  double goal_x_offset,goal_y_offset,goal_z_offset;
  double kp_offset, kd_offset, ki_offset;

  puts("Reading from keyboard");
  puts("---------------------------");
  puts("Use arrow keys to move the crazyflie.");

  puts("P - Position Mode");
  puts("O - Orientation Mode");
  puts("I - PID Tuning Mode");
  puts("C - Calibrate");

  for(;;)
  {
    // get the next event from the keyboard
    if(read(kfd, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }

   ROS_INFO("value: 0x%02X\n", c);

   thrust_offset = 1000 ;
   roll_offset = 1.0;
   yawrate_offset = 1.0;
   pitch_offset = 1.0;
   goal_z_offset = 0.05;
   goal_y_offset = 0.05;
   goal_x_offset = 0.05;

   kp_offset = KP_Z_OFFSET;
   kd_offset = KD_Z_OFFSET;
   ki_offset = KI_Z_OFFSET;

   if( tune_param == X_TUNE || tune_param == Y_TUNE)
   {
	   kp_offset = KP_XY_OFFSET;
	   kd_offset = KD_XY_OFFSET;
	   ki_offset = KI_XY_OFFSET;
   }

   //ros::Duration(0.05).sleep();

    switch(c)
    {
      case KEYCODE_T:
        ROS_INFO("TAKING OFF");
	if(mode == POSITION_MODE)
		state = TAKE_OFF;
	else
		ROS_INFO("To Take Off, Enable Position Mode");
	dirty = true;
        break;

      case KEYCODE_C:
        ROS_INFO("CALIBRATE");
	state = CALIBRATE;
	dirty = true;
        break;

      case KEYCODE_L:
        ROS_INFO("LAND");
	if(mode == POSITION_MODE)
		state = LAND;
	else
		ROS_INFO("To Land, Enable Position Mode");
	dirty = true;
        break;

      case KEYCODE_P:
        ROS_INFO("POSITION MODE");
        mode = POSITION_MODE;
	state = WAITING;
	dirty = true;
        break;

      case KEYCODE_O:
        ROS_INFO("ORIENTATION MODE");
	state = WAITING;
        mode = ORIENTATION_MODE;
	dirty = true;
        break;

      case KEYCODE_I:
        ROS_INFO("PID TUNING MODE");
        mode = PID_TUNING_MODE;
	state = tune_param;
	goal_x = 0.0;
	goal_y = 0.0;
	goal_z = 0.2;
	dirty = true;
        break;

      case KEYCODE_Q:
        ROS_INFO("STOP");
	state = EMERGENCY;
        thrust = 0;
	roll = 0;
	pitch = 0;
        yawrate = 0;
        dirty = true;
        break;

      case KEYCODE_LA:
        ROS_DEBUG("LEFT");

	if(mode == ORIENTATION_MODE)
        	roll -= roll_offset;
	else if(mode == POSITION_MODE)
	        goal_y -= goal_y_offset;
	else
	        kd -= kd_offset;

	dirty = true;
        break;

      case KEYCODE_RA:
        ROS_DEBUG("RIGHT");
	if(mode == ORIENTATION_MODE)
        	roll += roll_offset;
	else if(mode == POSITION_MODE)
	        goal_y += goal_y_offset;
	else
	        kd += kd_offset;

	dirty = true;
        break;

      case KEYCODE_UA:
        ROS_DEBUG("UP");
	if(mode == ORIENTATION_MODE)
        	pitch += pitch_offset;

	else if(mode == POSITION_MODE)
	        goal_x += goal_x_offset;

	else
	        kp += kp_offset;

	dirty = true;
        break;

      case KEYCODE_DA:

        ROS_DEBUG("DOWN");
	if(mode == ORIENTATION_MODE)
       		pitch -= pitch_offset;

	else if(mode == POSITION_MODE)
	        goal_x -= goal_x_offset;

	else
	        kp -= kp_offset;

	dirty = true;
        break;

      case KEYCODE_A:
        ROS_DEBUG("LEFT_A");
        yawrate -= yawrate_offset;
	if(mode == PID_TUNING_MODE)
	{
		if(tune_param == Z_TUNE)
	        	tune_param = Z_TUNE;
		else
			tune_param=  tune_param-1;

		state = tune_param;

		if(tune_param != prev_tune_param )
		{
			if(tune_param == Z_TUNE)
			{
				kp = KP_Z_INIT;
				kd = KD_Z_INIT;
				ki = KI_Z_INIT;
			}

			if(tune_param == X_TUNE || tune_param == Y_TUNE)
			{
				kp = KP_XY_INIT;
				kd = KD_XY_INIT;
				ki = KI_XY_INIT;
			}

		}

		prev_tune_param = tune_param;
	}
        break;

	dirty = true;
      case KEYCODE_D:
        ROS_DEBUG("RIGHT_D");
        yawrate += yawrate_offset;
	if(mode == PID_TUNING_MODE)
	{
		if(tune_param == X_TUNE)
	        	tune_param = X_TUNE;
		else
			tune_param = tune_param + 1;

		state = tune_param;

		if(tune_param != prev_tune_param )
		{
			if(tune_param == Z_TUNE)
			{
				kp = KP_Z_INIT;
				kd = KD_Z_INIT;
				ki = KI_Z_INIT;
			}

			if(tune_param == X_TUNE || tune_param == Y_TUNE)
			{
				kp = KP_XY_INIT;
				kd = KD_XY_INIT;
				ki = KI_XY_INIT;
			}

		}

		prev_tune_param = tune_param;
	}
	dirty = true;
        break;

      case KEYCODE_W:
        ROS_DEBUG("UP_W");
	if(mode == ORIENTATION_MODE)
        	thrust += thrust_offset;

	else if(mode == POSITION_MODE)
	        goal_z += goal_z_offset;

	else
	        ki += ki_offset;

	dirty = true;
        break;

      case KEYCODE_S:
        ROS_DEBUG("DOWN_S");
	if(mode == ORIENTATION_MODE)
        	thrust -= thrust_offset;
	else if(mode == POSITION_MODE)
	        goal_z -= goal_z_offset;
	else
	        ki -= ki_offset;

	dirty = true;
        break;

    }

    geometry_msgs::Twist twist;
    std_msgs::Int32 state_msg;

    twist.angular.y = yawrate;

    if(mode == ORIENTATION_MODE)
    {
	    twist.linear.z = thrust;
	    twist.linear.x = pitch;
	    twist.linear.y = roll;
    }

    else
    {
	    twist.linear.z = goal_z;
	    twist.linear.x = goal_x;
	    twist.linear.y = goal_y;

	    if(mode == PID_TUNING_MODE)
		{
		    twist.angular.x = kp;
		    twist.angular.y = ki;
		    twist.angular.z = kd;
		}

    }

    if(dirty ==true)
    {
      thrust_offset = 0.0;
      roll_offset = 0.0;
      yawrate_offset = 0.0;
      pitch_offset = 0.0;
      goal_x_offset = 0.0;
      goal_y_offset = 0.0;
      goal_z_offset = 0.0;
      kp_offset = 0.0;
      kd_offset = 0.0;
      ki_offset = 0.0;

      dirty=false;

    }

      if(mode == ORIENTATION_MODE)
      {
      	vel_pub_.publish(twist);
      }

      else
     {
        cmd_pub_.publish(twist);
     }

	state_msg.data = state;
        state_pub_.publish(state_msg);

   if(mode == POSITION_MODE)
	ROS_INFO("X - %f, Y - %f ,Z - %f,yawrate - %f", goal_x ,goal_y, goal_z,yawrate);
   else if(mode == ORIENTATION_MODE)
   	ROS_INFO("Thrust - %f, roll - %f ,pitch - %f,yawrate - %f",thrust,roll,pitch,yawrate);
   else
	ROS_INFO("Tune Param = %d, Kp - %f, Ki - %f , Kd - %f",tune_param,kp,ki,kd);

  }
  return;
}

<commit_msg>Make sure teleop node doesn't publish cmd during Calibration<commit_after>#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Int32.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>

#define KEYCODE_RA 0x43
#define KEYCODE_LA 0x44
#define KEYCODE_UA 0x41
#define KEYCODE_DA 0x42
#define KEYCODE_Q 0x71
#define KEYCODE_W 0x77
#define KEYCODE_S 0x73
#define KEYCODE_A 0x61
#define KEYCODE_D 0x64
#define KEYCODE_T 0x74
#define KEYCODE_L 0x6C
#define KEYCODE_P 0x70
#define KEYCODE_O 0x6F
#define KEYCODE_I 0x69
#define KEYCODE_C 0x63

#define POSITION_MODE 0
#define ORIENTATION_MODE 1
#define PID_TUNING_MODE 2

#define WAITING 0
#define TAKE_OFF 1
#define POS_CTRL 2
#define LAND 3
#define EMERGENCY 4
#define Z_TUNE 5
#define Y_TUNE 6
#define X_TUNE 7
#define CALIBRATE 8

#define KP_Z_INIT 250000.0
#define KI_Z_INIT 0.0
#define KD_Z_INIT 200000.0

#define KP_Z_OFFSET 1000.0
#define KI_Z_OFFSET 100.0
#define KD_Z_OFFSET 1000.0

#define KP_XY_INIT 0.0
#define KI_XY_INIT 0.0
#define KD_XY_INIT 0.0

#define KP_XY_OFFSET 0.05
#define KI_XY_OFFSET 0.05
#define KD_XY_OFFSET 0.05

using namespace geometry_msgs;
using namespace std;

class TeleopCrazyflie
 {

 public:
   TeleopCrazyflie();
   void keyLoop();

 private:

   ros::NodeHandle nh_;
   double roll, pitch, yawrate, thrust, goal_x, goal_y, goal_z, kp, ki, kd;
   int mode, state, tune_param, prev_tune_param;
   ros::Publisher vel_pub_, cmd_pub_, state_pub_;

 };

TeleopCrazyflie::TeleopCrazyflie():
  roll(0.0),
  pitch(0.0),
  yawrate(0.0),
  thrust(32767.0),
  mode(ORIENTATION_MODE),
  state(WAITING),
  goal_x(0.0),
  goal_y(0.0),
  goal_z(0.15),
  kp(KP_Z_INIT),
  kd(KD_Z_INIT),
  ki(KI_Z_INIT),
  tune_param(Z_TUNE),
  prev_tune_param(Z_TUNE)
{
  vel_pub_ =  nh_.advertise<geometry_msgs::Twist>("crazyflie/deep_learning/cmd_vel", 1);
  state_pub_ =  nh_.advertise<std_msgs::Int32>("crazyflie/deep_learning/cmd_state", 1);
  cmd_pub_ =  nh_.advertise<geometry_msgs::Twist>("crazyflie/deep_learning/cmd_pos", 1);

}

int kfd = 0;
struct termios cooked, raw;

void quit(int sig)
{
   tcsetattr(kfd, TCSANOW, &cooked);
   ros::shutdown();
   exit(0);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "crazyflie_teleop_node");
  TeleopCrazyflie teleop_crazyflie;

  signal(SIGINT,quit);

  teleop_crazyflie.keyLoop();

  return(0);
}

void TeleopCrazyflie::keyLoop()
{
  char c;
  bool dirty=false;

  // get the console in raw mode
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);
  double thrust_offset,roll_offset,pitch_offset,yawrate_offset;
  double goal_x_offset,goal_y_offset,goal_z_offset;
  double kp_offset, kd_offset, ki_offset;

  puts("Reading from keyboard");
  puts("---------------------------");
  puts("Use arrow keys to move the crazyflie.");

  puts("P - Position Mode");
  puts("O - Orientation Mode");
  puts("I - PID Tuning Mode");
  puts("C - Calibrate");

  for(;;)
  {
    // get the next event from the keyboard
    if(read(kfd, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }

   ROS_INFO("value: 0x%02X\n", c);

   thrust_offset = 1000 ;
   roll_offset = 1.0;
   yawrate_offset = 1.0;
   pitch_offset = 1.0;
   goal_z_offset = 0.05;
   goal_y_offset = 0.05;
   goal_x_offset = 0.05;

   kp_offset = KP_Z_OFFSET;
   kd_offset = KD_Z_OFFSET;
   ki_offset = KI_Z_OFFSET;

   if( tune_param == X_TUNE || tune_param == Y_TUNE)
   {
	   kp_offset = KP_XY_OFFSET;
	   kd_offset = KD_XY_OFFSET;
	   ki_offset = KI_XY_OFFSET;
   }

   //ros::Duration(0.05).sleep();

    switch(c)
    {
      case KEYCODE_T:
        ROS_INFO("TAKING OFF");
	if(mode == POSITION_MODE)
		state = TAKE_OFF;
	else
		ROS_INFO("To Take Off, Enable Position Mode");
	dirty = true;
        break;

      case KEYCODE_C:
        ROS_INFO("CALIBRATE");
	state = CALIBRATE;
	dirty = true;
        break;

      case KEYCODE_L:
        ROS_INFO("LAND");
	if(mode == POSITION_MODE)
		state = LAND;
	else
		ROS_INFO("To Land, Enable Position Mode");
	dirty = true;
        break;

      case KEYCODE_P:
        ROS_INFO("POSITION MODE");
        mode = POSITION_MODE;
	state = WAITING;
	dirty = true;
        break;

      case KEYCODE_O:
        ROS_INFO("ORIENTATION MODE");
	state = WAITING;
        mode = ORIENTATION_MODE;
	dirty = true;
        break;

      case KEYCODE_I:
        ROS_INFO("PID TUNING MODE");
        mode = PID_TUNING_MODE;
	state = tune_param;
	goal_x = 0.0;
	goal_y = 0.0;
	goal_z = 0.2;
	dirty = true;
        break;

      case KEYCODE_Q:
        ROS_INFO("STOP");
	state = EMERGENCY;
        thrust = 0;
	roll = 0;
	pitch = 0;
        yawrate = 0;
        dirty = true;
        break;

      case KEYCODE_LA:
        ROS_DEBUG("LEFT");

	if(mode == ORIENTATION_MODE)
        	roll -= roll_offset;
	else if(mode == POSITION_MODE)
	        goal_y -= goal_y_offset;
	else
	        kd -= kd_offset;

	dirty = true;
        break;

      case KEYCODE_RA:
        ROS_DEBUG("RIGHT");
	if(mode == ORIENTATION_MODE)
        	roll += roll_offset;
	else if(mode == POSITION_MODE)
	        goal_y += goal_y_offset;
	else
	        kd += kd_offset;

	dirty = true;
        break;

      case KEYCODE_UA:
        ROS_DEBUG("UP");
	if(mode == ORIENTATION_MODE)
        	pitch += pitch_offset;

	else if(mode == POSITION_MODE)
	        goal_x += goal_x_offset;

	else
	        kp += kp_offset;

	dirty = true;
        break;

      case KEYCODE_DA:

        ROS_DEBUG("DOWN");
	if(mode == ORIENTATION_MODE)
       		pitch -= pitch_offset;

	else if(mode == POSITION_MODE)
	        goal_x -= goal_x_offset;

	else
	        kp -= kp_offset;

	dirty = true;
        break;

      case KEYCODE_A:
        ROS_DEBUG("LEFT_A");
        yawrate -= yawrate_offset;
	if(mode == PID_TUNING_MODE)
	{
		if(tune_param == Z_TUNE)
	        	tune_param = Z_TUNE;
		else
			tune_param=  tune_param-1;

		state = tune_param;

		if(tune_param != prev_tune_param )
		{
			if(tune_param == Z_TUNE)
			{
				kp = KP_Z_INIT;
				kd = KD_Z_INIT;
				ki = KI_Z_INIT;
			}

			if(tune_param == X_TUNE || tune_param == Y_TUNE)
			{
				kp = KP_XY_INIT;
				kd = KD_XY_INIT;
				ki = KI_XY_INIT;
			}

		}

		prev_tune_param = tune_param;
	}
        break;

	dirty = true;
      case KEYCODE_D:
        ROS_DEBUG("RIGHT_D");
        yawrate += yawrate_offset;
	if(mode == PID_TUNING_MODE)
	{
		if(tune_param == X_TUNE)
	        	tune_param = X_TUNE;
		else
			tune_param = tune_param + 1;

		state = tune_param;

		if(tune_param != prev_tune_param )
		{
			if(tune_param == Z_TUNE)
			{
				kp = KP_Z_INIT;
				kd = KD_Z_INIT;
				ki = KI_Z_INIT;
			}

			if(tune_param == X_TUNE || tune_param == Y_TUNE)
			{
				kp = KP_XY_INIT;
				kd = KD_XY_INIT;
				ki = KI_XY_INIT;
			}

		}

		prev_tune_param = tune_param;
	}
	dirty = true;
        break;

      case KEYCODE_W:
        ROS_DEBUG("UP_W");
	if(mode == ORIENTATION_MODE)
        	thrust += thrust_offset;

	else if(mode == POSITION_MODE)
	        goal_z += goal_z_offset;

	else
	        ki += ki_offset;

	dirty = true;
        break;

      case KEYCODE_S:
        ROS_DEBUG("DOWN_S");
	if(mode == ORIENTATION_MODE)
        	thrust -= thrust_offset;
	else if(mode == POSITION_MODE)
	        goal_z -= goal_z_offset;
	else
	        ki -= ki_offset;

	dirty = true;
        break;

    }

    geometry_msgs::Twist twist;
    std_msgs::Int32 state_msg;

    twist.angular.y = yawrate;

    if(mode == ORIENTATION_MODE)
    {
	    twist.linear.z = thrust;
	    twist.linear.x = pitch;
	    twist.linear.y = roll;
    }

    else
    {
	    twist.linear.z = goal_z;
	    twist.linear.x = goal_x;
	    twist.linear.y = goal_y;

	    if(mode == PID_TUNING_MODE)
		{
		    twist.angular.x = kp;
		    twist.angular.y = ki;
		    twist.angular.z = kd;
		}

    }

    if(dirty ==true)
    {
      thrust_offset = 0.0;
      roll_offset = 0.0;
      yawrate_offset = 0.0;
      pitch_offset = 0.0;
      goal_x_offset = 0.0;
      goal_y_offset = 0.0;
      goal_z_offset = 0.0;
      kp_offset = 0.0;
      kd_offset = 0.0;
      ki_offset = 0.0;

      dirty=false;

    }

      state_msg.data = state;
      state_pub_.publish(state_msg);

      if(state!= CALIBRATE)
      {
	      if(mode == ORIENTATION_MODE)
	      {
	      	vel_pub_.publish(twist);
	      }

	      else
	     {
		cmd_pub_.publish(twist);
	     }

	   if(mode == POSITION_MODE)
		ROS_INFO("X - %f, Y - %f ,Z - %f,yawrate - %f", goal_x ,goal_y, goal_z,yawrate);

	   else if(mode == ORIENTATION_MODE)
	   	ROS_INFO("Thrust - %f, roll - %f ,pitch - %f,yawrate - %f",thrust,roll,pitch,yawrate);

	   else
		ROS_INFO("Tune Param = %d, Kp - %f, Ki - %f , Kd - %f",tune_param,kp,ki,kd);
     }

  }
  return;
}

<|endoftext|>