bdk-blender/intern/itasc/Scene.hpp
Campbell Barton 5035fbdd23 License headers: use SPDX identifiers in intern/itasc
Added license headers based on the original LGPL files from:
gitlab.kuleuven.be/rob-itasc
2022-03-18 10:27:01 +11:00

105 lines
2.9 KiB
C++

/* SPDX-License-Identifier: LGPL-2.1-or-later
* Copyright 2009 Ruben Smits. */
/** \file
* \ingroup intern_itasc
*/
#ifndef SCENE_HPP_
#define SCENE_HPP_
#include "eigen_types.hpp"
#include "WorldObject.hpp"
#include "ConstraintSet.hpp"
#include "Solver.hpp"
#include <map>
namespace iTaSC {
class SceneLock;
class Scene {
friend class SceneLock;
public:
enum SceneParam {
MIN_TIMESTEP = 0,
MAX_TIMESTEP,
COUNT
};
Scene();
virtual ~Scene();
bool addObject(const std::string& name, Object* object, UncontrolledObject* base=&Object::world, const std::string& baseFrame="");
bool addConstraintSet(const std::string& name, ConstraintSet* task,const std::string& object1,const std::string& object2,const std::string& ee1="",const std::string& ee2="");
bool addSolver(Solver* _solver);
bool addCache(Cache* _cache);
bool initialize();
bool update(double timestamp, double timestep, unsigned int numsubstep=1, bool reiterate=false, bool cache=true, bool interpolate=true);
bool setParam(SceneParam paramId, double value);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
e_matrix m_A,m_B,m_Atemp,m_Wq,m_Jf,m_Jq,m_Ju,m_Cf,m_Cq,m_Jf_inv;
e_matrix6 m_Vf,m_Uf;
e_vector m_Wy,m_ydot,m_qdot,m_xdot;
e_vector6 m_Sf,m_tempf;
double m_minstep;
double m_maxstep;
unsigned int m_ncTotal,m_nqTotal,m_nuTotal,m_nsets;
std::vector<bool> m_ytask;
Solver* m_solver;
Cache* m_cache;
struct Object_struct{
Object* object;
UncontrolledObject* base;
unsigned int baseFrameIndex;
Range constraintrange;
Range jointrange;
Range coordinaterange; // Xu range of base when object is controlled
// Xu range of object when object is uncontrolled
Object_struct(Object* _object,UncontrolledObject* _base,unsigned int _baseFrameIndex,Range nq_range,Range nc_range,Range nu_range):
object(_object),base(_base),baseFrameIndex(_baseFrameIndex),constraintrange(nc_range),jointrange(nq_range),coordinaterange(nu_range)
{};
};
typedef std::map<std::string,Object_struct*> ObjectMap;
struct ConstraintSet_struct{
ConstraintSet* task;
ObjectMap::iterator object1;
ObjectMap::iterator object2;
Range constraintrange;
Range featurerange;
unsigned int ee1index;
unsigned int ee2index;
ConstraintSet_struct(ConstraintSet* _task,
ObjectMap::iterator _object1,unsigned int _ee1index,
ObjectMap::iterator _object2,unsigned int _ee2index,
Range nc_range,Range coord_range):
task(_task),
object1(_object1),object2(_object2),
constraintrange(nc_range),featurerange(coord_range),
ee1index(_ee1index), ee2index(_ee2index)
{};
};
typedef std::map<std::string,ConstraintSet_struct*> ConstraintMap;
ObjectMap objects;
ConstraintMap constraints;
static bool getConstraintPose(ConstraintSet* constraint, void *_param, KDL::Frame& _pose);
};
}
#endif /* SCENE_HPP_ */