Changeset 15 in flair-src for trunk/lib/FlairSimulator/src


Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (9 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

Location:
trunk/lib/FlairSimulator/src
Files:
39 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairSimulator/src/AnimPoursuite.cpp

    r10 r15  
    2828using namespace scene;
    2929
    30 namespace flair
    31 {
    32 namespace simulator
    33 {
     30namespace flair {
     31namespace simulator {
    3432
    35 AnimPoursuite::AnimPoursuite(const ISceneNode* parent,float rotateSpeed , float zoomSpeed )
    36 {
    37     this->parent=parent;
    38     this->zoomSpeed=zoomSpeed;
    39     this->rotateSpeed=rotateSpeed;
    40     currentZoom=100;
    41     RotY=20;
    42     RotZ=0;
    43     Rotating=false;
    44     LMouseKey= false;
     33AnimPoursuite::AnimPoursuite(const ISceneNode *parent, float rotateSpeed,
     34                             float zoomSpeed) {
     35  this->parent = parent;
     36  this->zoomSpeed = zoomSpeed;
     37  this->rotateSpeed = rotateSpeed;
     38  currentZoom = 100;
     39  RotY = 20;
     40  RotZ = 0;
     41  Rotating = false;
     42  LMouseKey = false;
    4543}
    4644
    47 AnimPoursuite::~AnimPoursuite()
    48 {
     45AnimPoursuite::~AnimPoursuite() {}
    4946
     47void AnimPoursuite::setPositionOffset(vector3df newpos) { pos_offset = newpos; }
     48
     49void AnimPoursuite::setTargetOffset(vector3df newpos) {
     50  target_offset = newpos;
    5051}
    5152
    52 void AnimPoursuite::setPositionOffset(vector3df newpos)
    53 {
    54     pos_offset=newpos;
     53float AnimPoursuite::sat(float value) {
     54  if (value > 89)
     55    value = 89;
     56  if (value < -89)
     57    value = -89;
     58  return value;
    5559}
    5660
    57 void AnimPoursuite::setTargetOffset(vector3df newpos)
    58 {
    59     target_offset=newpos;
     61void AnimPoursuite::animateNode(ISceneNode *node, u32 timeMs) {
     62  ICameraSceneNode *camera = static_cast<ICameraSceneNode *>(node);
     63  vector3df pos;
     64
     65  float nRotY = RotY;
     66  float nRotZ = RotZ;
     67
     68  if (LMouseKey == true) {
     69    if (!Rotating) {
     70      RotateStart = MousePos;
     71      Rotating = true;
     72      nRotY = RotY;
     73      nRotZ = RotZ;
     74    } else {
     75      nRotY += (RotateStart.Y - MousePos.Y) * rotateSpeed;
     76      nRotZ += (RotateStart.X - MousePos.X) * rotateSpeed;
     77      nRotY = sat(nRotY);
     78    }
     79  } else if (Rotating) {
     80    RotY += (RotateStart.Y - MousePos.Y) * rotateSpeed;
     81    RotZ += (RotateStart.X - MousePos.X) * rotateSpeed;
     82    RotY = sat(RotY);
     83    nRotY = RotY;
     84    nRotZ = RotZ;
     85    Rotating = false;
     86  }
     87
     88  pos.X = -currentZoom;
     89  pos.Y = 0;
     90  pos.Z = 0;
     91
     92  pos.rotateXZBy(-nRotY);
     93  pos.rotateXYBy(getSimulator()->Yaw() + nRotZ + parent->getRotation().Z);
     94
     95  camera->setPosition(parent->getPosition() + pos + pos_offset);
     96  camera->setTarget(parent->getPosition() + target_offset);
    6097}
    6198
    62 float AnimPoursuite::sat(float value)
    63 {
    64     if(value>89) value=89;
    65     if(value<-89) value=-89;
    66     return value;
     99ISceneNodeAnimator *AnimPoursuite::createClone(ISceneNode *node,
     100                                               ISceneManager *newManager) {
     101  return NULL;
    67102}
    68103
    69 void AnimPoursuite::animateNode(ISceneNode* node, u32 timeMs)
    70 {
    71     ICameraSceneNode* camera = static_cast<ICameraSceneNode*>(node);
    72     vector3df pos;
     104bool AnimPoursuite::MouseMoved(const SEvent &event,
     105                               irr::core::position2df MousePos) {
     106  if (event.EventType != EET_MOUSE_INPUT_EVENT)
     107    return false;
    73108
    74     float nRotY = RotY;
    75     float nRotZ = RotZ;
     109  switch (event.MouseInput.Event) {
    76110
    77     if (LMouseKey==true)
    78     {
    79         if (!Rotating)
    80         {
    81             RotateStart = MousePos;
    82             Rotating = true;
    83             nRotY = RotY;
    84             nRotZ = RotZ;
    85         }
    86         else
    87         {
    88             nRotY += (RotateStart.Y - MousePos.Y) * rotateSpeed;
    89             nRotZ += (RotateStart.X - MousePos.X) * rotateSpeed;
    90             nRotY=sat(nRotY);
    91         }
    92     }
    93     else if (Rotating)
    94     {
    95         RotY += (RotateStart.Y - MousePos.Y) * rotateSpeed;
    96         RotZ += (RotateStart.X - MousePos.X) * rotateSpeed;
    97         RotY=sat(RotY);
    98         nRotY = RotY;
    99         nRotZ = RotZ;
    100         Rotating = false;
    101     }
     111  case EMIE_MOUSE_WHEEL:
     112    currentZoom -= event.MouseInput.Wheel * zoomSpeed;
     113    if (currentZoom <= 0)
     114      currentZoom = zoomSpeed;
     115    break;
     116  case EMIE_LMOUSE_PRESSED_DOWN:
     117    LMouseKey = true;
     118    break;
     119  case EMIE_LMOUSE_LEFT_UP:
     120    LMouseKey = false;
     121    break;
     122  case EMIE_MOUSE_MOVED:
     123    this->MousePos = MousePos;
     124    break;
     125  default:
     126    return false;
     127    break;
     128  }
    102129
    103     pos.X = -currentZoom;
    104     pos.Y=0;
    105     pos.Z=0;
    106 
    107     pos.rotateXZBy(-nRotY);
    108     pos.rotateXYBy(getSimulator()->Yaw()+nRotZ+parent->getRotation().Z);
    109 
    110     camera->setPosition(parent->getPosition()+pos+pos_offset);
    111     camera->setTarget(parent->getPosition()+target_offset);
    112 }
    113 
    114 ISceneNodeAnimator* AnimPoursuite::createClone(ISceneNode* node,
    115             ISceneManager* newManager)
    116 {
    117     return NULL;
    118 }
    119 
    120 bool AnimPoursuite::MouseMoved(const SEvent& event,irr::core::position2df MousePos)
    121 {
    122     if (event.EventType != EET_MOUSE_INPUT_EVENT)
    123         return false;
    124 
    125     switch(event.MouseInput.Event)
    126     {
    127 
    128     case EMIE_MOUSE_WHEEL:
    129         currentZoom -= event.MouseInput.Wheel * zoomSpeed;
    130         if(currentZoom<=0) currentZoom=zoomSpeed;
    131         break;
    132     case EMIE_LMOUSE_PRESSED_DOWN:
    133         LMouseKey = true;
    134         break;
    135     case EMIE_LMOUSE_LEFT_UP:
    136         LMouseKey= false;
    137         break;
    138     case EMIE_MOUSE_MOVED:
    139         this->MousePos = MousePos;
    140         break;
    141     default:
    142         return false;
    143         break;
    144     }
    145 
    146     return true;
     130  return true;
    147131}
    148132
    … …  
    150134} // end namespace flair
    151135
    152 #endif //GL
     136#endif // GL

  • trunk/lib/FlairSimulator/src/Blade.cpp

    r10 r15  
    3333using namespace flair::core;
    3434
    35 namespace flair
    36 {
    37 namespace simulator
    38 {
     35namespace flair {
     36namespace simulator {
    3937
    40 Blade::Blade(Model* parent,const vector3df& position,bool inverted,s32 id)
    41     : ISceneNode(parent->getSceneNode(), getGui()->getSceneManager(), id,position)
    42 {
    43     ISceneManager* mgr=getGui()->getSceneManager();
     38Blade::Blade(Model *parent, const vector3df &position, bool inverted, s32 id)
     39    : ISceneNode(parent->getSceneNode(), getGui()->getSceneManager(), id,
     40                 position) {
     41  ISceneManager *mgr = getGui()->getSceneManager();
    4442
    45     const IGeometryCreator *geo;
    46     geo=mgr->getGeometryCreator();
    47     pale=geo->createCubeMesh(vector3df(63.5,0.5,5));
     43  const IGeometryCreator *geo;
     44  geo = mgr->getGeometryCreator();
     45  pale = geo->createCubeMesh(vector3df(63.5, 0.5, 5));
    4846
    49     float angle;
    50     if(inverted==false)
    51     {
    52         angle=20;
    53     }
    54     else
    55     {
    56         angle=-20;
    57     }
     47  float angle;
     48  if (inverted == false) {
     49    angle = 20;
     50  } else {
     51    angle = -20;
     52  }
    5853
    59     ITexture* texture=getGui()->getTexture("carbone.jpg");
    60     pale_1=new MeshSceneNode(parent, pale, vector3df(-30,0,0),vector3df(-angle+90,0,0),texture);
    61     pale_1->setParent(this);
    62     pale_2=new MeshSceneNode(parent, pale, vector3df(30,0,0),vector3df(angle+90,0,0),texture);
    63     pale_2->setParent(this);
     54  ITexture *texture = getGui()->getTexture("carbone.jpg");
     55  pale_1 = new MeshSceneNode(parent, pale, vector3df(-30, 0, 0),
     56                             vector3df(-angle + 90, 0, 0), texture);
     57  pale_1->setParent(this);
     58  pale_2 = new MeshSceneNode(parent, pale, vector3df(30, 0, 0),
     59                             vector3df(angle + 90, 0, 0), texture);
     60  pale_2->setParent(this);
    6461
    65     anim = mgr->createRotationAnimator(vector3df(0.f, 0.f, 0.f));
    66     addAnimator(anim);
     62  anim = mgr->createRotationAnimator(vector3df(0.f, 0.f, 0.f));
     63  addAnimator(anim);
    6764}
    6865
    69 void Blade::OnRegisterSceneNode()
    70 {
    71     if (IsVisible)
    72         SceneManager->registerNodeForRendering(this);
     66void Blade::OnRegisterSceneNode() {
     67  if (IsVisible)
     68    SceneManager->registerNodeForRendering(this);
    7369
    74     ISceneNode::OnRegisterSceneNode();
     70  ISceneNode::OnRegisterSceneNode();
    7571}
    7672
    77 void Blade::SetRotationSpeed(float value)
    78 {
    79     IAttributes* attribs =getSceneManager()->getFileSystem()->createEmptyAttributes();
     73void Blade::SetRotationSpeed(float value) {
     74  IAttributes *attribs =
     75      getSceneManager()->getFileSystem()->createEmptyAttributes();
    8076
    81     attribs->setAttribute("Type","rotation");
    82     attribs->setAttribute("Rotation",vector3df(0.f, 0.f,value));
    83     anim->deserializeAttributes(attribs);
     77  attribs->setAttribute("Type", "rotation");
     78  attribs->setAttribute("Rotation", vector3df(0.f, 0.f, value));
     79  anim->deserializeAttributes(attribs);
    8480
    85     attribs->drop();
     81  attribs->drop();
    8682}
    8783
    88 void Blade::render()
    89 {
    90     IVideoDriver* driver = SceneManager->getVideoDriver();
    91     driver->setTransform(ETS_WORLD, AbsoluteTransformation);
     84void Blade::render() {
     85  IVideoDriver *driver = SceneManager->getVideoDriver();
     86  driver->setTransform(ETS_WORLD, AbsoluteTransformation);
    9287}
    9388
    9489} // end namespace simulator
    9590} // end namespace flair
    96 #endif //GL
     91#endif // GL

  • trunk/lib/FlairSimulator/src/Blade.h

    r10 r15  
    2020#include <ISceneNode.h>
    2121
    22 namespace irr
    23 {
    24     namespace scene
    25     {
    26         class IMesh;
    27     }
     22namespace irr {
     23namespace scene {
     24class IMesh;
     25}
    2826}
    2927
    30 namespace flair
    31 {
    32 namespace simulator
    33 {
    34     class MeshSceneNode;
    35     class Model;
     28namespace flair {
     29namespace simulator {
     30class MeshSceneNode;
     31class Model;
    3632
    37     class Blade : public irr::scene::ISceneNode
    38     {
    39         public:
     33class Blade : public irr::scene::ISceneNode {
     34public:
     35  Blade(Model *parent,
     36        const irr::core::vector3df &position = irr::core::vector3df(0, 0, 0),
     37        bool inverted = false, irr::s32 id = -1);
     38  virtual void OnRegisterSceneNode(void);
     39  virtual void render(void);
     40  virtual const irr::core::aabbox3d<irr::f32> &getBoundingBox(void) const {
     41    return Box;
     42  }
     43  void SetRotationSpeed(float value);
    4044
    41             Blade(Model* parent,const irr::core::vector3df& position = irr::core::vector3df(0,0,0),bool inverted=false,irr::s32 id=-1);
    42             virtual void OnRegisterSceneNode(void);
    43             virtual void render(void);
    44             virtual const irr::core::aabbox3d<irr::f32>& getBoundingBox(void) const
    45             {
    46                 return Box;
    47             }
    48             void SetRotationSpeed(float value);
    49 
    50 
    51         private:
    52             irr::scene::IMesh *pale;
    53             irr::core::aabbox3d<irr::f32> Box;
    54             MeshSceneNode *pale_1,*pale_2;
    55             irr::scene::ISceneNodeAnimator *anim;
    56     };
     45private:
     46  irr::scene::IMesh *pale;
     47  irr::core::aabbox3d<irr::f32> Box;
     48  MeshSceneNode *pale_1, *pale_2;
     49  irr::scene::ISceneNodeAnimator *anim;
     50};
    5751} // end namespace simulator
    5852} // end namespace flair

  • trunk/lib/FlairSimulator/src/Castle.cpp

    r10 r15  
    2424using namespace flair::core;
    2525
    26 namespace flair
    27 {
    28 namespace simulator
    29 {
     26namespace flair {
     27namespace simulator {
    3028
    31 Castle::Castle(const Simulator* parent,int app_width, int app_height,int scene_width, int scene_height,std::string media_path): Gui(parent,"Castle",app_width,app_height,scene_width,scene_height, media_path)
    32 {
    33     //carte
    34     std::string file=media_path +"/map-20kdm2.pk3";
    35         getDevice()->getFileSystem()->addFileArchive(file.c_str());
    36         setMesh("20kdm2.bsp",vector3df(-1400,-65,-1349));
     29Castle::Castle(const Simulator *parent, int app_width, int app_height,
     30               int scene_width, int scene_height, std::string media_path)
     31    : Gui(parent, "Castle", app_width, app_height, scene_width, scene_height,
     32          media_path) {
     33  // carte
     34  std::string file = media_path + "/map-20kdm2.pk3";
     35  getDevice()->getFileSystem()->addFileArchive(file.c_str());
     36  setMesh("20kdm2.bsp", vector3df(-1400, -65, -1349));
    3737}
    3838
    39 Castle::~Castle()
    40 {
    41 
    42 }
     39Castle::~Castle() {}
    4340
    4441} // end namespace simulator
    4542} // end namespace flair
    46 #endif //GL
     43#endif // GL

  • trunk/lib/FlairSimulator/src/Castle.h

    r10 r15  
    1919
    2020#include <Gui.h>
    21 namespace flair
    22 {
    23 namespace simulator
    24 {
    25     class Castle:public Gui
    26     {
    27         public:
    28             Castle(const flair::simulator::Simulator* parent,int app_width, int app_height,int scene_width, int scene_height,std::string media_path);
    29             ~Castle();
     21namespace flair {
     22namespace simulator {
     23class Castle : public Gui {
     24public:
     25  Castle(const flair::simulator::Simulator *parent, int app_width,
     26         int app_height, int scene_width, int scene_height,
     27         std::string media_path);
     28  ~Castle();
    3029
    31         private:
    32 
    33     };
     30private:
     31};
    3432} // end namespace simulator
    3533} // end namespace flair

  • trunk/lib/FlairSimulator/src/DiscreteTimeVariable.h

    r10 r15  
    2020#include <stdlib.h>
    2121
    22 namespace flair
    23 {
    24 namespace simulator
    25 {
    26     template <typename T,size_t size>
    27     class DiscreteTimeVariable
    28     {
    29         public:
    30             DiscreteTimeVariable(){};
    31             ~DiscreteTimeVariable(){};
    32             T& operator[](ssize_t idx)
    33             {
    34                 if(idx>0) idx=0;
    35                 if(idx<(ssize_t)(-size+1)) idx=-size+1;
    36                 return array[-idx];
    37             }
    38             const T&operator[](ssize_t idx) const
    39             {
    40                 return const_cast<T&>(*this)[idx];
    41             };
    42             void Update(void)
    43             {
    44                 for(int i=size-1;i>0;i--)
    45                 {
    46                     array[i]=array[i-1];
    47                 }
    48             }
     22namespace flair {
     23namespace simulator {
     24template <typename T, size_t size> class DiscreteTimeVariable {
     25public:
     26  DiscreteTimeVariable(){};
     27  ~DiscreteTimeVariable(){};
     28  T &operator[](ssize_t idx) {
     29    if (idx > 0)
     30      idx = 0;
     31    if (idx < (ssize_t)(-size + 1))
     32      idx = -size + 1;
     33    return array[-idx];
     34  }
     35  const T &operator[](ssize_t idx) const {
     36    return const_cast<T &>(*this)[idx];
     37  };
     38  void Update(void) {
     39    for (int i = size - 1; i > 0; i--) {
     40      array[i] = array[i - 1];
     41    }
     42  }
    4943
    50         private:
    51             T array[size];
    52 
    53     };
     44private:
     45  T array[size];
     46};
    5447} // end namespace simulator
    5548} // end namespace flair

  • trunk/lib/FlairSimulator/src/GenericObject.cpp

    r10 r15  
    3737using namespace flair::simulator;
    3838
    39 namespace flair
     39namespace flair {
     40namespace simulator {
     41
     42GenericObject::GenericObject(Simulator *parent, std::string name,
     43                             ISceneManager *sceneManager)
     44    : IMeshSceneNode(sceneManager->getRootSceneNode(), sceneManager,
     45                     -1) //, IODevice(parent,name)
    4046{
    41 namespace simulator
    42 {
     47  setMaterialFlag(EMF_LIGHTING, false);
     48  Material = getMaterial(0);
     49  // setMaterialTexture(0,sceneManager->getVideoDriver()->getTexture("/home/cesar/igep/uav_dev_svn/trunk/media/nskinbl.jpg"));
     50  Material.NormalizeNormals = true;
     51  Material.Wireframe = false;
     52  Material.Lighting = false;
    4353
    44 GenericObject::GenericObject(Simulator* parent,std::string name, ISceneManager* sceneManager) : IMeshSceneNode(sceneManager->getRootSceneNode(), sceneManager, -1)//, IODevice(parent,name)
    45 {
    46     setMaterialFlag(EMF_LIGHTING,false);
    47     Material=getMaterial(0);
    48     //setMaterialTexture(0,sceneManager->getVideoDriver()->getTexture("/home/cesar/igep/uav_dev_svn/trunk/media/nskinbl.jpg"));
    49     Material.NormalizeNormals = true;
    50     Material.Wireframe = false;
    51     Material.Lighting = false;
    52 
    53     parent->pimpl_->objects.push_back(this);
     54  parent->pimpl_->objects.push_back(this);
    5455}
    5556
    56 GenericObject::~GenericObject()
    57 {
     57GenericObject::~GenericObject() {}
    5858
     59void GenericObject::setScale(float value) {
     60  ISceneNode::setScale(vector3df(value, value, value));
    5961}
    6062
    61 void GenericObject::setScale(float value)
    62 {
    63     ISceneNode::setScale(vector3df(value,value,value));
     63void GenericObject::setScale(vector3df scale) { ISceneNode::setScale(scale); }
     64
     65ITriangleSelector *GenericObject::TriangleSelector(void) { return selector; }
     66
     67void GenericObject::OnRegisterSceneNode(void) {
     68  if (IsVisible)
     69    SceneManager->registerNodeForRendering(this);
     70
     71  ISceneNode::OnRegisterSceneNode();
    6472}
    6573
    66 void GenericObject::setScale(vector3df scale)
    67 {
    68     ISceneNode::setScale(scale);
     74void GenericObject::render(void) {
     75  IVideoDriver *driver = SceneManager->getVideoDriver();
     76
     77  driver->setMaterial(Material);
     78
     79  driver->setTransform(ETS_WORLD, AbsoluteTransformation);
     80  driver->drawMeshBuffer(mesh->getMeshBuffer(0));
    6981}
    7082
    71 ITriangleSelector* GenericObject::TriangleSelector(void)
    72 {
    73     return selector;
     83void GenericObject::setMesh(IMesh *mesh) {
     84  this->mesh = mesh;
     85  box = mesh->getBoundingBox();
     86
     87  selector = getSceneManager()->createTriangleSelector(mesh, this);
     88  setTriangleSelector(selector);
    7489}
    7590
    76 void GenericObject::OnRegisterSceneNode(void)
    77 {
    78     if (IsVisible)
    79         SceneManager->registerNodeForRendering(this);
     91IMesh *GenericObject::getMesh(void) { return mesh; }
    8092
    81     ISceneNode::OnRegisterSceneNode();
     93void GenericObject::setPosition(irr::core::vector3df pos) {
     94  ISceneNode::setPosition(ToIrrlichtCoordinates(pos));
    8295}
    8396
    84 void GenericObject::render(void)
    85 {
    86     IVideoDriver* driver = SceneManager->getVideoDriver();
     97void GenericObject::setRotation(irr::core::vector3df rotation) {
     98  Euler eulerA(rotation.X, rotation.Y, rotation.Z);
     99  Euler eulerB;
     100  Quaternion quatA, quatB;
     101  eulerA.ToQuaternion(quatA);
     102  quatB = ToIrrlichtOrientation(quatA);
     103  quatB.ToEuler(eulerB);
    87104
    88     driver->setMaterial(Material);
    89 
    90     driver->setTransform(ETS_WORLD, AbsoluteTransformation);
    91     driver->drawMeshBuffer(mesh->getMeshBuffer(0));
    92 }
    93 
    94 void GenericObject::setMesh(IMesh* mesh)
    95 {
    96     this->mesh=mesh;
    97     box=mesh->getBoundingBox();
    98 
    99     selector = getSceneManager()->createTriangleSelector(mesh,this);
    100     setTriangleSelector(selector);
    101 }
    102 
    103 IMesh* GenericObject::getMesh(void)
    104 {
    105     return mesh;
    106 }
    107 
    108 void GenericObject::setPosition(irr::core::vector3df pos)
    109 {
    110     ISceneNode::setPosition(ToIrrlichtCoordinates(pos));
    111 }
    112 
    113 void GenericObject::setRotation(irr::core::vector3df rotation)
    114 {
    115     Euler eulerA(rotation.X,rotation.Y,rotation.Z);
    116     Euler eulerB;
    117     Quaternion quatA,quatB;
    118     eulerA.ToQuaternion(quatA);
    119     quatB=ToIrrlichtOrientation(quatA);
    120     quatB.ToEuler(eulerB);
    121 
    122     ISceneNode::setRotation(Euler::ToDegree(1)*vector3df(eulerB.roll,eulerB.pitch,eulerB.yaw));
     105  ISceneNode::setRotation(Euler::ToDegree(1) *
     106                          vector3df(eulerB.roll, eulerB.pitch, eulerB.yaw));
    123107}
    124108

  • trunk/lib/FlairSimulator/src/Gui.cpp

    r10 r15  
    4040
    4141namespace {
    42     flair::simulator::Gui* gui_=NULL;
    43     std::vector <std::string> extensions;
    44     bool getGlInfo();
     42flair::simulator::Gui *gui_ = NULL;
     43std::vector<std::string> extensions;
     44bool getGlInfo();
    4545}
    4646
    47 namespace flair { namespace simulator {
     47namespace flair {
     48namespace simulator {
    4849
    49 Gui* getGui(void) {
    50     return gui_;
     50Gui *getGui(void) { return gui_; }
     51
     52bool noGui(void) {
     53  if (gui_ == NULL) {
     54    return true;
     55  } else {
     56    return false;
     57  }
    5158}
    5259
    53 bool noGui(void) {
    54     if(gui_==NULL) {
    55         return true;
    56     } else {
    57         return false;
     60// getGlinfo, code from Song Ho Ahn (song.ahn@gmail.com)
     61bool getGlInfo() {
     62  char *str = 0;
     63  char *tok = 0;
     64
     65  // get all extensions as a string
     66  str = (char *)glGetString(GL_EXTENSIONS);
     67
     68  // split extensions
     69  if (str) {
     70    tok = strtok((char *)str, " ");
     71    while (tok) {
     72      extensions.push_back(tok); // put a extension into struct
     73      tok = strtok(0, " ");      // next token
    5874    }
     75  } else {
     76    printf("cannot get gl extensions\n");
     77  }
     78
     79  // sort extension by alphabetical order
     80  std::sort(extensions.begin(), extensions.end());
    5981}
    6082
    61 //getGlinfo, code from Song Ho Ahn (song.ahn@gmail.com)
    62 bool getGlInfo() {
    63     char* str = 0;
    64     char* tok = 0;
     83// isGlExtensionSupported, code from Song Ho Ahn (song.ahn@gmail.com)
     84bool isGlExtensionSupported(const std::string &ext) {
     85  if (extensions.size() == 0)
     86    getGlInfo();
    6587
    66     // get all extensions as a string
    67     str = (char*)glGetString(GL_EXTENSIONS);
     88  // search corresponding extension
     89  std::vector<std::string>::const_iterator iter = extensions.begin();
     90  std::vector<std::string>::const_iterator endIter = extensions.end();
    6891
    69     // split extensions
    70     if(str) {
    71         tok = strtok((char*)str, " ");
    72         while(tok) {
    73             extensions.push_back(tok);    // put a extension into struct
    74             tok = strtok(0, " ");               // next token
    75         }
    76     } else {
    77         printf("cannot get gl extensions\n");
    78     }
    79 
    80     // sort extension by alphabetical order
    81     std::sort(extensions.begin(), extensions.end());
     92  while (iter != endIter) {
     93    if (ext == *iter)
     94      return true;
     95    else
     96      ++iter;
     97  }
     98  return false;
    8299}
    83100
    84 //isGlExtensionSupported, code from Song Ho Ahn (song.ahn@gmail.com)
    85 bool isGlExtensionSupported(const std::string& ext) {
    86     if(extensions.size()==0) getGlInfo();
     101float ToIrrlichtScale(float value) { return value * 100.; }
    87102
    88     // search corresponding extension
    89     std::vector<std::string>::const_iterator iter = extensions.begin();
    90     std::vector<std::string>::const_iterator endIter = extensions.end();
    91 
    92     while(iter != endIter) {
    93         if(ext == *iter)
    94             return true;
    95         else
    96             ++iter;
    97     }
    98     return false;
    99 }
    100 
    101 float ToIrrlichtScale(float value) {
    102     return value*100.;
    103 }
    104 
    105 float ToSimulatorScale(float value) {
    106     return value/100.;
    107 }
     103float ToSimulatorScale(float value) { return value / 100.; }
    108104
    109105vector3df ToIrrlichtCoordinates(vector3df vect) {
    110     return ToIrrlichtScale(1)*vector3df(vect.X,vect.Y,-vect.Z);
     106  return ToIrrlichtScale(1) * vector3df(vect.X, vect.Y, -vect.Z);
    111107}
    112108
    113109vector3df ToIrrlichtCoordinates(Vector3D vect) {
    114     return ToIrrlichtScale(1)*vector3df(vect.x,vect.y,-vect.z);
     110  return ToIrrlichtScale(1) * vector3df(vect.x, vect.y, -vect.z);
    115111}
    116112
    117113Vector3D ToSimulatorCoordinates(vector3df vect) {
    118     return ToSimulatorScale(1)*Vector3D(vect.X,vect.Y,-vect.Z);
     114  return ToSimulatorScale(1) * Vector3D(vect.X, vect.Y, -vect.Z);
    119115}
    120116
    121117Quaternion ToIrrlichtOrientation(Quaternion quat) {
    122     /* original code
    123     Euler euler;
    124     quat.ToEuler(euler);
    125     matrix4 m;
    126     m.setRotationRadians(vector3df(0, 0, euler.yaw));
    127     matrix4 n;
    128     n.setRotationRadians(vector3df(0, -euler.pitch,0));
    129     m *= n;
    130     n.setRotationRadians(vector3df(-euler.roll, 0, 0));
    131     m *= n;
    132     */
    133     //seems to be equivalent to:
    134     return Quaternion(quat.q0,-quat.q1,-quat.q2,quat.q3);
     118  /* original code
     119  Euler euler;
     120  quat.ToEuler(euler);
     121  matrix4 m;
     122  m.setRotationRadians(vector3df(0, 0, euler.yaw));
     123  matrix4 n;
     124  n.setRotationRadians(vector3df(0, -euler.pitch,0));
     125  m *= n;
     126  n.setRotationRadians(vector3df(-euler.roll, 0, 0));
     127  m *= n;
     128  */
     129  // seems to be equivalent to:
     130  return Quaternion(quat.q0, -quat.q1, -quat.q2, quat.q3);
    135131}
    136132
    137 Gui::Gui(const Simulator* parent,std::string name,int app_width, int app_height,int scene_width, int scene_height,std::string media_path,E_DRIVER_TYPE driver_type): Object(parent,name,"Gui") {
    138     if(gui_!=NULL) Err("Gui should be instanced only one time\n");
    139     pimpl_=new Gui_impl(this,app_width,app_height,scene_width,scene_height,media_path,driver_type);
    140     gui_=this;
     133Gui::Gui(const Simulator *parent, std::string name, int app_width,
     134         int app_height, int scene_width, int scene_height,
     135         std::string media_path, E_DRIVER_TYPE driver_type)
     136    : Object(parent, name, "Gui") {
     137  if (gui_ != NULL)
     138    Err("Gui should be instanced only one time\n");
     139  pimpl_ = new Gui_impl(this, app_width, app_height, scene_width, scene_height,
     140                        media_path, driver_type);
     141  gui_ = this;
    141142}
    142143
    143 Gui::~Gui() {
    144     delete pimpl_;
     144Gui::~Gui() { delete pimpl_; }
     145
     146float Gui::getAspectRatio(void) const {
     147  return (float)pimpl_->scene_width / (float)pimpl_->scene_height;
    145148}
    146149
    147 float Gui::getAspectRatio(void) const {
    148     return (float)pimpl_->scene_width/(float)pimpl_->scene_height;
     150ISceneManager *Gui::getSceneManager(void) const { return pimpl_->smgr; }
     151
     152void Gui::setMesh(std::string file, vector3df position, vector3df rotation,
     153                  vector3df scale) {
     154  pimpl_->setMesh(file, position, rotation, scale);
    149155}
    150156
    151 ISceneManager* Gui::getSceneManager(void) const {
    152     return pimpl_->smgr;
     157vector3df Gui::getRotation(void) const { return pimpl_->node->getRotation(); }
     158
     159IrrlichtDevice *Gui::getDevice(void) const { return pimpl_->device; }
     160
     161ITexture *Gui::getTexture(std::string filename) const {
     162  filename = pimpl_->media_path + "/" + filename;
     163  return pimpl_->driver->getTexture(filename.c_str());
    153164}
    154165
    155 void Gui::setMesh(std::string file,vector3df position,vector3df rotation,vector3df scale) {
    156     pimpl_->setMesh(file,position,rotation,scale);
    157 }
    158 
    159 vector3df Gui::getRotation(void) const {
    160     return pimpl_->node->getRotation();
    161 }
    162 
    163 IrrlichtDevice *Gui::getDevice(void) const {
    164     return pimpl_->device;
    165 }
    166 
    167 ITexture* Gui::getTexture(std::string filename) const {
    168     filename=pimpl_->media_path+ "/" +filename;
    169     return pimpl_->driver->getTexture(filename.c_str());
    170 }
    171 
    172 IAnimatedMesh* Gui::getMesh(std::string filename) const {
    173     filename=pimpl_->media_path+ "/" +filename;
    174     return pimpl_->smgr->getMesh(filename.c_str());
     166IAnimatedMesh *Gui::getMesh(std::string filename) const {
     167  filename = pimpl_->media_path + "/" + filename;
     168  return pimpl_->smgr->getMesh(filename.c_str());
    175169}
    176170
    177171} // end namespace simulator
    178172} // end namespace flair
    179 #endif //GL
     173#endif // GL

  • trunk/lib/FlairSimulator/src/Gui.h

    r10 r15  
    2222#include <vector3d.h>
    2323
    24 namespace irr
    25 {
    26     class IrrlichtDevice;
    27     namespace video
    28     {
    29         class ITexture;
    30     }
    31     namespace scene
    32     {
    33         class IAnimatedMesh;
    34         class ISceneManager;
    35     }
     24namespace irr {
     25class IrrlichtDevice;
     26namespace video {
     27class ITexture;
     28}
     29namespace scene {
     30class IAnimatedMesh;
     31class ISceneManager;
     32}
    3633}
    3734
    38 namespace flair
    39 {
    40     namespace core
    41     {
    42        class Object;
    43        class Vector3D;
    44        class Euler;
    45        class Quaternion;
    46     }
     35namespace flair {
     36namespace core {
     37class Object;
     38class Vector3D;
     39class Euler;
     40class Quaternion;
     41}
    4742}
    4843
    … …  
    5146class Model_impl;
    5247
    53 namespace flair
    54 {
    55 namespace simulator
    56 {
    57     class Simulator;
     48namespace flair {
     49namespace simulator {
     50class Simulator;
    5851
    59     class Gui: public core::Object
    60     {
    61         friend class ::Simulator_impl;
     52class Gui : public core::Object {
     53  friend class ::Simulator_impl;
    6254
    63         public:
    64             Gui(const Simulator* parent,std::string name,int app_width, int app_height,int scene_width, int scene_height,std::string media_path,irr::video::E_DRIVER_TYPE driver_type=irr::video::EDT_OPENGL);
    65             ~Gui();
    66             irr::core::vector3df getRotation(void) const;
    67             irr::video::ITexture* getTexture(std::string filename) const;
    68             irr::scene::IAnimatedMesh* getMesh(std::string filename) const;
    69             irr::scene::ISceneManager* getSceneManager(void) const;
    70             float getAspectRatio(void) const;
     55public:
     56  Gui(const Simulator *parent, std::string name, int app_width, int app_height,
     57      int scene_width, int scene_height, std::string media_path,
     58      irr::video::E_DRIVER_TYPE driver_type = irr::video::EDT_OPENGL);
     59  ~Gui();
     60  irr::core::vector3df getRotation(void) const;
     61  irr::video::ITexture *getTexture(std::string filename) const;
     62  irr::scene::IAnimatedMesh *getMesh(std::string filename) const;
     63  irr::scene::ISceneManager *getSceneManager(void) const;
     64  float getAspectRatio(void) const;
    7165
    72         protected:
    73             irr::IrrlichtDevice *getDevice(void) const;
    74             void setMesh(std::string file,irr::core::vector3df position = irr::core::vector3df(0,0,0),irr::core::vector3df rotation= irr::core::vector3df(0,0,0),irr::core::vector3df scale= irr::core::vector3df(1,1,1));
     66protected:
     67  irr::IrrlichtDevice *getDevice(void) const;
     68  void setMesh(std::string file,
     69               irr::core::vector3df position = irr::core::vector3df(0, 0, 0),
     70               irr::core::vector3df rotation = irr::core::vector3df(0, 0, 0),
     71               irr::core::vector3df scale = irr::core::vector3df(1, 1, 1));
    7572
    76         private:
    77             Gui_impl *pimpl_;
    78     };
     73private:
     74  Gui_impl *pimpl_;
     75};
    7976
    80     /*!
    81     * \brief get Gui
    82     *
    83     * \return the Gui
    84     */
    85     Gui* getGui(void);
     77/*!
     78* \brief get Gui
     79*
     80* \return the Gui
     81*/
     82Gui *getGui(void);
    8683
    87     bool noGui(void);
     84bool noGui(void);
    8885
    89     bool isGlExtensionSupported(const std::string& ext); // check if a extension is supported
     86bool isGlExtensionSupported(
     87    const std::string &ext); // check if a extension is supported
    9088
    91     /*!
    92     * \brief Convert to irrlicht scale
    93     *
    94     * \param value value in simulator scale
    95     *
    96     * \return value in irrlicht scale
    97     */
    98     float ToIrrlichtScale(float value);
     89/*!
     90* \brief Convert to irrlicht scale
     91*
     92* \param value value in simulator scale
     93*
     94* \return value in irrlicht scale
     95*/
     96float ToIrrlichtScale(float value);
    9997
    100     /*!
    101     * \brief Convert to simulator scale
    102     *
    103     * \param value value in irrlicht scale
    104     *
    105     * \return value in simulator scale
    106     */
    107     float ToSimulatorScale(float value);
     98/*!
     99* \brief Convert to simulator scale
     100*
     101* \param value value in irrlicht scale
     102*
     103* \return value in simulator scale
     104*/
     105float ToSimulatorScale(float value);
    108106
    109     /*!
    110     * \brief Convert to irrlicht coordinates
    111     *
    112     * irrlicht is left handed and as a different scale
    113     *
    114     * \param vect vector in simulator coordinates
    115     *
    116     * \return vector in irrlicht coordinates
    117     */
    118     irr::core::vector3df ToIrrlichtCoordinates(irr::core::vector3df vect);
     107/*!
     108* \brief Convert to irrlicht coordinates
     109*
     110* irrlicht is left handed and as a different scale
     111*
     112* \param vect vector in simulator coordinates
     113*
     114* \return vector in irrlicht coordinates
     115*/
     116irr::core::vector3df ToIrrlichtCoordinates(irr::core::vector3df vect);
    119117
    120     /*!
    121     * \brief Convert to irrlicht coordinates
    122     *
    123     * irrlicht is left handed and as a different scale
    124     *
    125     * \param vect vector in simulator coordinates
    126     *
    127     * \return vector in irrlicht coordinates
    128     */
    129     irr::core::vector3df ToIrrlichtCoordinates(core::Vector3D vect);
     118/*!
     119* \brief Convert to irrlicht coordinates
     120*
     121* irrlicht is left handed and as a different scale
     122*
     123* \param vect vector in simulator coordinates
     124*
     125* \return vector in irrlicht coordinates
     126*/
     127irr::core::vector3df ToIrrlichtCoordinates(core::Vector3D vect);
    130128
    131     /*!
    132     * \brief Convert to simulator coordinates
    133     *
    134     * irrlicht is left handed and as a different scale
    135     *
    136     * \param vect vector in irrlicht coordinates
    137     *
    138     * \return vector in simulator coordinates
    139     */
    140     core::Vector3D ToSimulatorCoordinates(irr::core::vector3df vect);
     129/*!
     130* \brief Convert to simulator coordinates
     131*
     132* irrlicht is left handed and as a different scale
     133*
     134* \param vect vector in irrlicht coordinates
     135*
     136* \return vector in simulator coordinates
     137*/
     138core::Vector3D ToSimulatorCoordinates(irr::core::vector3df vect);
    141139
    142     /*!
    143     * \brief Convert to irrlicht orientation
    144     *
    145     * irrlicht is left handed
    146     *
    147     * \param quat quaternion in simulator frame
    148     *
    149     * \return quaternion in irrlicht frame
    150     */
    151     core::Quaternion ToIrrlichtOrientation(core::Quaternion quat);
     140/*!
     141* \brief Convert to irrlicht orientation
     142*
     143* irrlicht is left handed
     144*
     145* \param quat quaternion in simulator frame
     146*
     147* \return quaternion in irrlicht frame
     148*/
     149core::Quaternion ToIrrlichtOrientation(core::Quaternion quat);
    152150
    153151} // end namespace simulator

  • trunk/lib/FlairSimulator/src/Gui_impl.cpp

    r10 r15  
    3737using namespace flair::simulator;
    3838
    39 class MyEventReceiver : public IEventReceiver
    40 {
     39class MyEventReceiver : public IEventReceiver {
    4140public:
    42         // This is the one method that we have to implement
    43         virtual bool OnEvent(const SEvent& event)
    44         {
    45                 // Remember whether each key is down or up
    46                 if (event.EventType == EET_KEY_INPUT_EVENT )
    47                         KeyIsDown[event.KeyInput.Key] = event.KeyInput.PressedDown;
    48 
    49                 //Send all other events to camera
    50                 if (event.EventType == EET_MOUSE_INPUT_EVENT && camera)
    51                         //return camera->OnEvent(event);
    52                         return camera->MouseMoved(event,cursorControl->getRelativePosition());
    53 
    54                 if(model)
    55                         return model->OnEvent(event);
    56 
    57                 return false;
    58         }
    59 
    60         // This is used to check whether a key is being held down
    61         virtual bool IsKeyDown(EKEY_CODE keyCode)
    62         {
    63                 if(KeyIsDown[keyCode]==true)
    64                 {
    65                         KeyIsDown[keyCode]=false;
    66                         return true;
    67                 }
    68                 else
    69                 {
    70                         return false;
    71                 }
    72         }
    73 
    74         MyEventReceiver(ICursorControl* cursorControl)
    75         {
    76                 this->cursorControl=cursorControl;
    77                 camera=NULL;
    78                 model=NULL;
    79                 for (u32 i=0; i<KEY_KEY_CODES_COUNT; ++i)
    80                         KeyIsDown[i] = false;
    81         }
    82         void SetCamera(AnimPoursuite* camera)
    83         {
    84                 this->camera=camera;
    85         }
    86         void SetModel(Model* model)
    87         {
    88                 this->model=model;
    89         }
     41  // This is the one method that we have to implement
     42  virtual bool OnEvent(const SEvent &event) {
     43    // Remember whether each key is down or up
     44    if (event.EventType == EET_KEY_INPUT_EVENT)
     45      KeyIsDown[event.KeyInput.Key] = event.KeyInput.PressedDown;
     46
     47    // Send all other events to camera
     48    if (event.EventType == EET_MOUSE_INPUT_EVENT && camera)
     49      // return camera->OnEvent(event);
     50      return camera->MouseMoved(event, cursorControl->getRelativePosition());
     51
     52    if (model)
     53      return model->OnEvent(event);
     54
     55    return false;
     56  }
     57
     58  // This is used to check whether a key is being held down
     59  virtual bool IsKeyDown(EKEY_CODE keyCode) {
     60    if (KeyIsDown[keyCode] == true) {
     61      KeyIsDown[keyCode] = false;
     62      return true;
     63    } else {
     64      return false;
     65    }
     66  }
     67
     68  MyEventReceiver(ICursorControl *cursorControl) {
     69    this->cursorControl = cursorControl;
     70    camera = NULL;
     71    model = NULL;
     72    for (u32 i = 0; i < KEY_KEY_CODES_COUNT; ++i)
     73      KeyIsDown[i] = false;
     74  }
     75  void SetCamera(AnimPoursuite *camera) { this->camera = camera; }
     76  void SetModel(Model *model) { this->model = model; }
    9077
    9178private:
    92         // We use this array to store the current state of each key
    93         bool KeyIsDown[KEY_KEY_CODES_COUNT];
    94         AnimPoursuite* camera;
    95         Model* model;
    96         ICursorControl* cursorControl;
     79  // We use this array to store the current state of each key
     80  bool KeyIsDown[KEY_KEY_CODES_COUNT];
     81  AnimPoursuite *camera;
     82  Model *model;
     83  ICursorControl *cursorControl;
    9784};
    9885
    99 Gui_impl::Gui_impl(Gui* self,int app_width, int app_height,int scene_width, int scene_height,std::string media_path,E_DRIVER_TYPE driver_type)
    100 {
    101         this->self=self;
    102         dbtFile_w=NULL;
    103         dbtFile_r=NULL;
    104         this->media_path=media_path;
    105         this->scene_width=scene_width;
    106         this->scene_height=scene_height;
    107 
    108         device =createDevice(driver_type, dimension2d<u32>(app_width, app_height),16, false, false, false);
    109         receiver=new MyEventReceiver(device->getCursorControl());
    110         device->setEventReceiver(receiver);
    111         device->getLogger()->setLogLevel(ELL_NONE);
    112 
    113         device->getCursorControl()->setVisible(false);
    114         device->setResizable(false);
    115 
    116         //font = device->getGUIEnvironment()->getBuiltInFont();
    117         driver = device->getVideoDriver();
    118         smgr = device->getSceneManager();
    119 
    120         smgr->setAmbientLight(video::SColorf(1,1,1));
    121 
    122 
    123         /*
    124     env = device->getGUIEnvironment();
     86Gui_impl::Gui_impl(Gui *self, int app_width, int app_height, int scene_width,
     87                   int scene_height, std::string media_path,
     88                   E_DRIVER_TYPE driver_type) {
     89  this->self = self;
     90  dbtFile_w = NULL;
     91  dbtFile_r = NULL;
     92  this->media_path = media_path;
     93  this->scene_width = scene_width;
     94  this->scene_height = scene_height;
     95
     96  device = createDevice(driver_type, dimension2d<u32>(app_width, app_height),
     97                        16, false, false, false);
     98  receiver = new MyEventReceiver(device->getCursorControl());
     99  device->setEventReceiver(receiver);
     100  device->getLogger()->setLogLevel(ELL_NONE);
     101
     102  device->getCursorControl()->setVisible(false);
     103  device->setResizable(false);
     104
     105  // font = device->getGUIEnvironment()->getBuiltInFont();
     106  driver = device->getVideoDriver();
     107  smgr = device->getSceneManager();
     108
     109  smgr->setAmbientLight(video::SColorf(1, 1, 1));
     110
     111  /*
     112env = device->getGUIEnvironment();
    125113IGUISkin* skin = env->getSkin();
    126         font = env->getFont("./fonthaettenschweiler.bmp");
    127 
    128         if (font)
    129                 skin->setFont(font);
    130 
    131         // create menu
    132 
    133         IGUIContextMenu* menu = env->addMenu();
    134         menu->setMinSize(core::dimension2du(640,20));
    135         menu->addItem(L"File", -1, true, true);
    136         menu->addItem(L"View", -1, true, true);
    137         menu->addItem(L"Camera", -1, true, true);
    138         menu->addItem(L"Help", -1, true, true);
    139 
    140         // disable alpha
    141 
    142         for (s32 i=0; i<gui::EGDC_COUNT ; ++i)
    143         {
    144                 video::SColor col = env->getSkin()->getColor((gui::EGUI_DEFAULT_COLOR)i);
    145                 col.setAlpha(255);
    146                 env->getSkin()->setColor((gui::EGUI_DEFAULT_COLOR)i, col);
    147         }
    148          */
    149 }
    150 
    151 Gui_impl::~Gui_impl()
    152 {
    153         //printf("del Gui_impl\n");
    154         device->drop();
    155 
    156         delete receiver;
    157         // printf("del Gui_impl ok\n");
    158 }
    159 
    160 void Gui_impl::setMesh(std::string file,vector3df position,vector3df rotation,vector3df scale)
    161 {
    162         IAnimatedMesh* mesh = smgr->getMesh(file.c_str());
    163 
    164         if (!mesh)      {
    165                 // model could not be loaded
    166         self->Err("Model %s could not be loaded\n",file.c_str());
    167         return;
    168         }
    169 
    170 
    171         node = smgr->addOctreeSceneNode(mesh->getMesh(0), 0, -1, 1024);
    172         node->setPosition(position);
    173         rotation+=irr::core::vector3df(90,0,Euler::ToDegree(getSimulator()->Yaw()));
    174         node->setRotation(rotation);
    175         for(int i=0; i<node->getMaterialCount();i++){
    176                 node->getMaterial(i).TextureLayer->TextureWrapU = video::ETC_REPEAT;
    177                 node->getMaterial(i).TextureLayer->TextureWrapV = video::ETC_REPEAT;
    178         }
    179         //Ceillig
    180         //node->getMaterial(0).getTextureMatrix(0).setTextureScale(scale.X/2.0,scale.Z/2.0);
    181         //Walls
    182         node->getMaterial(1).getTextureMatrix(0).setTextureScale(1/(scale.Y/2.5),1/(scale.Z/2.5));
    183         //Floor
    184         node->getMaterial(2).getTextureMatrix(0).setTextureScale(1/(scale.X/20.0),1/(scale.Z/20.0));
    185 
    186         node->setScale(scale);
    187         //selector
    188         selector = smgr->createOctreeTriangleSelector(node->getMesh(), node, 128);
    189         node->setTriangleSelector(selector);
    190 }
    191 
    192 void Gui_impl::RunGui(std::vector<Model*> models,std::vector<GenericObject*> objects)
    193 {
    194         int lastFPS = -1;
    195         int cam_id=0;
    196 
    197         receiver->SetCamera(models.at(cam_id)->pimpl_->animator);
    198         receiver->SetModel(models.at(cam_id));
    199 
    200         for(size_t i=0;i<models.size();i++)
    201         {
    202                 models.at(i)->Draw();
    203         }
    204 
    205         for(size_t i=0;i<models.size();i++)
    206         {
    207                 models.at(i)->pimpl_->MetaTriangleSelector()->addTriangleSelector(selector);
    208                 for(size_t j=0;j<objects.size();j++)
    209                 {
    210                         models.at(i)->pimpl_->MetaTriangleSelector()->addTriangleSelector(objects.at(j)->TriangleSelector());
    211                 }
    212                 for(size_t j=0;j<models.size();j++)
    213                 {
    214                         if(i==j) continue;
    215                         models.at(i)->pimpl_->MetaTriangleSelector()->addTriangleSelector(models.at(j)->pimpl_->TriangleSelector());
    216                 }
    217         }
    218 
    219         selector->drop(); // As soon as we're done with the selector, drop it.*/
    220 
    221         //wait all models to be started
    222         for(size_t i=0;i<models.size();i++)
    223         {
    224                 models.at(i)->pimpl_->SynchronizationPoint();
    225         }
    226 
    227         setWindowCaption(models.at(0),0);
    228 
    229         while(device->run())
    230         {
    231                 if(dbtFile_r!=NULL)//rejeu
    232                 {
    233                         takeScreenshot();//on enregistre l'image precedente
    234                         road_time_t time;
    235                         road_timerange_t tr = 0;
    236                         if(read_hdfile(dbtFile_r,(void*)dbtbuf,&time,&tr)!=0)
    237                         {
    238                                 vector3df vect;
    239                                 char *buf=dbtbuf;
    240                                 for(size_t i=0;i<models.size();i++)
    241                                 {
    242                                         models.at(i)->ReaddbtBuf(buf);
    243                                         buf+=models.at(i)->dbtSize();
    244                                 }
    245                         }
    246                         else
    247                         {
    248                                 //Printf("fin play\n");
    249                                 close_hdfile(dbtFile_r);
    250                                 dbtFile_r=NULL;
    251                                 free(dbtbuf);
    252                                 for(size_t i=0;i<models.size();i++)
    253                                 {
    254                                         models.at(i)->pimpl_->Resume();
    255                                 }
    256                         }
    257                 }
    258                 else//mode normal
    259                 {
    260                         for(size_t i=0;i<models.size();i++)
    261                         {
    262                                 models.at(i)->pimpl_->UpdatePos();
    263                         }
    264                 }
    265 
    266                 driver->beginScene(true, true, video::SColor(255,200,200,200));
    267 
    268                 //vue poursuite
    269                 smgr->setActiveCamera(models.at(cam_id)->pimpl_->camera);
    270                 driver->setViewPort(core::rect<s32>(0,0,scene_width,scene_height));
    271                 smgr->drawAll();//commente voir plus bas
    272                 /*
    273         env->drawAll();
    274 if (font)
    275 {
    276                                 font->draw(L"This demo shows that Irrlicht is also capable of drawing 2D graphics.",
    277                                         core::rect<s32>(130,10,300,50),
    278                                         video::SColor(255,255,255,255));
    279 }
    280 else
    281 {
    282     printf("err\n");
    283 }
    284 device->setWindowCaption(L"toto");*/
    285 
    286                 if(dbtFile_r==NULL)//mode normal
    287                 {
    288                         for(size_t i=0;i<models.size();i++)
    289                         {
    290                                 models.at(i)->pimpl_->CheckCollision();
    291                         }
    292                 }
    293 
    294                 for(size_t i=0;i<models.size();i++)
    295                 {
    296                         models.at(i)->ProcessUpdate(NULL);
    297                 }
    298 
    299                 //on fait ca ici, devrait etre un peu plus haut
    300                 //mais a priori souci avec models.at(i)->pimpl_->CheckCollision(); (setelipsoid?)
    301                 smgr->setActiveCamera(models.at(cam_id)->pimpl_->camera);
    302                 driver->setViewPort(core::rect<s32>(0,0,scene_width,scene_height));
    303                 smgr->drawAll();
    304 
    305                 driver->endScene();
    306 
    307                 int fps =driver->getFPS();
    308                 //printf("fps %i\n",fps);
    309                 if (lastFPS != fps)
    310                 {
    311                         setWindowCaption(models.at(cam_id),fps);
    312                         lastFPS = fps;
    313                 }
    314 
    315                 if(receiver->IsKeyDown(KEY_PRIOR))
    316                 {
    317                         cam_id++;
    318                         if(cam_id>=(int)models.size()) cam_id=0;
    319                         receiver->SetCamera(models.at(cam_id)->pimpl_->animator);
    320                         receiver->SetModel(models.at(cam_id));
    321                         setWindowCaption(models.at(cam_id),fps);
    322                 }
    323                 if(receiver->IsKeyDown(KEY_NEXT))
    324                 {
    325                         cam_id--;
    326                         if(cam_id<0) cam_id=models.size()-1;
    327                         receiver->SetCamera(models.at(cam_id)->pimpl_->animator);
    328                         receiver->SetModel(models.at(cam_id));
    329                         setWindowCaption(models.at(cam_id),fps);
    330                 }
    331 
    332                 //enregistrement DBT
    333                 if(receiver->IsKeyDown(KEY_KEY_R) && dbtFile_w==NULL)
    334                 {
    335                         dbtFile_w = inithdFile((char*)"./record.dbt",UAV,dbtSize(models));
    336                         dbtbuf=(char*)malloc(dbtSize(models));
    337                 }
    338                 if(receiver->IsKeyDown(KEY_KEY_S) && dbtFile_w!=NULL)
    339                 {
    340                         close_hdfile(dbtFile_w);
    341                         dbtFile_w=NULL;
    342                         free(dbtbuf);
    343                         //rt_printf("stop rec\n");
    344                 }
    345                 if(dbtFile_w!=NULL)
    346                 {
    347                         Time time=GetTime();
    348                         vector3df vect;
    349                         char *buf=dbtbuf;
    350 
    351                         for(size_t i=0;i<models.size();i++)
    352                         {
    353                                 models.at(i)->WritedbtBuf(buf);
    354                                 buf+=models.at(i)->dbtSize();
    355                         }
    356 
    357                         write_hdfile(dbtFile_w,dbtbuf,(road_time_t)(time/1000),(road_timerange_t)(time%1000),dbtSize(models));
    358                 }
    359 
    360                 //lecture dbt
    361                 if(receiver->IsKeyDown(KEY_KEY_P) && dbtFile_r==NULL)
    362                 {
    363                         dbtFile_r = open_hdfile((char*)"./record.dbt",READ_MODE);
    364                         dbtbuf=(char*)malloc(dbtSize(models));
    365                         //on suspend les models pour ne pas interferer
    366                         for(size_t i=0;i<models.size();i++)
    367                         {
    368                                 models.at(i)->pimpl_->Suspend();
    369                         }
    370                 }
    371                 if(receiver->IsKeyDown(KEY_KEY_S) && dbtFile_r!=NULL)
    372                 {
    373                         //rt_printf("stop play\n");
    374                         close_hdfile(dbtFile_r);
    375                         dbtFile_r=NULL;
    376                         free(dbtbuf);
    377                         //on resume les models
    378                         for(size_t i=0;i<models.size();i++)
    379                         {
    380                                 models.at(i)->pimpl_->Resume();
    381                         }
    382                 }
    383 
    384         }
    385 
    386         receiver->SetCamera(NULL);
    387         receiver->SetModel(NULL);
    388 
    389 }
    390 
    391 void Gui_impl::setWindowCaption(Object* object, int fps)
    392 {
    393         std::ostringstream text;
    394         text << "Cam: " << object->ObjectName().c_str() << ", Kbd: " << object->ObjectName().c_str() << ", FPS: " << fps;
    395 
    396         device->setWindowCaption(stringw(text.str().c_str()).c_str());
    397 }
    398 
    399 void Gui_impl::takeScreenshot(void)
    400 {
    401         static int cpt=0;
    402         //get image from the last rendered frame
    403         IImage* const image = driver->createScreenShot();
    404         if (image) //should always be true, but you never know. ;)
    405         {
    406                 //construct a filename, consisting of local time and file extension
    407                 c8 filename[64];
    408                 //snprintf(filename, 64, "screenshot_%u.png", device->getTimer()->getRealTime());
    409                 snprintf(filename, 64, "screenshot_%u.png", cpt);
    410                 cpt++;
    411                 //write screenshot to file
    412                 if (!driver->writeImageToFile(image, filename))
    413                         device->getLogger()->log(L"Failed to take screenshot.", ELL_WARNING);
    414 
    415                 //Don't forget to drop image since we don't need it anymore.
    416                 image->drop();
    417         }
    418 }
    419 
    420 size_t Gui_impl::dbtSize(std::vector<Model*> models)
    421 {
    422         size_t size=0;
    423         for(size_t i=0;i<models.size();i++)
    424         {
    425                 size+=models.at(i)->dbtSize();
    426         }
    427 
    428         return size;
    429 }
    430 #endif //GL
     114  font = env->getFont("./fonthaettenschweiler.bmp");
     115
     116  if (font)
     117          skin->setFont(font);
     118
     119  // create menu
     120
     121  IGUIContextMenu* menu = env->addMenu();
     122  menu->setMinSize(core::dimension2du(640,20));
     123  menu->addItem(L"File", -1, true, true);
     124  menu->addItem(L"View", -1, true, true);
     125  menu->addItem(L"Camera", -1, true, true);
     126  menu->addItem(L"Help", -1, true, true);
     127
     128  // disable alpha
     129
     130  for (s32 i=0; i<gui::EGDC_COUNT ; ++i)
     131  {
     132          video::SColor col =
     133env->getSkin()->getColor((gui::EGUI_DEFAULT_COLOR)i);
     134          col.setAlpha(255);
     135          env->getSkin()->setColor((gui::EGUI_DEFAULT_COLOR)i, col);
     136  }
     137   */
     138}
     139
     140Gui_impl::~Gui_impl() {
     141  // printf("del Gui_impl\n");
     142  device->drop();
     143
     144  delete receiver;
     145  // printf("del Gui_impl ok\n");
     146}
     147
     148void Gui_impl::setMesh(std::string file, vector3df position, vector3df rotation,
     149                       vector3df scale) {
     150  IAnimatedMesh *mesh = smgr->getMesh(file.c_str());
     151
     152  if (!mesh) {
     153    // model could not be loaded
     154    self->Err("Model %s could not be loaded\n", file.c_str());
     155    return;
     156  }
     157
     158  node = smgr->addOctreeSceneNode(mesh->getMesh(0), 0, -1, 1024);
     159  node->setPosition(position);
     160  rotation +=
     161      irr::core::vector3df(90, 0, Euler::ToDegree(getSimulator()->Yaw()));
     162  node->setRotation(rotation);
     163  for (int i = 0; i < node->getMaterialCount(); i++) {
     164    node->getMaterial(i).TextureLayer->TextureWrapU = video::ETC_REPEAT;
     165    node->getMaterial(i).TextureLayer->TextureWrapV = video::ETC_REPEAT;
     166  }
     167  // Ceillig
     168  // node->getMaterial(0).getTextureMatrix(0).setTextureScale(scale.X/2.0,scale.Z/2.0);
     169  // Walls
     170  node->getMaterial(1).getTextureMatrix(0).setTextureScale(1 / (scale.Y / 2.5),
     171                                                           1 / (scale.Z / 2.5));
     172  // Floor
     173  node->getMaterial(2).getTextureMatrix(0).setTextureScale(
     174      1 / (scale.X / 20.0), 1 / (scale.Z / 20.0));
     175
     176  node->setScale(scale);
     177  // selector
     178  selector = smgr->createOctreeTriangleSelector(node->getMesh(), node, 128);
     179  node->setTriangleSelector(selector);
     180}
     181
     182void Gui_impl::RunGui(std::vector<Model *> models,
     183                      std::vector<GenericObject *> objects) {
     184  int lastFPS = -1;
     185  int cam_id = 0;
     186
     187  receiver->SetCamera(models.at(cam_id)->pimpl_->animator);
     188  receiver->SetModel(models.at(cam_id));
     189
     190  for (size_t i = 0; i < models.size(); i++) {
     191    models.at(i)->Draw();
     192  }
     193
     194  for (size_t i = 0; i < models.size(); i++) {
     195    models.at(i)->pimpl_->MetaTriangleSelector()->addTriangleSelector(selector);
     196    for (size_t j = 0; j < objects.size(); j++) {
     197      models.at(i)->pimpl_->MetaTriangleSelector()->addTriangleSelector(
     198          objects.at(j)->TriangleSelector());
     199    }
     200    for (size_t j = 0; j < models.size(); j++) {
     201      if (i == j)
     202        continue;
     203      models.at(i)->pimpl_->MetaTriangleSelector()->addTriangleSelector(
     204          models.at(j)->pimpl_->TriangleSelector());
     205    }
     206  }
     207
     208  selector->drop(); // As soon as we're done with the selector, drop it.*/
     209
     210  // wait all models to be started
     211  for (size_t i = 0; i < models.size(); i++) {
     212    models.at(i)->pimpl_->SynchronizationPoint();
     213  }
     214
     215  setWindowCaption(models.at(0), 0);
     216
     217  while (device->run()) {
     218    if (dbtFile_r != NULL) // rejeu
     219    {
     220      takeScreenshot(); // on enregistre l'image precedente
     221      road_time_t time;
     222      road_timerange_t tr = 0;
     223      if (read_hdfile(dbtFile_r, (void *)dbtbuf, &time, &tr) != 0) {
     224        vector3df vect;
     225        char *buf = dbtbuf;
     226        for (size_t i = 0; i < models.size(); i++) {
     227          models.at(i)->ReaddbtBuf(buf);
     228          buf += models.at(i)->dbtSize();
     229        }
     230      } else {
     231        // Printf("fin play\n");
     232        close_hdfile(dbtFile_r);
     233        dbtFile_r = NULL;
     234        free(dbtbuf);
     235        for (size_t i = 0; i < models.size(); i++) {
     236          models.at(i)->pimpl_->Resume();
     237        }
     238      }
     239    } else // mode normal
     240    {
     241      for (size_t i = 0; i < models.size(); i++) {
     242        models.at(i)->pimpl_->UpdatePos();
     243      }
     244    }
     245
     246    driver->beginScene(true, true, video::SColor(255, 200, 200, 200));
     247
     248    // vue poursuite
     249    smgr->setActiveCamera(models.at(cam_id)->pimpl_->camera);
     250    driver->setViewPort(core::rect<s32>(0, 0, scene_width, scene_height));
     251    smgr->drawAll(); // commente voir plus bas
     252                     /*
     253                 env->drawAll();
     254                 if (font)
     255                 {
     256                                     font->draw(L"This demo shows that Irrlicht is also capable
     257                 of drawing 2D graphics.",
     258                                             core::rect<s32>(130,10,300,50),
     259                                             video::SColor(255,255,255,255));
     260                 }
     261                 else
     262                 {
     263                 printf("err\n");
     264                 }
     265                 device->setWindowCaption(L"toto");*/
     266
     267    if (dbtFile_r == NULL) // mode normal
     268    {
     269      for (size_t i = 0; i < models.size(); i++) {
     270        models.at(i)->pimpl_->CheckCollision();
     271      }
     272    }
     273
     274    for (size_t i = 0; i < models.size(); i++) {
     275      models.at(i)->ProcessUpdate(NULL);
     276    }
     277
     278    // on fait ca ici, devrait etre un peu plus haut
     279    // mais a priori souci avec models.at(i)->pimpl_->CheckCollision();
     280    // (setelipsoid?)
     281    smgr->setActiveCamera(models.at(cam_id)->pimpl_->camera);
     282    driver->setViewPort(core::rect<s32>(0, 0, scene_width, scene_height));
     283    smgr->drawAll();
     284
     285    driver->endScene();
     286
     287    int fps = driver->getFPS();
     288    // printf("fps %i\n",fps);
     289    if (lastFPS != fps) {
     290      setWindowCaption(models.at(cam_id), fps);
     291      lastFPS = fps;
     292    }
     293
     294    if (receiver->IsKeyDown(KEY_PRIOR)) {
     295      cam_id++;
     296      if (cam_id >= (int)models.size())
     297        cam_id = 0;
     298      receiver->SetCamera(models.at(cam_id)->pimpl_->animator);
     299      receiver->SetModel(models.at(cam_id));
     300      setWindowCaption(models.at(cam_id), fps);
     301    }
     302    if (receiver->IsKeyDown(KEY_NEXT)) {
     303      cam_id--;
     304      if (cam_id < 0)
     305        cam_id = models.size() - 1;
     306      receiver->SetCamera(models.at(cam_id)->pimpl_->animator);
     307      receiver->SetModel(models.at(cam_id));
     308      setWindowCaption(models.at(cam_id), fps);
     309    }
     310
     311    // enregistrement DBT
     312    if (receiver->IsKeyDown(KEY_KEY_R) && dbtFile_w == NULL) {
     313      dbtFile_w = inithdFile((char *)"./record.dbt", UAV, dbtSize(models));
     314      dbtbuf = (char *)malloc(dbtSize(models));
     315    }
     316    if (receiver->IsKeyDown(KEY_KEY_S) && dbtFile_w != NULL) {
     317      close_hdfile(dbtFile_w);
     318      dbtFile_w = NULL;
     319      free(dbtbuf);
     320      // rt_printf("stop rec\n");
     321    }
     322    if (dbtFile_w != NULL) {
     323      Time time = GetTime();
     324      vector3df vect;
     325      char *buf = dbtbuf;
     326
     327      for (size_t i = 0; i < models.size(); i++) {
     328        models.at(i)->WritedbtBuf(buf);
     329        buf += models.at(i)->dbtSize();
     330      }
     331
     332      write_hdfile(dbtFile_w, dbtbuf, (road_time_t)(time / 1000),
     333                   (road_timerange_t)(time % 1000), dbtSize(models));
     334    }
     335
     336    // lecture dbt
     337    if (receiver->IsKeyDown(KEY_KEY_P) && dbtFile_r == NULL) {
     338      dbtFile_r = open_hdfile((char *)"./record.dbt", READ_MODE);
     339      dbtbuf = (char *)malloc(dbtSize(models));
     340      // on suspend les models pour ne pas interferer
     341      for (size_t i = 0; i < models.size(); i++) {
     342        models.at(i)->pimpl_->Suspend();
     343      }
     344    }
     345    if (receiver->IsKeyDown(KEY_KEY_S) && dbtFile_r != NULL) {
     346      // rt_printf("stop play\n");
     347      close_hdfile(dbtFile_r);
     348      dbtFile_r = NULL;
     349      free(dbtbuf);
     350      // on resume les models
     351      for (size_t i = 0; i < models.size(); i++) {
     352        models.at(i)->pimpl_->Resume();
     353      }
     354    }
     355  }
     356
     357  receiver->SetCamera(NULL);
     358  receiver->SetModel(NULL);
     359}
     360
     361void Gui_impl::setWindowCaption(Object *object, int fps) {
     362  std::ostringstream text;
     363  text << "Cam: " << object->ObjectName().c_str()
     364       << ", Kbd: " << object->ObjectName().c_str() << ", FPS: " << fps;
     365
     366  device->setWindowCaption(stringw(text.str().c_str()).c_str());
     367}
     368
     369void Gui_impl::takeScreenshot(void) {
     370  static int cpt = 0;
     371  // get image from the last rendered frame
     372  IImage *const image = driver->createScreenShot();
     373  if (image) // should always be true, but you never know. ;)
     374  {
     375    // construct a filename, consisting of local time and file extension
     376    c8 filename[64];
     377    // snprintf(filename, 64, "screenshot_%u.png",
     378    // device->getTimer()->getRealTime());
     379    snprintf(filename, 64, "screenshot_%u.png", cpt);
     380    cpt++;
     381    // write screenshot to file
     382    if (!driver->writeImageToFile(image, filename))
     383      device->getLogger()->log(L"Failed to take screenshot.", ELL_WARNING);
     384
     385    // Don't forget to drop image since we don't need it anymore.
     386    image->drop();
     387  }
     388}
     389
     390size_t Gui_impl::dbtSize(std::vector<Model *> models) {
     391  size_t size = 0;
     392  for (size_t i = 0; i < models.size(); i++) {
     393    size += models.at(i)->dbtSize();
     394  }
     395
     396  return size;
     397}
     398#endif // GL

  • trunk/lib/FlairSimulator/src/Man.cpp

    r10 r15  
    3434using namespace flair::gui;
    3535
    36 namespace flair
    37 {
    38 namespace simulator
    39 {
     36namespace flair {
     37namespace simulator {
    4038
    41 Man::Man(const Simulator* parent,std::string name): Model(parent,name)
    42 {
    43     node = getGui()->getSceneManager()->addAnimatedMeshSceneNode(getGui()->getMesh("ninja.b3d"),getSceneNode(),-1,
    44                                                                  vector3df(0,0,0),vector3df(90,0,90),vector3df(10,10,10));
     39Man::Man(const Simulator *parent, std::string name) : Model(parent, name) {
     40  node = getGui()->getSceneManager()->addAnimatedMeshSceneNode(
     41      getGui()->getMesh("ninja.b3d"), getSceneNode(), -1, vector3df(0, 0, 0),
     42      vector3df(90, 0, 90), vector3df(10, 10, 10));
    4543
    46         node->setFrameLoop(0, 13);
    47         node->setAnimationSpeed(0);
    48         node->getMaterial(0).NormalizeNormals = true;
    49         node->getMaterial(0).Lighting = false;
     44  node->setFrameLoop(0, 13);
     45  node->setAnimationSpeed(0);
     46  node->getMaterial(0).NormalizeNormals = true;
     47  node->getMaterial(0).Lighting = false;
    5048
    51         getCamera()->setPositionOffset(vector3df(0,0,100));
    52     getCamera()->setTargetOffset(vector3df(0,0,100));
     49  getCamera()->setPositionOffset(vector3df(0, 0, 100));
     50  getCamera()->setTargetOffset(vector3df(0, 0, 100));
    5351
    54     setTriangleSelector(getGui()->getSceneManager()->createTriangleSelector(node));
    55     Box()->addInternalBox(node->getTransformedBoundingBox());
     52  setTriangleSelector(
     53      getGui()->getSceneManager()->createTriangleSelector(node));
     54  Box()->addInternalBox(node->getTransformedBoundingBox());
    5655
    57     Tab *setup_tab=new Tab(GetTabWidget(),"model");
    58         t_speed=new DoubleSpinBox(setup_tab->NewRow(),"translational speed (m/s):",0,5,0.1);
    59         r_speed=new DoubleSpinBox(setup_tab->NewRow(),"rotational speed (deg/s):",0,180,10);
     56  Tab *setup_tab = new Tab(GetTabWidget(), "model");
     57  t_speed = new DoubleSpinBox(setup_tab->NewRow(), "translational speed (m/s):",
     58                              0, 5, 0.1);
     59  r_speed = new DoubleSpinBox(setup_tab->NewRow(), "rotational speed (deg/s):",
     60                              0, 180, 10);
    6061}
    6162
    62 Man::~Man()
    63 {
     63Man::~Man() {}
    6464
     65void Man::CalcModel(void) {
     66  // compute quaternion from W
     67  // Quaternion derivative: dQ = 0.5*(Q*Qw)
     68  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
     69
     70  // Quaternion integration
     71  state[0].Quat = state[-1].Quat + dQ * dT();
     72  state[0].Quat.Normalize();
     73
     74  Vector3D dir = state[0].Vel;
     75  dir.Rotate(state[0].Quat);
     76  state[0].Pos = state[-1].Pos + dT() * dir;
    6577}
    6678
    67 void Man::CalcModel(void)
    68 {
    69     // compute quaternion from W
    70     // Quaternion derivative: dQ = 0.5*(Q*Qw)
    71     Quaternion dQ=state[-1].Quat.GetDerivative(state[0].W);
     79bool Man::OnEvent(const SEvent &event) {
     80  if (event.EventType != EET_KEY_INPUT_EVENT)
     81    return false;
    7282
    73     // Quaternion integration
    74     state[0].Quat = state[-1].Quat +dQ*dT();
    75     state[0].Quat.Normalize();
     83  if (event.KeyInput.PressedDown == false) {
     84    state[0].Vel.x = 0;
     85    state[0].W.z = 0;
     86    node->setAnimationSpeed(0);
     87  } else {
     88    switch (event.KeyInput.Key) {
     89    case KEY_UP:
     90      state[0].Vel.x = t_speed->Value();
     91      node->setAnimationSpeed(t_speed->Value() * 10.f);
     92      break;
     93    case KEY_DOWN:
     94      state[0].Vel.x = -t_speed->Value();
     95      node->setAnimationSpeed(-t_speed->Value() * 10.f);
     96      break;
     97    case KEY_LEFT:
     98      state[0].W.z = -Euler::ToRadian(r_speed->Value());
     99      node->setAnimationSpeed(r_speed->Value() * .15f);
     100      break;
     101    case KEY_RIGHT:
     102      state[0].W.z = Euler::ToRadian(r_speed->Value());
     103      node->setAnimationSpeed(-r_speed->Value() * .15f);
     104      break;
     105    default:
     106      return false;
     107      break;
     108    }
     109  }
    76110
    77     Vector3D dir=state[0].Vel;
    78     dir.Rotate(state[0].Quat);
    79     state[0].Pos=state[-1].Pos+dT()*dir;
     111  return true;
    80112}
    81113
    82 bool Man::OnEvent(const SEvent& event)
    83 {
    84     if (event.EventType != EET_KEY_INPUT_EVENT)
    85         return false;
     114size_t Man::dbtSize(void) const { return 6 * sizeof(float); }
    86115
    87     if(event.KeyInput.PressedDown==false)
    88     {
    89         state[0].Vel.x=0;
    90         state[0].W.z=0;
    91         node->setAnimationSpeed(0);
    92     }
    93     else
    94     {
    95         switch(event.KeyInput.Key)
    96         {
    97         case KEY_UP:
    98             state[0].Vel.x=t_speed->Value();
    99             node->setAnimationSpeed(t_speed->Value()*10.f);
    100             break;
    101         case KEY_DOWN:
    102             state[0].Vel.x=-t_speed->Value();
    103             node->setAnimationSpeed(-t_speed->Value()*10.f);
    104             break;
    105         case KEY_LEFT:
    106             state[0].W.z=-Euler::ToRadian(r_speed->Value());
    107             node->setAnimationSpeed(r_speed->Value()*.15f);
    108             break;
    109         case KEY_RIGHT:
    110             state[0].W.z=Euler::ToRadian(r_speed->Value());
    111             node->setAnimationSpeed(-r_speed->Value()*.15f);
    112             break;
    113         default:
    114             return false;
    115             break;
    116         }
    117     }
    118 
    119     return true;
     116void Man::WritedbtBuf(char *dbtbuf) { /*
     117                                         float *buf=(float*)dbtbuf;
     118                                         vector3df vect=node->getPosition();
     119                                         memcpy(buf,&vect.X,sizeof(float));
     120                                         buf++;
     121                                         memcpy(buf,&vect.Y,sizeof(float));
     122                                         buf++;
     123                                         memcpy(buf,&vect.Z,sizeof(float));
     124                                         buf++;
     125                                         vect=node->getRotation();
     126                                         memcpy(buf,&vect.X,sizeof(float));
     127                                         buf++;
     128                                         memcpy(buf,&vect.Y,sizeof(float));
     129                                         buf++;
     130                                         memcpy(buf,&vect.Z,sizeof(float));
     131                                         buf++;*/
    120132}
    121133
    122 size_t Man::dbtSize(void) const
    123 {
    124     return 6*sizeof(float);
    125 }
    126 
    127 void Man::WritedbtBuf(char* dbtbuf)
    128 {/*
    129     float *buf=(float*)dbtbuf;
    130     vector3df vect=node->getPosition();
    131     memcpy(buf,&vect.X,sizeof(float));
    132     buf++;
    133     memcpy(buf,&vect.Y,sizeof(float));
    134     buf++;
    135     memcpy(buf,&vect.Z,sizeof(float));
    136     buf++;
    137     vect=node->getRotation();
    138     memcpy(buf,&vect.X,sizeof(float));
    139     buf++;
    140     memcpy(buf,&vect.Y,sizeof(float));
    141     buf++;
    142     memcpy(buf,&vect.Z,sizeof(float));
    143     buf++;*/
    144 }
    145 
    146 void Man::ReaddbtBuf(char* dbtbuf)
    147 {/*
    148     float *buf=(float*)dbtbuf;
    149     vector3df vect;
    150     memcpy(&vect.X,buf,sizeof(float));
    151     buf++;
    152     memcpy(&vect.Y,buf,sizeof(float));
    153     buf++;
    154     memcpy(&vect.Z,buf,sizeof(float));
    155     buf++;
    156     node->setPosition(vect);
    157     memcpy(&vect.X,buf,sizeof(float));
    158     buf++;
    159     memcpy(&vect.Y,buf,sizeof(float));
    160     buf++;
    161     memcpy(&vect.Z,buf,sizeof(float));
    162     buf++;
    163     node->setRotation(vect);
    164     node->setAnimationSpeed(2.f);*/
     134void Man::ReaddbtBuf(char *dbtbuf) { /*
     135                                        float *buf=(float*)dbtbuf;
     136                                        vector3df vect;
     137                                        memcpy(&vect.X,buf,sizeof(float));
     138                                        buf++;
     139                                        memcpy(&vect.Y,buf,sizeof(float));
     140                                        buf++;
     141                                        memcpy(&vect.Z,buf,sizeof(float));
     142                                        buf++;
     143                                        node->setPosition(vect);
     144                                        memcpy(&vect.X,buf,sizeof(float));
     145                                        buf++;
     146                                        memcpy(&vect.Y,buf,sizeof(float));
     147                                        buf++;
     148                                        memcpy(&vect.Z,buf,sizeof(float));
     149                                        buf++;
     150                                        node->setRotation(vect);
     151                                        node->setAnimationSpeed(2.f);*/
    165152}
    166153
    167154} // end namespace simulator
    168155} // end namespace flair
    169 #endif //GL
     156#endif // GL

  • trunk/lib/FlairSimulator/src/Man.h

    r10 r15  
    2020#include <Model.h>
    2121
    22 namespace irr
    23 {
    24     namespace scene
    25     {
    26         class IAnimatedMeshSceneNode;
    27     }
     22namespace irr {
     23namespace scene {
     24class IAnimatedMeshSceneNode;
     25}
    2826}
    2927
    30 namespace flair
    31 {
    32     namespace gui
    33     {
    34         class DoubleSpinBox;
    35     }
     28namespace flair {
     29namespace gui {
     30class DoubleSpinBox;
     31}
    3632}
    3733
    38 namespace flair
    39 {
    40 namespace simulator
    41 {
    42     class Simulator;
     34namespace flair {
     35namespace simulator {
     36class Simulator;
    4337
    44     class Man: private Model
    45     {
    46         public:
    47             Man(const Simulator* parent,std::string name);
    48             ~Man();
     38class Man : private Model {
     39public:
     40  Man(const Simulator *parent, std::string name);
     41  ~Man();
    4942
    50         private:
    51             size_t dbtSize(void) const;
    52             void WritedbtBuf(char* dbtbuf);
    53             void ReaddbtBuf(char* dbtbuf);
    54             void CalcModel(void);
    55             void AnimateModel(void){};
    56             bool OnEvent(const irr::SEvent& event);
     43private:
     44  size_t dbtSize(void) const;
     45  void WritedbtBuf(char *dbtbuf);
     46  void ReaddbtBuf(char *dbtbuf);
     47  void CalcModel(void);
     48  void AnimateModel(void){};
     49  bool OnEvent(const irr::SEvent &event);
    5750
    58             irr::scene::IAnimatedMeshSceneNode* node;
    59             gui::DoubleSpinBox *t_speed,*r_speed;
    60     };
     51  irr::scene::IAnimatedMeshSceneNode *node;
     52  gui::DoubleSpinBox *t_speed, *r_speed;
     53};
    6154} // end namespace simulator
    6255} // end namespace flair

  • trunk/lib/FlairSimulator/src/MeshSceneNode.cpp

    r10 r15  
    2828using namespace flair::core;
    2929
    30 namespace flair
    31 {
    32 namespace simulator
    33 {
     30namespace flair {
     31namespace simulator {
    3432
    35 MeshSceneNode::MeshSceneNode(Model* parent,IMesh* mesh,
    36                         const vector3df& position ,const vector3df& rotation,
    37                         ITexture* texture, s32 id ): IMeshSceneNode(parent->getSceneNode(), getGui()->getSceneManager(), id,position,rotation)
    38 {
    39     Material.Wireframe = false;
    40     Material.Lighting = false;
     33MeshSceneNode::MeshSceneNode(Model *parent, IMesh *mesh,
     34                             const vector3df &position,
     35                             const vector3df &rotation, ITexture *texture,
     36                             s32 id)
     37    : IMeshSceneNode(parent->getSceneNode(), getGui()->getSceneManager(), id,
     38                     position, rotation) {
     39  Material.Wireframe = false;
     40  Material.Lighting = false;
    4141
    42     setMesh(mesh);
     42  setMesh(mesh);
    4343
    44     if(texture!=NULL)
    45     {
    46         setMaterialTexture(0,texture);
    47     }
     44  if (texture != NULL) {
     45    setMaterialTexture(0, texture);
     46  }
    4847
    49     parent->Box()->addInternalBox(getTransformedBoundingBox());
     48  parent->Box()->addInternalBox(getTransformedBoundingBox());
    5049}
    5150
     51void MeshSceneNode::OnRegisterSceneNode(void) {
     52  if (IsVisible)
     53    SceneManager->registerNodeForRendering(this);
    5254
    53 void MeshSceneNode::OnRegisterSceneNode(void)
    54 {
    55     if (IsVisible)
    56         SceneManager->registerNodeForRendering(this);
    57 
    58     ISceneNode::OnRegisterSceneNode();
     55  ISceneNode::OnRegisterSceneNode();
    5956}
    6057
    61 void MeshSceneNode::render(void)
    62 {
    63     IVideoDriver* driver = SceneManager->getVideoDriver();
     58void MeshSceneNode::render(void) {
     59  IVideoDriver *driver = SceneManager->getVideoDriver();
    6460
    65     driver->setMaterial(Material);
    66     driver->setTransform(ETS_WORLD, AbsoluteTransformation);
    67     driver->drawMeshBuffer(mesh->getMeshBuffer(0));
     61  driver->setMaterial(Material);
     62  driver->setTransform(ETS_WORLD, AbsoluteTransformation);
     63  driver->drawMeshBuffer(mesh->getMeshBuffer(0));
    6864}
    6965
     66SMaterial &MeshSceneNode::getMaterial(u32 i) { return Material; }
    7067
    71 SMaterial& MeshSceneNode::getMaterial(u32 i)
    72 {
    73     return Material;
     68void MeshSceneNode::setMesh(IMesh *ptr) {
     69  mesh = ptr;
     70  Box = mesh->getBoundingBox();
    7471}
    7572
    76 void MeshSceneNode::setMesh(IMesh* ptr)
    77 {
    78     mesh=ptr;
    79     Box=mesh->getBoundingBox();
    80 }
     73IMesh *MeshSceneNode::getMesh(void) { return mesh; }
    8174
    82 
    83 IMesh* MeshSceneNode::getMesh(void)
    84 {
    85     return mesh;
    86 }
    87 
    88 
    89 void MeshSceneNode::setReadOnlyMaterials(bool readonly)
    90 {
    91 
    92 }
     75void MeshSceneNode::setReadOnlyMaterials(bool readonly) {}
    9376
    9477} // end namespace simulator
    9578} // end namespace flair
    96 #endif //GL
     79#endif // GL

  • trunk/lib/FlairSimulator/src/MeshSceneNode.h

    r10 r15  
    2020#include <IMeshSceneNode.h>
    2121
    22 namespace flair
    23 {
    24 namespace simulator
    25 {
    26     class Model;
     22namespace flair {
     23namespace simulator {
     24class Model;
    2725
    28     class MeshSceneNode : public irr::scene::IMeshSceneNode
    29     {
    30         public:
    31             MeshSceneNode(Model* parent,irr::scene::IMesh* mesh,
    32                 const irr::core::vector3df& position = irr::core::vector3df(0,0,0),
    33                 const irr::core::vector3df& rotation = irr::core::vector3df(0,0,0),
    34                 irr::video::ITexture* texture=NULL, irr::s32 id=-1);
     26class MeshSceneNode : public irr::scene::IMeshSceneNode {
     27public:
     28  MeshSceneNode(
     29      Model *parent, irr::scene::IMesh *mesh,
     30      const irr::core::vector3df &position = irr::core::vector3df(0, 0, 0),
     31      const irr::core::vector3df &rotation = irr::core::vector3df(0, 0, 0),
     32      irr::video::ITexture *texture = NULL, irr::s32 id = -1);
    3533
    36             virtual void OnRegisterSceneNode(void);
    37             virtual void render(void);
    38             virtual const irr::core::aabbox3d<irr::f32>& getBoundingBox() const
    39             {
    40                 return Box;
    41             }
    42             virtual irr::u32 getMaterialCount(void) const
    43             {
    44                 return 1;
    45             }
    46             virtual irr::video::SMaterial& getMaterial(irr::u32 i);
    47             virtual void setMesh( irr::scene::IMesh* ptr);
    48             virtual  irr::scene::IMesh* getMesh(void);
    49             virtual void setReadOnlyMaterials(bool readonly);
    50             virtual bool isReadOnlyMaterials(void) const
    51             {
    52                 return false;
    53             }
    54             virtual irr::scene::IShadowVolumeSceneNode* addShadowVolumeSceneNode(const irr::scene::IMesh* shadowMesh=0,
    55             irr::s32 id=-1, bool zfailmethod=true, irr::f32 infinity=1000.0f)
    56             {
    57                 return NULL;
    58             }
     34  virtual void OnRegisterSceneNode(void);
     35  virtual void render(void);
     36  virtual const irr::core::aabbox3d<irr::f32> &getBoundingBox() const {
     37    return Box;
     38  }
     39  virtual irr::u32 getMaterialCount(void) const { return 1; }
     40  virtual irr::video::SMaterial &getMaterial(irr::u32 i);
     41  virtual void setMesh(irr::scene::IMesh *ptr);
     42  virtual irr::scene::IMesh *getMesh(void);
     43  virtual void setReadOnlyMaterials(bool readonly);
     44  virtual bool isReadOnlyMaterials(void) const { return false; }
     45  virtual irr::scene::IShadowVolumeSceneNode *
     46  addShadowVolumeSceneNode(const irr::scene::IMesh *shadowMesh = 0,
     47                           irr::s32 id = -1, bool zfailmethod = true,
     48                           irr::f32 infinity = 1000.0f) {
     49    return NULL;
     50  }
    5951
    60         private:
    61             irr::scene::IMesh *mesh;
    62             irr::core::aabbox3d<irr::f32> Box;
    63             irr::video::SMaterial Material;
    64     };
     52private:
     53  irr::scene::IMesh *mesh;
     54  irr::core::aabbox3d<irr::f32> Box;
     55  irr::video::SMaterial Material;
     56};
    6557} // end namespace simulator
    6658} // end namespace flair

  • trunk/lib/FlairSimulator/src/Model.cpp

    r10 r15  
    3232using namespace flair::gui;
    3333
    34 namespace flair
    35 {
    36 namespace simulator
    37 {
     34namespace flair {
     35namespace simulator {
    3836
    39 Model::Model(const Simulator* parent,std::string name) : IODevice(parent,name)
    40 {
     37Model::Model(const Simulator *parent, std::string name)
     38    : IODevice(parent, name) {
    4139#ifdef GL
    42     pimpl_=new Model_impl(this,name,getGui()->getSceneManager(),parent->pimpl_);
     40  pimpl_ =
     41      new Model_impl(this, name, getGui()->getSceneManager(), parent->pimpl_);
    4342#else
    44     pimpl_=new Model_impl(this,name,parent->pimpl_);
     43  pimpl_ = new Model_impl(this, name, parent->pimpl_);
    4544#endif
    46     parent->pimpl_->models.push_back(this);
     45  parent->pimpl_->models.push_back(this);
    4746}
    4847
    49 Model::~Model()
    50 {
    51     delete pimpl_;
    52 }
     48Model::~Model() { delete pimpl_; }
    5349#ifdef GL
    54 ISceneNode* Model::getSceneNode() const
    55 {
    56     return pimpl_;
    57 }
     50ISceneNode *Model::getSceneNode() const { return pimpl_; }
    5851/*
    5952only used by dbt, to rewrite using conversion functions (irrlicht <-> simulator)
    … …  
    6457}
    6558*/
    66 void Model::setScale(float value)
    67 {
    68     pimpl_->setScale(vector3df(value,value,value));
     59void Model::setScale(float value) {
     60  pimpl_->setScale(vector3df(value, value, value));
    6961}
    7062
    71 aabbox3d<f32> *Model::Box() const
    72 {
    73     return &(pimpl_->box);
     63aabbox3d<f32> *Model::Box() const { return &(pimpl_->box); }
     64
     65AnimPoursuite *Model::getCamera(void) const { return pimpl_->animator; }
     66
     67void Model::setTriangleSelector(ITriangleSelector *selector) {
     68
     69  pimpl_->selector = selector;
     70  pimpl_->setTriangleSelector(selector);
    7471}
    7572
    76 AnimPoursuite* Model::getCamera(void) const
    77 {
    78     return pimpl_->animator;
    79 }
    80 
    81 void Model::setTriangleSelector(ITriangleSelector* selector) {
    82 
    83     pimpl_->selector=selector;
    84     pimpl_->setTriangleSelector(selector);
    85 }
    86 
    87 void Model::setCameraFarValue(float zf) {
    88     pimpl_->camera->setFarValue(zf);
    89 }
     73void Model::setCameraFarValue(float zf) { pimpl_->camera->setFarValue(zf); }
    9074#endif
    9175
    92 TabWidget* Model::GetTabWidget(void) const
    93 {
    94     return pimpl_->tabwidget;
    95 }
     76TabWidget *Model::GetTabWidget(void) const { return pimpl_->tabwidget; }
    9677
    97 float Model::dT(void) const
    98 {
    99     return pimpl_->dT->Value();
    100 }
     78float Model::dT(void) const { return pimpl_->dT->Value(); }
    10179
    10280} // end namespace simulator

  • trunk/lib/FlairSimulator/src/Model.h

    r10 r15  
    2525#ifdef GL
    2626#include <aabbox3d.h>
    27 namespace irr
    28 {
    29     class SEvent;
    30     namespace scene
    31     {
    32         class ISceneManager;
    33         class ISceneNode;
    34         class ITriangleSelector;
    35     }
     27namespace irr {
     28class SEvent;
     29namespace scene {
     30class ISceneManager;
     31class ISceneNode;
     32class ITriangleSelector;
     33}
    3634}
    3735#endif
    3836
    39 namespace flair
    40 {
    41     namespace gui
    42     {
    43         class TabWidget;
    44     }
    45     namespace sensor
    46     {
    47         class SensorGL;
    48     }
     37namespace flair {
     38namespace gui {
     39class TabWidget;
     40}
     41namespace sensor {
     42class SensorGL;
     43}
    4944}
    5045
    … …  
    5348class Model_impl;
    5449
    55 namespace flair
    56 {
    57 namespace simulator
    58 {
    59     class Simulator;
    60     class AnimPoursuite;
     50namespace flair {
     51namespace simulator {
     52class Simulator;
     53class AnimPoursuite;
    6154
    62     class Model : public core::IODevice
    63     {
    64         friend class ::Gui_impl;
    65         friend class ::Simulator_impl;
    66         friend class ::Model_impl;
    67         friend class AnimPoursuite;
    68         friend class sensor::SensorGL;
     55class Model : public core::IODevice {
     56  friend class ::Gui_impl;
     57  friend class ::Simulator_impl;
     58  friend class ::Model_impl;
     59  friend class AnimPoursuite;
     60  friend class sensor::SensorGL;
    6961
    70         public:
    71             Model(const Simulator* parent,std::string name);
    72             virtual ~Model();
     62public:
     63  Model(const Simulator *parent, std::string name);
     64  virtual ~Model();
    7365
     66  typedef struct simu_state {
     67    core::Quaternion Quat;
     68    core::Vector3D W;
     69    core::Vector3D Pos;
     70    core::Vector3D Vel;
     71  } simu_state_t;
    7472
    75             typedef struct simu_state {
    76                 core::Quaternion Quat;
    77                 core::Vector3D W;
    78                 core::Vector3D Pos;
    79                 core::Vector3D Vel;
    80             } simu_state_t;
     73#ifdef GL
     74  irr::scene::ISceneNode *getSceneNode() const;
     75  irr::core::aabbox3d<irr::f32> *Box() const;
    8176
    82     #ifdef GL
    83             irr::scene::ISceneNode* getSceneNode() const;
    84             irr::core::aabbox3d<irr::f32>* Box() const;
     77  virtual size_t dbtSize(void) const = 0;
     78  virtual void Draw(void){};
     79  virtual void ExtraDraw(void){};
     80  virtual void WritedbtBuf(char *dbtbuf) = 0;
     81  virtual void ReaddbtBuf(char *dbtbuf) = 0;
     82  virtual bool OnEvent(const irr::SEvent &event) = 0;
    8583
    86             virtual size_t dbtSize(void) const =0;
    87             virtual void Draw(void){};
    88             virtual void ExtraDraw(void){};
    89             virtual void WritedbtBuf(char* dbtbuf)=0;
    90             virtual void ReaddbtBuf(char* dbtbuf)=0;
    91             virtual bool OnEvent(const irr::SEvent& event)=0;
     84  //! Sets the value of the camera's far clipping plane (default: 2000.0f)
     85  /** \param zf: New z far value. */
     86  void setCameraFarValue(float zf);
     87#endif
     88  gui::TabWidget *GetTabWidget(void) const;
    9289
    93             //! Sets the value of the camera's far clipping plane (default: 2000.0f)
    94             /** \param zf: New z far value. */
    95             void setCameraFarValue(float zf);
    96     #endif
    97             gui::TabWidget* GetTabWidget(void) const;
     90protected:
     91  DiscreteTimeVariable<simu_state_t, 3> state;
     92  float dT(void) const;
     93  virtual void CalcModel(void) = 0;
     94#ifdef GL
     95  AnimPoursuite *getCamera(void) const;
     96  virtual void AnimateModel(void) = 0;
     97  // void setPosition(core::Vector3D pos);
     98  void setScale(float value);
     99  void setTriangleSelector(irr::scene::ITriangleSelector *selector);
     100#endif
    98101
    99         protected:
    100             DiscreteTimeVariable<simu_state_t,3> state;
    101             float dT(void) const;
    102             virtual void CalcModel(void)=0;
    103     #ifdef GL
    104             AnimPoursuite* getCamera(void) const;
    105             virtual void AnimateModel(void)=0;
    106             //void setPosition(core::Vector3D pos);
    107             void setScale(float value);
    108             void setTriangleSelector(irr::scene::ITriangleSelector* selector);
    109     #endif
    110 
    111         private:
    112             void UpdateFrom(const core::io_data *data){};
    113             class Model_impl* pimpl_;
    114     };
     102private:
     103  void UpdateFrom(const core::io_data *data){};
     104  class Model_impl *pimpl_;
     105};
    115106} // end namespace simulator
    116107} // end namespace flair

  • trunk/lib/FlairSimulator/src/Model_impl.cpp

    r10 r15  
    5050
    5151#ifdef GL
    52 Model_impl::Model_impl(Model* self,std::string name,ISceneManager* scenemanager,vrpn_Connection_IP* vrpn)
    53     :  ISceneNode(scenemanager->getRootSceneNode(), scenemanager, -1),Thread(self,name,50),vrpn_Tracker( name.c_str(), vrpn )
     52Model_impl::Model_impl(Model *self, std::string name,
     53                       ISceneManager *scenemanager, vrpn_Connection_IP *vrpn)
     54    : ISceneNode(scenemanager->getRootSceneNode(), scenemanager, -1),
     55      Thread(self, name, 50), vrpn_Tracker(name.c_str(), vrpn)
    5456
    5557#else
    56 Model_impl::Model_impl(Model* self,std::string name,vrpn_Connection_IP* vrpn)
    57     : Thread(self,name,50),vrpn_Tracker( name.c_str(), vrpn )
     58Model_impl::Model_impl(Model *self, std::string name, vrpn_Connection_IP *vrpn)
     59    : Thread(self, name, 50), vrpn_Tracker(name.c_str(), vrpn)
    5860#endif
    5961{
    60     this->self=self;
    61 
    62 #ifdef GL
    63     //for sync with gui
    64     cond=new ConditionVariable(this,name);
    65     sync_count=0;
    66 
    67     //collisions
    68     collision_mutex=new Mutex(this);
    69     collision_occured=false;
    70 
    71     //selector for collisions
    72     selector = getSceneManager()->createTriangleSelectorFromBoundingBox(this);
    73     setTriangleSelector(selector);
    74     meta_selector = getSceneManager()->createMetaTriangleSelector();
    75 
    76     anim = getSceneManager()->createCollisionResponseAnimator(meta_selector, this,vector3df(1,1,1),vector3df(0,0,0), vector3df(0,0,0));
    77     addAnimator(anim);
    78 
    79     //camera
    80         camera =getSceneManager()->addCameraSceneNode();
    81     camera->setAspectRatio(getGui()->getAspectRatio());//on force a cause du view port
    82     camera->setUpVector(vector3df(0,0,1));
    83 
    84     animator=new AnimPoursuite(this);
    85     camera->addAnimator(animator);
    86     camera->setFarValue(8000);
    87 
    88     position_init=false;
    89 #endif
    90 
    91     //init user interface
    92     Tab* tab=new Tab(getSimulator()->GetTabWidget(),ObjectName());
    93     tabwidget=new TabWidget(tab->NewRow(),"tabs");
    94     Tab* sampl=new Tab(tabwidget,"sampling");
    95     dT=new DoubleSpinBox(sampl->NewRow(),"Tech (s):",0.001,1,0.001,3);
    96     Tab* layout=new Tab(tabwidget,"optitrack");
    97     enable_opti=new CheckBox(layout->NewRow(),"enabled");
    98     Tab* init=new Tab(tabwidget,"init");
    99     pos_init=new Vector3DSpinBox(init->NewRow(),"position",-50,50,1);
    100     yaw_init=new SpinBox(init->NewRow(),"yaw (deg):",-180,180,10);
    101 
    102     //modele
    103     states_mutex=new Mutex(this);
    104         self->state[0].Pos=pos_init->Value();
    105         self->state[0].Vel.x=0;
    106     self->state[0].Vel.y=0;
    107     self->state[0].Vel.z=0;
    108         self->state[0].Quat=ComputeInitRotation(Quaternion(1,0,0,0));
    109     self->state[0].W.x=0;
    110     self->state[0].W.y=0;
    111     self->state[0].W.z=0;
    112 
    113     self->state[-1]=self->state[0];
    114     self->state[-2]=self->state[0];
    115 
    116     cvmatrix_descriptor* desc=new cvmatrix_descriptor(13,1);
    117         desc->SetElementName(0,0,"q0");
    118         desc->SetElementName(1,0,"q1");
    119         desc->SetElementName(2,0,"q2");
    120         desc->SetElementName(3,0,"q3");
    121         desc->SetElementName(4,0,"x");
    122         desc->SetElementName(5,0,"y");
    123         desc->SetElementName(6,0,"z");
    124         desc->SetElementName(7,0,"wx");
    125         desc->SetElementName(8,0,"wy");
    126         desc->SetElementName(9,0,"wz");
    127         desc->SetElementName(10,0,"vx");
    128         desc->SetElementName(11,0,"vy");
    129         desc->SetElementName(12,0,"vz");
    130     output=new cvmatrix(this,desc,floatType,"state");
    131 
    132     self->AddDataToLog(output);
    133 }
    134 
    135 Model_impl::~Model_impl()
    136 {
    137     SafeStop();
    138     Join();
    139 #ifdef GL
    140     remove();//remove ISceneNode
     62  this->self = self;
     63
     64#ifdef GL
     65  // for sync with gui
     66  cond = new ConditionVariable(this, name);
     67  sync_count = 0;
     68
     69  // collisions
     70  collision_mutex = new Mutex(this);
     71  collision_occured = false;
     72
     73  // selector for collisions
     74  selector = getSceneManager()->createTriangleSelectorFromBoundingBox(this);
     75  setTriangleSelector(selector);
     76  meta_selector = getSceneManager()->createMetaTriangleSelector();
     77
     78  anim = getSceneManager()->createCollisionResponseAnimator(
     79      meta_selector, this, vector3df(1, 1, 1), vector3df(0, 0, 0),
     80      vector3df(0, 0, 0));
     81  addAnimator(anim);
     82
     83  // camera
     84  camera = getSceneManager()->addCameraSceneNode();
     85  camera->setAspectRatio(
     86      getGui()->getAspectRatio()); // on force a cause du view port
     87  camera->setUpVector(vector3df(0, 0, 1));
     88
     89  animator = new AnimPoursuite(this);
     90  camera->addAnimator(animator);
     91  camera->setFarValue(8000);
     92
     93  position_init = false;
     94#endif
     95
     96  // init user interface
     97  Tab *tab = new Tab(getSimulator()->GetTabWidget(), ObjectName());
     98  tabwidget = new TabWidget(tab->NewRow(), "tabs");
     99  Tab *sampl = new Tab(tabwidget, "sampling");
     100  dT = new DoubleSpinBox(sampl->NewRow(), "Tech (s):", 0.001, 1, 0.001, 3);
     101  Tab *layout = new Tab(tabwidget, "optitrack");
     102  enable_opti = new CheckBox(layout->NewRow(), "enabled");
     103  Tab *init = new Tab(tabwidget, "init");
     104  pos_init = new Vector3DSpinBox(init->NewRow(), "position", -50, 50, 1);
     105  yaw_init = new SpinBox(init->NewRow(), "yaw (deg):", -180, 180, 10);
     106
     107  // modele
     108  states_mutex = new Mutex(this);
     109  self->state[0].Pos = pos_init->Value();
     110  self->state[0].Vel.x = 0;
     111  self->state[0].Vel.y = 0;
     112  self->state[0].Vel.z = 0;
     113  self->state[0].Quat = ComputeInitRotation(Quaternion(1, 0, 0, 0));
     114  self->state[0].W.x = 0;
     115  self->state[0].W.y = 0;
     116  self->state[0].W.z = 0;
     117
     118  self->state[-1] = self->state[0];
     119  self->state[-2] = self->state[0];
     120
     121  cvmatrix_descriptor *desc = new cvmatrix_descriptor(13, 1);
     122  desc->SetElementName(0, 0, "q0");
     123  desc->SetElementName(1, 0, "q1");
     124  desc->SetElementName(2, 0, "q2");
     125  desc->SetElementName(3, 0, "q3");
     126  desc->SetElementName(4, 0, "x");
     127  desc->SetElementName(5, 0, "y");
     128  desc->SetElementName(6, 0, "z");
     129  desc->SetElementName(7, 0, "wx");
     130  desc->SetElementName(8, 0, "wy");
     131  desc->SetElementName(9, 0, "wz");
     132  desc->SetElementName(10, 0, "vx");
     133  desc->SetElementName(11, 0, "vy");
     134  desc->SetElementName(12, 0, "vz");
     135  output = new cvmatrix(this, desc, floatType, "state");
     136
     137  self->AddDataToLog(output);
     138}
     139
     140Model_impl::~Model_impl() {
     141  SafeStop();
     142  Join();
     143#ifdef GL
     144  remove(); // remove ISceneNode
    141145#endif
    142146}
    143147
    144148Quaternion Model_impl::ComputeInitRotation(Quaternion quat_in) {
    145     Quaternion yaw_rot_quat;
    146     Euler yaw_rot_euler(0,0,Euler::ToRadian(yaw_init->Value()));
    147     yaw_rot_euler.ToQuaternion(yaw_rot_quat);
    148     return yaw_rot_quat*quat_in;
    149 }
    150 
    151 void Model_impl::mainloop(void)
    152 {
    153     if(enable_opti->Value()==false) return;
    154     vrpn_gettimeofday(&_timestamp, NULL);
    155     vrpn_Tracker::timestamp = _timestamp;
    156 
    157     //change to vrpn reference
     149  Quaternion yaw_rot_quat;
     150  Euler yaw_rot_euler(0, 0, Euler::ToRadian(yaw_init->Value()));
     151  yaw_rot_euler.ToQuaternion(yaw_rot_quat);
     152  return yaw_rot_quat * quat_in;
     153}
     154
     155void Model_impl::mainloop(void) {
     156  if (enable_opti->Value() == false)
     157    return;
     158  vrpn_gettimeofday(&_timestamp, NULL);
     159  vrpn_Tracker::timestamp = _timestamp;
     160
     161  // change to vrpn reference
     162  states_mutex->GetMutex();
     163  Quaternion quat = getSimulator()->ToVRPNReference(self->state[0].Quat);
     164  Vector3D position = getSimulator()->ToVRPNReference(self->state[0].Pos);
     165  states_mutex->ReleaseMutex();
     166
     167  pos[0] = position.x;
     168  pos[1] = position.y;
     169  pos[2] = position.z;
     170  // warning: d_quat is defined as (qx,qy,qz,qw), which is different from
     171  // flair::core::Quaternion
     172  d_quat[0] = quat.q1;
     173  d_quat[1] = quat.q2;
     174  d_quat[2] = quat.q3;
     175  d_quat[3] = quat.q0;
     176
     177  char msgbuf[1000];
     178
     179  d_sensor = 0;
     180
     181  int len = vrpn_Tracker::encode_to(msgbuf);
     182
     183  if (d_connection->pack_message(len, _timestamp, position_m_id, d_sender_id,
     184                                 msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
     185    fprintf(stderr, "can't write message: tossing\n");
     186  }
     187
     188  server_mainloop();
     189}
     190
     191#ifdef GL
     192ITriangleSelector *Model_impl::TriangleSelector(void) { return selector; }
     193
     194IMetaTriangleSelector *Model_impl::MetaTriangleSelector(void) {
     195  return meta_selector;
     196}
     197
     198void Model_impl::UpdatePos(void) {
     199  vector3df nodePosition;
     200  Quaternion nodeOrientation;
     201  Euler euler;
     202
     203  states_mutex->GetMutex();
     204  nodePosition = ToIrrlichtCoordinates(self->state[0].Pos);
     205  nodeOrientation = ToIrrlichtOrientation(self->state[0].Quat);
     206  states_mutex->ReleaseMutex();
     207
     208  setPosition(nodePosition);
     209
     210  nodeOrientation.ToEuler(euler);
     211  ISceneNode::setRotation(Euler::ToDegree(1) *
     212                          vector3df(euler.roll, euler.pitch, euler.yaw));
     213
     214  if (position_init == false) {
     215    anim->setTargetNode(this); // a faire pour se teleporter sans les collisions
     216    position_init = true;
     217  }
     218
     219  self->AnimateModel();
     220}
     221
     222void Model_impl::CheckCollision(void) {
     223  // TODO: setEllipsoidRadius should be called in Model::setScale
     224  // but we need to call recalculateBoundingBox
     225  anim->setEllipsoidRadius(getTransformedBoundingBox().getExtent());
     226
     227  if (anim->collisionOccurred() == true) {
     228    vector3df pos;
     229    vector3df pos_rel;
     230    vector3df nodePosition;
     231    pos = anim->getCollisionPoint();
     232    nodePosition = getPosition();
     233    pos_rel = pos - nodePosition;
     234    // printf("collision %f %f %f\n",pos.X,pos.Y,pos.Z);
     235    // printf("drone %f %f %f\n",nodePosition.X,nodePosition.Y,nodePosition.Z);
     236    // printf("rel %f %f %f\n",pos_rel.X,pos_rel.Z,pos_rel.Y);
     237
     238    collision_mutex->GetMutex();
     239    collision_occured = true;
     240    collision_point = ToSimulatorCoordinates(nodePosition);
     241    collision_mutex->ReleaseMutex();
     242  }
     243}
     244
     245void Model_impl::CollisionHandler(void) {
     246  collision_mutex->GetMutex();
     247  if (collision_occured == true) {
     248    collision_occured = false;
    158249    states_mutex->GetMutex();
    159     Quaternion quat=getSimulator()->ToVRPNReference(self->state[0].Quat);
    160     Vector3D position=getSimulator()->ToVRPNReference(self->state[0].Pos);
     250    self->state[0].Pos = collision_point;
     251    self->state[-1].Pos = self->state[0].Pos;
     252    self->state[-2].Pos = self->state[0].Pos;
    161253    states_mutex->ReleaseMutex();
    162 
    163     pos[0]=position.x;
    164     pos[1]=position.y;
    165     pos[2]=position.z;
    166     //warning: d_quat is defined as (qx,qy,qz,qw), which is different from flair::core::Quaternion
    167     d_quat[0] = quat.q1;
    168     d_quat[1] = quat.q2;
    169     d_quat[2] = quat.q3;
    170     d_quat[3] = quat.q0;
    171 
    172     char msgbuf[1000];
    173 
    174     d_sensor = 0;
    175 
    176     int  len = vrpn_Tracker::encode_to(msgbuf);
    177 
    178     if (d_connection->pack_message(len, _timestamp, position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY))
    179     {
    180         fprintf(stderr,"can't write message: tossing\n");
     254  }
     255  collision_mutex->ReleaseMutex();
     256}
     257
     258void Model_impl::OnRegisterSceneNode(void) {
     259  if (IsVisible)
     260    SceneManager->registerNodeForRendering(this);
     261
     262  ISceneNode::OnRegisterSceneNode();
     263}
     264
     265void Model_impl::render(void) {
     266  IVideoDriver *driver = SceneManager->getVideoDriver();
     267  driver->setTransform(ETS_WORLD, AbsoluteTransformation);
     268}
     269
     270// le premier arrive attend l'autre
     271void Model_impl::SynchronizationPoint() {
     272  cond->GetMutex();
     273  sync_count++;
     274
     275  if (sync_count < 2) {
     276    cond->CondWait();
     277  } else {
     278    cond->CondSignal();
     279  }
     280
     281  cond->ReleaseMutex();
     282}
     283#endif // GL
     284
     285void Model_impl::Run(void) {
     286  // Ask Xenomai to warn us upon switches to secondary mode.
     287  WarnUponSwitches(true);
     288
     289#ifdef GL
     290  // synchronize with gui
     291  SynchronizationPoint();
     292#endif
     293
     294  SetPeriodMS(dT->Value() * 1000.);
     295
     296  while (!ToBeStopped()) {
     297    if (dT->ValueChanged())
     298      SetPeriodMS(dT->Value() * 1000.);
     299    WaitPeriod();
     300
     301#ifdef GL
     302    CollisionHandler();
     303#endif
     304    states_mutex->GetMutex();
     305    self->CalcModel();
     306
     307    output->GetMutex();
     308    output->SetValueNoMutex(0, 0, self->state[0].Quat.q0);
     309    output->SetValueNoMutex(1, 0, self->state[0].Quat.q1);
     310    output->SetValueNoMutex(2, 0, self->state[0].Quat.q2);
     311    output->SetValueNoMutex(3, 0, self->state[0].Quat.q3);
     312    output->SetValueNoMutex(4, 0, self->state[0].Pos.x);
     313    output->SetValueNoMutex(5, 0, self->state[0].Pos.y);
     314    output->SetValueNoMutex(6, 0, self->state[0].Pos.z);
     315    output->SetValueNoMutex(7, 0, self->state[0].W.x);
     316    output->SetValueNoMutex(8, 0, self->state[0].W.y);
     317    output->SetValueNoMutex(9, 0, self->state[0].W.z);
     318    output->SetValueNoMutex(10, 0, self->state[0].Vel.x);
     319    output->SetValueNoMutex(11, 0, self->state[0].Vel.y);
     320    output->SetValueNoMutex(12, 0, self->state[0].Vel.z);
     321    output->ReleaseMutex();
     322    output->SetDataTime(GetTime());
     323
     324    self->state.Update();
     325
     326    if (pos_init->ValueChanged() || yaw_init->ValueChanged()) {
     327      self->state[-1].Quat = ComputeInitRotation(Quaternion(1, 0, 0, 0));
     328      self->state[-2].Quat = ComputeInitRotation(Quaternion(1, 0, 0, 0));
     329      self->state[-1].Pos = pos_init->Value();
     330      self->state[-2].Pos = self->state[-1].Pos;
     331#ifdef GL
     332      position_init = false;
     333#endif
    181334    }
    182335
    183     server_mainloop();
    184 }
    185 
    186 #ifdef GL
    187 ITriangleSelector* Model_impl::TriangleSelector(void)
    188 {
    189     return selector;
    190 }
    191 
    192 IMetaTriangleSelector* Model_impl::MetaTriangleSelector(void)
    193 {
    194     return meta_selector;
    195 }
    196 
    197 void Model_impl::UpdatePos(void)
    198 {
    199     vector3df nodePosition;
    200     Quaternion nodeOrientation;
    201     Euler euler;
    202 
    203     states_mutex->GetMutex();
    204     nodePosition=ToIrrlichtCoordinates(self->state[0].Pos);
    205     nodeOrientation=ToIrrlichtOrientation(self->state[0].Quat);
    206336    states_mutex->ReleaseMutex();
    207337
    208     setPosition(nodePosition);
    209 
    210     nodeOrientation.ToEuler(euler);
    211     ISceneNode::setRotation(Euler::ToDegree(1)*vector3df(euler.roll,euler.pitch,euler.yaw));
    212 
    213     if(position_init==false)
    214     {
    215         anim->setTargetNode(this); // a faire pour se teleporter sans les collisions
    216         position_init=true;
    217     }
    218 
    219     self->AnimateModel();
    220 }
    221 
    222 void Model_impl::CheckCollision(void)
    223 {
    224     //TODO: setEllipsoidRadius should be called in Model::setScale
    225     //but we need to call recalculateBoundingBox
    226     anim->setEllipsoidRadius(getTransformedBoundingBox().getExtent());
    227 
    228     if(anim->collisionOccurred()==true)
    229     {
    230         vector3df pos;
    231         vector3df pos_rel;
    232         vector3df nodePosition;
    233         pos= anim->getCollisionPoint();
    234         nodePosition=getPosition();
    235         pos_rel=pos-nodePosition;
    236         //printf("collision %f %f %f\n",pos.X,pos.Y,pos.Z);
    237         //printf("drone %f %f %f\n",nodePosition.X,nodePosition.Y,nodePosition.Z);
    238         //printf("rel %f %f %f\n",pos_rel.X,pos_rel.Z,pos_rel.Y);
    239 
    240         collision_mutex->GetMutex();
    241         collision_occured=true;
    242         collision_point=ToSimulatorCoordinates(nodePosition);
    243         collision_mutex->ReleaseMutex();
    244     }
    245 }
    246 
    247 void Model_impl::CollisionHandler(void)
    248 {
    249     collision_mutex->GetMutex();
    250     if(collision_occured==true)
    251     {
    252         collision_occured=false;
    253         states_mutex->GetMutex();
    254         self->state[0].Pos=collision_point;
    255         self->state[-1].Pos=self->state[0].Pos;
    256         self->state[-2].Pos=self->state[0].Pos;
    257         states_mutex->ReleaseMutex();
    258     }
    259     collision_mutex->ReleaseMutex();
    260 }
    261 
    262 void Model_impl::OnRegisterSceneNode(void)
    263 {
    264     if (IsVisible)
    265         SceneManager->registerNodeForRendering(this);
    266 
    267     ISceneNode::OnRegisterSceneNode();
    268 }
    269 
    270 void Model_impl::render(void)
    271 {
    272     IVideoDriver* driver = SceneManager->getVideoDriver();
    273     driver->setTransform(ETS_WORLD, AbsoluteTransformation);
    274 }
    275 
    276 //le premier arrive attend l'autre
    277 void Model_impl::SynchronizationPoint()
    278 {
    279     cond->GetMutex();
    280     sync_count++;
    281 
    282     if (sync_count < 2)
    283     {
    284          cond->CondWait();
    285     }
    286     else
    287     {
    288         cond->CondSignal();
    289     }
    290 
    291     cond->ReleaseMutex();
    292 }
    293 #endif //GL
    294 
    295 void Model_impl::Run(void)
    296 {
    297     // Ask Xenomai to warn us upon switches to secondary mode.
    298     WarnUponSwitches(true);
    299 
    300 #ifdef GL
    301     //synchronize with gui
    302     SynchronizationPoint();
    303 #endif
    304 
    305     SetPeriodMS(dT->Value()*1000.);
    306 
    307     while(!ToBeStopped())
    308     {
    309         if(dT->ValueChanged()) SetPeriodMS(dT->Value()*1000.);
    310         WaitPeriod();
    311 
    312 #ifdef GL
    313         CollisionHandler();
    314 #endif
    315         states_mutex->GetMutex();
    316         self->CalcModel();
    317 
    318         output->GetMutex();
    319         output->SetValueNoMutex(0,0,self->state[0].Quat.q0);
    320         output->SetValueNoMutex(1,0,self->state[0].Quat.q1);
    321         output->SetValueNoMutex(2,0,self->state[0].Quat.q2);
    322         output->SetValueNoMutex(3,0,self->state[0].Quat.q3);
    323         output->SetValueNoMutex(4,0,self->state[0].Pos.x);
    324         output->SetValueNoMutex(5,0,self->state[0].Pos.y);
    325         output->SetValueNoMutex(6,0,self->state[0].Pos.z);
    326         output->SetValueNoMutex(7,0,self->state[0].W.x);
    327         output->SetValueNoMutex(8,0,self->state[0].W.y);
    328         output->SetValueNoMutex(9,0,self->state[0].W.z);
    329         output->SetValueNoMutex(10,0,self->state[0].Vel.x);
    330         output->SetValueNoMutex(11,0,self->state[0].Vel.y);
    331         output->SetValueNoMutex(12,0,self->state[0].Vel.z);
    332         output->ReleaseMutex();
    333         output->SetDataTime(GetTime());
    334 
    335         self->state.Update();
    336 
    337         if(pos_init->ValueChanged() || yaw_init->ValueChanged())
    338         {
    339             self->state[-1].Quat=ComputeInitRotation(Quaternion(1,0,0,0));
    340             self->state[-2].Quat=ComputeInitRotation(Quaternion(1,0,0,0));
    341             self->state[-1].Pos=pos_init->Value();
    342             self->state[-2].Pos=self->state[-1].Pos;
    343 #ifdef GL
    344             position_init=false;
    345 #endif
    346         }
    347 
    348         states_mutex->ReleaseMutex();
    349 
    350         self->ProcessUpdate(output);
    351 
    352     }
    353 
    354     WarnUponSwitches(false);
    355 }
     338    self->ProcessUpdate(output);
     339  }
     340
     341  WarnUponSwitches(false);
     342}

  • trunk/lib/FlairSimulator/src/Parser.cpp

    r10 r15  
    2323#include "GenericObject.h"
    2424
    25 
    2625using namespace irr;
    2726using namespace irr::core;
    … …  
    3130using namespace flair::simulator;
    3231
    33 //todo: getMeshVect doit plutot donner point3D et euler
    34 //adaptr le xml et la dtd
    35 
    36 namespace flair
    37 {
    38 namespace simulator
    39 {
    40 Parser::Parser(Simulator* parent,int app_width, int app_height,int scene_width, int scene_height,std::string media_path, std::string xmlFile): Gui(parent,"Parser",app_width,app_height,scene_width,scene_height, media_path)
    41 {
    42         this->media_path = media_path;
    43         this->parent = parent;
    44         xmlNode        *root_element = NULL;
    45         doc = xmlReadFile(xmlFile.c_str(), NULL, 0);
    46         xmlDtdPtr dtd = NULL;
    47         dtd = xmlParseDTD(NULL,  (const xmlChar *)(media_path.append("/scene.dtd").c_str()));
    48 
    49         if (dtd == NULL)
    50         {
    51                 //xmlGenericError(xmlGenericErrorContext,
    52                 Err("Could not parse DTD scene.dtd\n");
    53         }
    54         else
    55         {
    56                 xmlValidCtxt cvp;
    57                 cvp.userData = (void *) stderr;
    58                 cvp.error    = (xmlValidityErrorFunc) fprintf;
    59                 cvp.warning  = (xmlValidityWarningFunc) fprintf;
    60                 if (!xmlValidateDtd(&cvp, doc, dtd))
    61                 {
    62                         //xmlGenericError(xmlGenericErrorContext,
    63                         Err("Document does not validate against scene.dtd\n");
    64                 }
    65         }
    66 
    67         if (doc == NULL)
    68         {
    69                 Err("error: could not parse file %s\n", xmlFile.c_str());
    70         }
    71         else
    72         {
    73                 /*
    74                  * Get the root element node
    75                  */
    76                 root_element = xmlDocGetRootElement(doc);
    77                 processElements(root_element);
    78     }
    79 
    80 }
    81 
    82 Parser::~Parser()
    83 {
    84         if(doc!=NULL)
    85         {
    86                 /*
    87                  * free the document
    88                  */
    89                 xmlFreeDoc(doc);;
    90         }
    91         /*
    92          *Free the global variables that may
    93          *have been allocated by the parser.
    94          */
    95         xmlCleanupParser();
     32// todo: getMeshVect doit plutot donner point3D et euler
     33// adaptr le xml et la dtd
     34
     35namespace flair {
     36namespace simulator {
     37Parser::Parser(Simulator *parent, int app_width, int app_height,
     38               int scene_width, int scene_height, std::string media_path,
     39               std::string xmlFile)
     40    : Gui(parent, "Parser", app_width, app_height, scene_width, scene_height,
     41          media_path) {
     42  this->media_path = media_path;
     43  this->parent = parent;
     44  xmlNode *root_element = NULL;
     45  doc = xmlReadFile(xmlFile.c_str(), NULL, 0);
     46  xmlDtdPtr dtd = NULL;
     47  dtd = xmlParseDTD(NULL,
     48                    (const xmlChar *)(media_path.append("/scene.dtd").c_str()));
     49
     50  if (dtd == NULL) {
     51    // xmlGenericError(xmlGenericErrorContext,
     52    Err("Could not parse DTD scene.dtd\n");
     53  } else {
     54    xmlValidCtxt cvp;
     55    cvp.userData = (void *)stderr;
     56    cvp.error = (xmlValidityErrorFunc)fprintf;
     57    cvp.warning = (xmlValidityWarningFunc)fprintf;
     58    if (!xmlValidateDtd(&cvp, doc, dtd)) {
     59      // xmlGenericError(xmlGenericErrorContext,
     60      Err("Document does not validate against scene.dtd\n");
     61    }
     62  }
     63
     64  if (doc == NULL) {
     65    Err("error: could not parse file %s\n", xmlFile.c_str());
     66  } else {
     67    /*
     68     * Get the root element node
     69     */
     70    root_element = xmlDocGetRootElement(doc);
     71    processElements(root_element);
     72  }
     73}
     74
     75Parser::~Parser() {
     76  if (doc != NULL) {
     77    /*
     78     * free the document
     79     */
     80    xmlFreeDoc(doc);
     81    ;
     82  }
     83  /*
     84   *Free the global variables that may
     85   *have been allocated by the parser.
     86   */
     87  xmlCleanupParser();
    9688}
    9789
    9890/* Recursive function that prints the XML structure */
    99 void Parser::processElements(xmlNode * a_node)
    100 {
    101         xmlNode *cur_node = NULL;
    102         for (cur_node = a_node; cur_node; cur_node = cur_node->next)
    103         {
    104                 if(cur_node->type == XML_ELEMENT_NODE)
    105                 {
    106                         if(xmlStrEqual(cur_node->name, (xmlChar*) "params"))
    107                         {
    108                                 processParams(cur_node->children);
    109                         }
    110                         else if(xmlStrEqual(cur_node->name, (xmlChar*) "objects"))
    111                         {
    112                                 processObjects(cur_node->children);
    113                         }
    114                         else
    115                         {
    116                                 processElements(cur_node->children);
    117                         }
    118                 }
    119         }
    120 }
    121 
    122 void Parser::processObjects(xmlNode * a_node)
    123 {
    124         FILE* fp;
    125         std::string fileName;
    126         xmlNode *cur_node = NULL;
    127 
    128         const IGeometryCreator *geo;
    129 
    130     geo=getGui()->getSceneManager()->getGeometryCreator();
    131 
    132         for (cur_node = a_node; cur_node; cur_node = cur_node->next)
    133         {
    134                 if(xmlStrEqual(cur_node->name, (xmlChar*) "mesh"))
    135                 {
    136                         fileName=this->media_path;
    137                         fp = NULL;
    138                         fp = fopen( fileName.append((char*)xmlGetProp(cur_node,(xmlChar*)"model")).c_str(), "rb" );
    139                         if(fp!=NULL){
    140                                 GenericObject* object = new GenericObject(parent,"Object", getSceneManager());
    141                                 object->setMesh(getGui()->getMesh((char*)xmlGetProp(cur_node,(xmlChar*)"model")));
    142                                 object->setPosition(getMeshVect(cur_node->children,(xmlChar*)"position"));
    143                                 object->setRotation(getMeshVect(cur_node->children,(xmlChar*)"rotation"));
    144                                 object->setScale(getMeshVect(cur_node->children,(xmlChar*)"scale"));
    145                                 object->render();
    146                         }else{
    147                                 Err("FATAL ERROR : File %s doesn't exist !\r\n",(char*)xmlGetProp(cur_node,(xmlChar*)"model"));
    148                         }
    149                 }else if(xmlStrEqual(cur_node->name, (xmlChar*) "cylinder")){
    150             GenericObject* object = new GenericObject(parent,"Object", getSceneManager());
    151             object->setMesh(geo->createCylinderMesh(ToIrrlichtScale(atof((char*)xmlGetProp(cur_node,(xmlChar*)"radius"))),
    152                                                     ToIrrlichtScale(atof((char*)xmlGetProp(cur_node,(xmlChar*)"length"))),
    153                                                     atof((char*)xmlGetProp(cur_node,(xmlChar*)"tesselation")),
    154                                                     SColor(100, 255, 100, 100)));
    155             object->setPosition(getMeshVect(cur_node->children,(xmlChar*)"position"));
    156             object->setRotation(getMeshVect(cur_node->children,(xmlChar*)"rotation"));
    157             object->setScale(getMeshVect(cur_node->children,(xmlChar*)"scale"));
    158             //object->setMaterialTexture(0,getTexture("/home/apeiron/igep/uav_dev_svn/trunk/media/nskinbl.jpg"));
    159                     object->setMaterialType( video::EMT_SOLID );
    160                     object->render();
    161                 }else if(xmlStrEqual(cur_node->name, (xmlChar*) "eight")){
    162             GenericObject* object = new GenericObject(parent,"Object", getSceneManager());
    163             object->setMesh(geo->createCubeMesh(vector3df(atof((char*)xmlGetProp(cur_node,(xmlChar*)"length")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"width")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"eight")))));
    164             object->setPosition(getMeshVect(cur_node->children,(xmlChar*)"position"));
    165             object->setRotation(getMeshVect(cur_node->children,(xmlChar*)"rotation"));
    166             object->setScale(getMeshVect(cur_node->children,(xmlChar*)"scale"));
    167                     object->setMaterialType( video::EMT_TRANSPARENT_ALPHA_CHANNEL );
    168                     //object->getMaterial(0).Textures[0] = getTexture("/home/apeiron/igep/uav_dev_svn/trunk/media/nskinbl.jpg");
    169                     object->setMaterialFlag(EMF_LIGHTING, false);
    170                     object->render();
    171         }
    172         }
    173 }
    174 
    175 void Parser::processParams(xmlNode * a_node)
    176 {
    177 
    178         xmlNode *cur_node = NULL;
    179         for (cur_node = a_node; cur_node; cur_node = cur_node->next)
    180         {
    181                 if(xmlStrEqual(cur_node->name, (xmlChar*) "archive"))
    182                 {
    183                         std::string file=media_path + "/" + (char*)xmlGetProp(cur_node,(xmlChar*)"path");
    184                         getDevice()->getFileSystem()->addFileArchive(file.c_str());
    185                 }
    186                 else if(xmlStrEqual(cur_node->name, (xmlChar*) "mesh"))
    187                 {
    188                         setMesh((char*)xmlGetProp(cur_node,(xmlChar*)"model"),getSceneVect(cur_node->children,(xmlChar*)"position"),getSceneVect(cur_node->children,(xmlChar*)"rotation"),getSceneVect(cur_node->children,(xmlChar*)"scale",true));
    189 
    190                 }
    191         }
    192 }
    193 //todo rendre un vector3D framework
    194 //retirer irr::core::vector3df ToIrrlichtCoordinates(irr::core::vector3df vect);
    195 vector3df Parser::getMeshVect(xmlNode * mesh_node, xmlChar * param)
    196 {
    197         xmlNode *cur_node = NULL;
    198         for (cur_node = mesh_node; cur_node; cur_node = cur_node->next)
    199         {
    200                 if(xmlStrEqual(cur_node->name, param)){
    201                         return vector3df(atof((char*)xmlGetProp(cur_node,(xmlChar*)"x")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"y")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"z")));
    202                 }
    203         }
    204         return vector3df(0,0,0);
    205 }
    206 
    207 vector3df Parser::getSceneVect(xmlNode * mesh_node, xmlChar * param, bool isScale)
    208 {
    209         xmlNode *cur_node = NULL;
    210         for (cur_node = mesh_node; cur_node; cur_node = cur_node->next)
    211         {
    212                 if(xmlStrEqual(cur_node->name, param)){
    213                         if(isScale){
    214                                 return vector3df(atof((char*)xmlGetProp(cur_node,(xmlChar*)"x")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"z")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"y")));
    215                         }
    216                         return vector3df(atof((char*)xmlGetProp(cur_node,(xmlChar*)"x")),-atof((char*)xmlGetProp(cur_node,(xmlChar*)"z")),atof((char*)xmlGetProp(cur_node,(xmlChar*)"y")));
    217                 }
    218         }
    219         return vector3df(0,0,0);
     91void Parser::processElements(xmlNode *a_node) {
     92  xmlNode *cur_node = NULL;
     93  for (cur_node = a_node; cur_node; cur_node = cur_node->next) {
     94    if (cur_node->type == XML_ELEMENT_NODE) {
     95      if (xmlStrEqual(cur_node->name, (xmlChar *)"params")) {
     96        processParams(cur_node->children);
     97      } else if (xmlStrEqual(cur_node->name, (xmlChar *)"objects")) {
     98        processObjects(cur_node->children);
     99      } else {
     100        processElements(cur_node->children);
     101      }
     102    }
     103  }
     104}
     105
     106void Parser::processObjects(xmlNode *a_node) {
     107  FILE *fp;
     108  std::string fileName;
     109  xmlNode *cur_node = NULL;
     110
     111  const IGeometryCreator *geo;
     112
     113  geo = getGui()->getSceneManager()->getGeometryCreator();
     114
     115  for (cur_node = a_node; cur_node; cur_node = cur_node->next) {
     116    if (xmlStrEqual(cur_node->name, (xmlChar *)"mesh")) {
     117      fileName = this->media_path;
     118      fp = NULL;
     119      fp = fopen(fileName.append((char *)xmlGetProp(
     120                                     cur_node, (xmlChar *)"model")).c_str(),
     121                 "rb");
     122      if (fp != NULL) {
     123        GenericObject *object =
     124            new GenericObject(parent, "Object", getSceneManager());
     125        object->setMesh(getGui()->getMesh(
     126            (char *)xmlGetProp(cur_node, (xmlChar *)"model")));
     127        object->setPosition(
     128            getMeshVect(cur_node->children, (xmlChar *)"position"));
     129        object->setRotation(
     130            getMeshVect(cur_node->children, (xmlChar *)"rotation"));
     131        object->setScale(getMeshVect(cur_node->children, (xmlChar *)"scale"));
     132        object->render();
     133      } else {
     134        Err("FATAL ERROR : File %s doesn't exist !\r\n",
     135            (char *)xmlGetProp(cur_node, (xmlChar *)"model"));
     136      }
     137    } else if (xmlStrEqual(cur_node->name, (xmlChar *)"cylinder")) {
     138      GenericObject *object =
     139          new GenericObject(parent, "Object", getSceneManager());
     140      object->setMesh(geo->createCylinderMesh(
     141          ToIrrlichtScale(
     142              atof((char *)xmlGetProp(cur_node, (xmlChar *)"radius"))),
     143          ToIrrlichtScale(
     144              atof((char *)xmlGetProp(cur_node, (xmlChar *)"length"))),
     145          atof((char *)xmlGetProp(cur_node, (xmlChar *)"tesselation")),
     146          SColor(100, 255, 100, 100)));
     147      object->setPosition(
     148          getMeshVect(cur_node->children, (xmlChar *)"position"));
     149      object->setRotation(
     150          getMeshVect(cur_node->children, (xmlChar *)"rotation"));
     151      object->setScale(getMeshVect(cur_node->children, (xmlChar *)"scale"));
     152      // object->setMaterialTexture(0,getTexture("/home/apeiron/igep/uav_dev_svn/trunk/media/nskinbl.jpg"));
     153      object->setMaterialType(video::EMT_SOLID);
     154      object->render();
     155    } else if (xmlStrEqual(cur_node->name, (xmlChar *)"eight")) {
     156      GenericObject *object =
     157          new GenericObject(parent, "Object", getSceneManager());
     158      object->setMesh(geo->createCubeMesh(
     159          vector3df(atof((char *)xmlGetProp(cur_node, (xmlChar *)"length")),
     160                    atof((char *)xmlGetProp(cur_node, (xmlChar *)"width")),
     161                    atof((char *)xmlGetProp(cur_node, (xmlChar *)"eight")))));
     162      object->setPosition(
     163          getMeshVect(cur_node->children, (xmlChar *)"position"));
     164      object->setRotation(
     165          getMeshVect(cur_node->children, (xmlChar *)"rotation"));
     166      object->setScale(getMeshVect(cur_node->children, (xmlChar *)"scale"));
     167      object->setMaterialType(video::EMT_TRANSPARENT_ALPHA_CHANNEL);
     168      // object->getMaterial(0).Textures[0] =
     169      // getTexture("/home/apeiron/igep/uav_dev_svn/trunk/media/nskinbl.jpg");
     170      object->setMaterialFlag(EMF_LIGHTING, false);
     171      object->render();
     172    }
     173  }
     174}
     175
     176void Parser::processParams(xmlNode *a_node) {
     177
     178  xmlNode *cur_node = NULL;
     179  for (cur_node = a_node; cur_node; cur_node = cur_node->next) {
     180    if (xmlStrEqual(cur_node->name, (xmlChar *)"archive")) {
     181      std::string file =
     182          media_path + "/" + (char *)xmlGetProp(cur_node, (xmlChar *)"path");
     183      getDevice()->getFileSystem()->addFileArchive(file.c_str());
     184    } else if (xmlStrEqual(cur_node->name, (xmlChar *)"mesh")) {
     185      setMesh((char *)xmlGetProp(cur_node, (xmlChar *)"model"),
     186              getSceneVect(cur_node->children, (xmlChar *)"position"),
     187              getSceneVect(cur_node->children, (xmlChar *)"rotation"),
     188              getSceneVect(cur_node->children, (xmlChar *)"scale", true));
     189    }
     190  }
     191}
     192// todo rendre un vector3D framework
     193// retirer irr::core::vector3df ToIrrlichtCoordinates(irr::core::vector3df
     194// vect);
     195vector3df Parser::getMeshVect(xmlNode *mesh_node, xmlChar *param) {
     196  xmlNode *cur_node = NULL;
     197  for (cur_node = mesh_node; cur_node; cur_node = cur_node->next) {
     198    if (xmlStrEqual(cur_node->name, param)) {
     199      return vector3df(atof((char *)xmlGetProp(cur_node, (xmlChar *)"x")),
     200                       atof((char *)xmlGetProp(cur_node, (xmlChar *)"y")),
     201                       atof((char *)xmlGetProp(cur_node, (xmlChar *)"z")));
     202    }
     203  }
     204  return vector3df(0, 0, 0);
     205}
     206
     207vector3df Parser::getSceneVect(xmlNode *mesh_node, xmlChar *param,
     208                               bool isScale) {
     209  xmlNode *cur_node = NULL;
     210  for (cur_node = mesh_node; cur_node; cur_node = cur_node->next) {
     211    if (xmlStrEqual(cur_node->name, param)) {
     212      if (isScale) {
     213        return vector3df(atof((char *)xmlGetProp(cur_node, (xmlChar *)"x")),
     214                         atof((char *)xmlGetProp(cur_node, (xmlChar *)"z")),
     215                         atof((char *)xmlGetProp(cur_node, (xmlChar *)"y")));
     216      }
     217      return vector3df(atof((char *)xmlGetProp(cur_node, (xmlChar *)"x")),
     218                       -atof((char *)xmlGetProp(cur_node, (xmlChar *)"z")),
     219                       atof((char *)xmlGetProp(cur_node, (xmlChar *)"y")));
     220    }
     221  }
     222  return vector3df(0, 0, 0);
    220223}
    221224
    222225} // end namespace simulator
    223226} // end namespace flair
    224 #endif //GL
     227#endif // GL

  • trunk/lib/FlairSimulator/src/Parser.h

    r10 r15  
    2222#include <libxml/tree.h>
    2323
    24 namespace flair
    25 {
    26 namespace simulator
    27 {
    28 class Parser:public Gui
    29 {
     24namespace flair {
     25namespace simulator {
     26class Parser : public Gui {
    3027
    31 /*can create:
    32 - cylinders: in y axis
     28  /*can create:
     29  - cylinders: in y axis
    3330
    34 */
    35     public:
    36         Parser(Simulator* parent,int app_width, int app_height,int scene_width, int scene_height,std::string media_path, std::string xmlFile);
    37         ~Parser();
     31  */
     32public:
     33  Parser(Simulator *parent, int app_width, int app_height, int scene_width,
     34         int scene_height, std::string media_path, std::string xmlFile);
     35  ~Parser();
    3836
    39 
    40     private:
    41         xmlDoc *doc;
    42         std::string media_path;
    43         Simulator* parent;
    44         void processElements(xmlNode * a_node);
    45         void processObjects(xmlNode * a_node);
    46         void processParams(xmlNode * a_node);
    47         irr::core::vector3df getMeshVect(xmlNode * mesh_node, xmlChar * param);
    48         irr::core::vector3df getSceneVect(xmlNode * mesh_node, xmlChar * param, bool isScale=false);
     37private:
     38  xmlDoc *doc;
     39  std::string media_path;
     40  Simulator *parent;
     41  void processElements(xmlNode *a_node);
     42  void processObjects(xmlNode *a_node);
     43  void processParams(xmlNode *a_node);
     44  irr::core::vector3df getMeshVect(xmlNode *mesh_node, xmlChar *param);
     45  irr::core::vector3df getSceneVect(xmlNode *mesh_node, xmlChar *param,
     46                                    bool isScale = false);
    4947};
    5048}

  • trunk/lib/FlairSimulator/src/SensorGL.cpp

    r10 r15  
    2525using namespace flair::simulator;
    2626
    27 namespace flair
    28 {
    29 namespace sensor
    30 {
     27namespace flair {
     28namespace sensor {
    3129
    32 SensorGL::SensorGL(const Model *parent)
    33 {
    34     collMan =getGui()->getSceneManager()->getSceneCollisionManager();
    35     node=parent->Model::pimpl_;
     30SensorGL::SensorGL(const Model *parent) {
     31  collMan = getGui()->getSceneManager()->getSceneCollisionManager();
     32  node = parent->Model::pimpl_;
    3633}
    3734
    38 SensorGL::~SensorGL()
    39 {
    40 }
     35SensorGL::~SensorGL() {}
    4136
    42 ISceneCollisionManager* SensorGL::CollMan(void) const
    43 {
    44     return collMan;
    45 }
     37ISceneCollisionManager *SensorGL::CollMan(void) const { return collMan; }
    4638
    47 ISceneNode* SensorGL::Node(void) const
    48 {
    49     return node;
    50 }
     39ISceneNode *SensorGL::Node(void) const { return node; }
    5140
    5241} // end namespace simulator

  • trunk/lib/FlairSimulator/src/SensorGL.h

    r10 r15  
    1818#define SENSORGL_H
    1919
    20 namespace irr
    21 {
    22     namespace scene
    23     {
    24         class ISceneNode;
    25         class ISceneCollisionManager;
    26     }
     20namespace irr {
     21namespace scene {
     22class ISceneNode;
     23class ISceneCollisionManager;
     24}
    2725}
    2826
    29 namespace flair
    30 {
    31     namespace simulator
    32     {
    33         class Model;
    34     }
     27namespace flair {
     28namespace simulator {
     29class Model;
     30}
    3531}
    3632
    37 namespace flair
    38 {
    39 namespace sensor
    40 {
    41     class SensorGL
    42     {
    43         public:
    44             SensorGL(const simulator::Model *parent);
    45             virtual ~SensorGL()=0;
     33namespace flair {
     34namespace sensor {
     35class SensorGL {
     36public:
     37  SensorGL(const simulator::Model *parent);
     38  virtual ~SensorGL() = 0;
    4639
    47         protected:
    48     #ifdef GL
    49             irr::scene::ISceneCollisionManager* CollMan(void) const;
    50             irr::scene::ISceneNode* Node(void) const;
    51     #endif
    52         private:
    53     #ifdef GL
    54             irr::scene::ISceneCollisionManager* collMan;
    55             irr::scene::ISceneNode* node;
    56     #endif
    57     };
     40protected:
     41#ifdef GL
     42  irr::scene::ISceneCollisionManager *CollMan(void) const;
     43  irr::scene::ISceneNode *Node(void) const;
     44#endif
     45private:
     46#ifdef GL
     47  irr::scene::ISceneCollisionManager *collMan;
     48  irr::scene::ISceneNode *node;
     49#endif
     50};
    5851} // end namespace sensor
    5952} // end namespace flair

  • trunk/lib/FlairSimulator/src/SimuCameraGL.cpp

    r10 r15  
    4141using namespace flair::simulator;
    4242
    43 namespace flair
    44 {
    45 namespace sensor
    46 {
    47 
    48 SimuCameraGL::SimuCameraGL(const Model* parent,std::string name,int width,int height,int x,int y,int dev_id) :SimuCamera(parent,name,width,height,3,dev_id),SensorGL(parent)
    49 {
    50     smgr=getGui()->getSceneManager();
    51     camera=smgr->addCameraSceneNode();
    52     camera->addAnimator(this);
    53     camera->setAspectRatio(4.0f/3.0f);//on force a cause du view port
    54 
    55     this->width=width;
    56     this->height=height;
    57     this->x=x;
    58     this->y=y;
    59 
    60     index = 0;
    61 
    62     buffer=(char*)malloc(width*height*3);
    63 
    64     //user interface
    65     Tab* setup_tab=new Tab(parent->GetTabWidget(),name);
    66         position=new Vector3DSpinBox(setup_tab->NewRow(),"position",-2,2,.01);
    67         direction=new Vector3DSpinBox(setup_tab->NewRow(),"direction",-2,2,.01);
    68         up=new Vector3DSpinBox(setup_tab->NewRow(),"up",-2,2,.01);
    69         fov=new DoubleSpinBox(setup_tab->NewRow(),"fov:",0,180,5);
    70 
    71     if(strcmp((char*)glGetString(GL_VENDOR),"Intel Open Source Technology Center")==0) {
    72         Thread::Warn("disabling cameras output for Intel card (bug with PBO)\n");
    73         disable_output=true;
    74     } else {
    75         disable_output=false;
     43namespace flair {
     44namespace sensor {
     45
     46SimuCameraGL::SimuCameraGL(const Model *parent, std::string name, int width,
     47                           int height, int x, int y, int dev_id)
     48    : SimuCamera(parent, name, width, height, 3, dev_id), SensorGL(parent) {
     49  smgr = getGui()->getSceneManager();
     50  camera = smgr->addCameraSceneNode();
     51  camera->addAnimator(this);
     52  camera->setAspectRatio(4.0f / 3.0f); // on force a cause du view port
     53
     54  this->width = width;
     55  this->height = height;
     56  this->x = x;
     57  this->y = y;
     58
     59  index = 0;
     60
     61  buffer = (char *)malloc(width * height * 3);
     62
     63  // user interface
     64  Tab *setup_tab = new Tab(parent->GetTabWidget(), name);
     65  position = new Vector3DSpinBox(setup_tab->NewRow(), "position", -2, 2, .01);
     66  direction = new Vector3DSpinBox(setup_tab->NewRow(), "direction", -2, 2, .01);
     67  up = new Vector3DSpinBox(setup_tab->NewRow(), "up", -2, 2, .01);
     68  fov = new DoubleSpinBox(setup_tab->NewRow(), "fov:", 0, 180, 5);
     69
     70  if (strcmp((char *)glGetString(GL_VENDOR),
     71             "Intel Open Source Technology Center") == 0) {
     72    Thread::Warn("disabling cameras output for Intel card (bug with PBO)\n");
     73    disable_output = true;
     74  } else {
     75    disable_output = false;
     76  }
     77
     78  if (isGlExtensionSupported("GL_ARB_pixel_buffer_object")) {
     79    use_pbo = true;
     80    // create 2 pixel buffer objects, you need to delete them when program
     81    // exits.
     82    // glBufferDataARB with NULL pointer reserves only memory space.
     83    pboIds = (GLuint *)malloc(PBO_COUNT * sizeof(GLuint));
     84    glGenBuffersARB(PBO_COUNT, pboIds);
     85    glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[0]);
     86    glBufferDataARB(GL_PIXEL_PACK_BUFFER_ARB, width * height * 3, 0,
     87                    GL_STREAM_READ_ARB);
     88    glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[1]);
     89    glBufferDataARB(GL_PIXEL_PACK_BUFFER_ARB, width * height * 3, 0,
     90                    GL_STREAM_READ_ARB);
     91
     92    glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, 0);
     93  } else {
     94    use_pbo = false;
     95    Thread::Warn("GL_ARB_pixel_buffer_object is not suppoorted\n");
     96  }
     97  if (isGlExtensionSupported("GL_PACK_INVERT_MESA")) {
     98    invert_pixel = false;
     99    Thread::Warn("GL_PACK_INVERT_MESA is suppoorted\n");
     100  } else {
     101    invert_pixel = true;
     102  }
     103}
     104
     105SimuCameraGL::~SimuCameraGL() {
     106  free(buffer);
     107
     108  if (use_pbo) {
     109    glDeleteBuffersARB(PBO_COUNT, pboIds);
     110    free(pboIds);
     111  }
     112}
     113
     114void SimuCameraGL::setNearValue(float zn) { camera->setNearValue(zn); }
     115
     116void SimuCameraGL::setFarValue(float zf) { camera->setFarValue(zf); }
     117
     118void SimuCameraGL::UpdateFrom(const io_data *data) {
     119  if (noGui() == false && data == NULL) {
     120    smgr->setActiveCamera(camera);
     121    smgr->getVideoDriver()->setViewPort(rect<s32>(x, y, x + width, y + height));
     122    smgr->drawAll();
     123    // use_pbo=false;
     124    getImage();
     125  }
     126}
     127
     128void SimuCameraGL::getImage(void) {
     129  if (disable_output)
     130    return;
     131  // convert from irrlicht top left origin to gl bottom left origin
     132  int y = smgr->getVideoDriver()->getScreenSize().Height - height - this->y;
     133
     134  // We want to read the front buffer to get the latest render finished.
     135  Time a = GetTime();
     136  glReadBuffer(GL_FRONT);
     137  if (use_pbo) // with PBO
     138  {
     139    // increment current index first then get the next index
     140    // "index" is used to read pixels from a framebuffer to a PBO
     141    // "nextIndex" is used to process pixels in the other PBO
     142    index = (index + 1) % PBO_COUNT;
     143    int nextIndex = (index + 1) % PBO_COUNT;
     144
     145    // copy pixels from framebuffer to PBO
     146    // Use offset instead of pointer.
     147    // OpenGL should perform asynch DMA transfer, so glReadPixels() will return
     148    // immediately.
     149    glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[index]);
     150    glReadPixels(x, y, width, height, GL_BGR, GL_UNSIGNED_BYTE, 0);
     151
     152    // map the PBO that contain framebuffer pixels before processing it
     153    glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[nextIndex]);
     154    GLubyte *src = (GLubyte *)glMapBufferARB(
     155        GL_PIXEL_PACK_BUFFER_ARB, GL_READ_WRITE_ARB); // GL_READ_ONLY_ARB);
     156    if (src) {
     157      putImage((char *)src);
     158      glUnmapBufferARB(
     159          GL_PIXEL_PACK_BUFFER_ARB); // release pointer to the mapped buffer
    76160    }
    77 
    78     if(isGlExtensionSupported("GL_ARB_pixel_buffer_object"))
    79     {
    80         use_pbo=true;
    81          // create 2 pixel buffer objects, you need to delete them when program exits.
    82         // glBufferDataARB with NULL pointer reserves only memory space.
    83         pboIds=(GLuint*)malloc(PBO_COUNT*sizeof(GLuint));
    84         glGenBuffersARB(PBO_COUNT, pboIds);
    85         glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[0]);
    86         glBufferDataARB(GL_PIXEL_PACK_BUFFER_ARB, width*height*3, 0, GL_STREAM_READ_ARB);
    87         glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[1]);
    88         glBufferDataARB(GL_PIXEL_PACK_BUFFER_ARB, width*height*3, 0, GL_STREAM_READ_ARB);
    89 
    90         glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, 0);
     161    glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, 0);
     162  } else {
     163    glReadPixels(x, y, width, height, GL_BGR, GL_UNSIGNED_BYTE, buffer);
     164    putImage(buffer);
     165  }
     166
     167  glReadBuffer(GL_BACK);
     168  Time b = GetTime();
     169  Time c = b - a;
     170  // printf("%s %f\n",Thread::ObjectName().c_str(),(float)(c/1000000.));
     171}
     172
     173void SimuCameraGL::putImage(char *buf) {
     174  if (invert_pixel == true) {
     175    // opengl images are horizontally flipped, so we have to fix that here.
     176    const s32 pitch = width * 3;
     177    char *pixels = buf;
     178    char *p2 = pixels + (height - 1) * pitch;
     179    char *tmpBuffer = new char[pitch];
     180    for (int i = 0; i < height; i += 2) {
     181      memcpy(tmpBuffer, pixels, pitch);
     182      memcpy(pixels, p2, pitch);
     183      memcpy(p2, tmpBuffer, pitch);
     184      pixels += pitch;
     185      p2 -= pitch;
    91186    }
    92     else
    93     {
    94         use_pbo=false;
    95         Thread::Warn("GL_ARB_pixel_buffer_object is not suppoorted\n");
    96     }
    97     if(isGlExtensionSupported("GL_PACK_INVERT_MESA"))
    98     {
    99         invert_pixel=false;
    100         Thread::Warn("GL_PACK_INVERT_MESA is suppoorted\n");
    101     }
    102     else
    103     {
    104         invert_pixel=true;
    105     }
    106 }
    107 
    108 SimuCameraGL::~SimuCameraGL()
    109 {
    110     free(buffer);
    111 
    112     if(use_pbo)
    113     {
    114         glDeleteBuffersARB(PBO_COUNT, pboIds);
    115         free(pboIds);
    116     }
    117 }
    118 
    119 void SimuCameraGL::setNearValue(float zn) {
    120     camera->setNearValue(zn);
    121 }
    122 
    123 
    124 void SimuCameraGL::setFarValue(float zf) {
    125     camera->setFarValue(zf);
    126 }
    127 
    128 
    129 void SimuCameraGL::UpdateFrom(const io_data *data)
    130 {
    131     if(noGui()==false && data==NULL)
    132     {
    133         smgr->setActiveCamera(camera);
    134         smgr->getVideoDriver()->setViewPort(rect<s32>(x,y,x+width,y+height));
    135         smgr->drawAll();
    136         //use_pbo=false;
    137         getImage();
    138     }
    139 }
    140 
    141 void SimuCameraGL::getImage(void)
    142 {
    143     if(disable_output) return;
    144     //convert from irrlicht top left origin to gl bottom left origin
    145     int y=smgr->getVideoDriver()->getScreenSize().Height-height-this->y;
    146 
    147         // We want to read the front buffer to get the latest render finished.
    148         Time a=GetTime();
    149         glReadBuffer(GL_FRONT);
    150         if(use_pbo) // with PBO
    151     {
    152         // increment current index first then get the next index
    153         // "index" is used to read pixels from a framebuffer to a PBO
    154         // "nextIndex" is used to process pixels in the other PBO
    155         index = (index + 1) % PBO_COUNT;
    156         int nextIndex = (index + 1) % PBO_COUNT;
    157 
    158         // copy pixels from framebuffer to PBO
    159         // Use offset instead of pointer.
    160         // OpenGL should perform asynch DMA transfer, so glReadPixels() will return immediately.
    161         glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[index]);
    162         glReadPixels(x,y,width,height, GL_BGR, GL_UNSIGNED_BYTE, 0);
    163 
    164         // map the PBO that contain framebuffer pixels before processing it
    165         glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pboIds[nextIndex]);
    166         GLubyte* src = (GLubyte*)glMapBufferARB(GL_PIXEL_PACK_BUFFER_ARB,GL_READ_WRITE_ARB);//GL_READ_ONLY_ARB);
    167         if(src)
    168         {
    169             putImage((char*)src);
    170             glUnmapBufferARB(GL_PIXEL_PACK_BUFFER_ARB);     // release pointer to the mapped buffer
    171         }
    172         glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, 0);
    173     }
    174     else
    175     {
    176         glReadPixels(x,y,width,height, GL_BGR, GL_UNSIGNED_BYTE, buffer);
    177         putImage(buffer);
    178     }
    179 
    180         glReadBuffer(GL_BACK);
    181         Time b=GetTime();
    182         Time c=b-a;
    183         //printf("%s %f\n",Thread::ObjectName().c_str(),(float)(c/1000000.));
    184 }
    185 
    186 void SimuCameraGL::putImage(char* buf)
    187 {
    188     if(invert_pixel==true)
    189     {
    190         // opengl images are horizontally flipped, so we have to fix that here.
    191         const s32 pitch=width*3;
    192         char* pixels = buf;
    193         char* p2 = pixels + (height - 1) * pitch;
    194         char* tmpBuffer = new char[pitch];
    195         for (int i=0; i < height; i += 2)
    196         {
    197             memcpy(tmpBuffer, pixels, pitch);
    198             memcpy(pixels, p2, pitch);
    199             memcpy(p2, tmpBuffer, pitch);
    200             pixels += pitch;
    201             p2 -= pitch;
    202         }
    203         delete [] tmpBuffer;
    204     }
    205 
    206     shmem->Write(buf,width*height*3);
    207 }
    208 
    209 void SimuCameraGL::animateNode(ISceneNode* node, u32 timeMs)
    210 {
    211     ICameraSceneNode* camera = static_cast<ICameraSceneNode*>(node);
    212 
    213     matrix4 m;
    214     m.setRotationDegrees(Node()->getRotation());
    215 
    216     // transform forward vector of camera
    217     vector3df frv =ToIrrlichtCoordinates(direction->Value());
    218     m.transformVect(frv);
    219 
    220     // transform upvector of camera
    221     vector3df upv =ToIrrlichtCoordinates(up->Value());
    222     m.transformVect(upv);
    223 
    224     // transform camera offset (thanks to Zeuss for finding it was missing)
    225     vector3df offset = ToIrrlichtCoordinates(position->Value());
    226     m.transformVect(offset);
    227 
    228     // set camera
    229     camera->setPosition(Node()->getPosition() + offset); //position camera in front of the ship
    230     camera->setUpVector(upv); //set up vector of camera >> Zeuss - tested with +node->getPostion() and it didnt work, but this works fine.
    231     camera->setTarget(Node()->getPosition() + offset+frv); //set target of camera (look at point) >> Zeuss - Dont forget to add the node positiob
    232 
    233     camera->setFOV(Euler::ToRadian(fov->Value()));
    234 }
    235 
    236 ISceneNodeAnimator* SimuCameraGL::createClone(ISceneNode* node,
    237             ISceneManager* newManager)
    238 {
    239     return NULL;
     187    delete[] tmpBuffer;
     188  }
     189
     190  shmem->Write(buf, width * height * 3);
     191}
     192
     193void SimuCameraGL::animateNode(ISceneNode *node, u32 timeMs) {
     194  ICameraSceneNode *camera = static_cast<ICameraSceneNode *>(node);
     195
     196  matrix4 m;
     197  m.setRotationDegrees(Node()->getRotation());
     198
     199  // transform forward vector of camera
     200  vector3df frv = ToIrrlichtCoordinates(direction->Value());
     201  m.transformVect(frv);
     202
     203  // transform upvector of camera
     204  vector3df upv = ToIrrlichtCoordinates(up->Value());
     205  m.transformVect(upv);
     206
     207  // transform camera offset (thanks to Zeuss for finding it was missing)
     208  vector3df offset = ToIrrlichtCoordinates(position->Value());
     209  m.transformVect(offset);
     210
     211  // set camera
     212  camera->setPosition(Node()->getPosition() +
     213                      offset); // position camera in front of the ship
     214  camera->setUpVector(upv);    // set up vector of camera >> Zeuss - tested with
     215  // +node->getPostion() and it didnt work, but this
     216  // works fine.
     217  camera->setTarget(Node()->getPosition() + offset + frv); // set target of
     218                                                           // camera (look at
     219                                                           // point) >> Zeuss -
     220                                                           // Dont forget to add
     221                                                           // the node positiob
     222
     223  camera->setFOV(Euler::ToRadian(fov->Value()));
     224}
     225
     226ISceneNodeAnimator *SimuCameraGL::createClone(ISceneNode *node,
     227                                              ISceneManager *newManager) {
     228  return NULL;
    240229}
    241230

  • trunk/lib/FlairSimulator/src/SimuCameraGL.h

    r10 r15  
    1818#include <ISceneNodeAnimator.h>
    1919
    20 // in order to get function prototypes from glext.h, define GL_GLEXT_PROTOTYPES before including glext.h
     20// in order to get function prototypes from glext.h, define GL_GLEXT_PROTOTYPES
     21// before including glext.h
    2122#define GL_GLEXT_PROTOTYPES
    2223#include <GL/gl.h>
    2324
    24 namespace irr
    25 {
    26     namespace scene
    27     {
    28         class ICameraSceneNode;
    29     }
     25namespace irr {
     26namespace scene {
     27class ICameraSceneNode;
     28}
    3029}
    3130
    32 namespace flair
    33 {
    34     namespace gui
    35     {
    36         class DoubleSpinBox;
    37         class Vector3DSpinBox;
    38     }
    39     namespace simulator
    40     {
    41         class Model;
    42     }
     31namespace flair {
     32namespace gui {
     33class DoubleSpinBox;
     34class Vector3DSpinBox;
     35}
     36namespace simulator {
     37class Model;
     38}
    4339}
    4440
    45 namespace flair
    46 {
    47 namespace sensor
    48 {
    49     /*! \class SimuCameraGL
    50     * \brief Class for a simulation camera
    51     *
    52     */
    53     class SimuCameraGL : public SimuCamera, public SensorGL, public irr::scene::ISceneNodeAnimator
    54     {
    55         public:
    56             //top left origin
    57             SimuCameraGL(const simulator::Model* parent,std::string name,int width,int height,int x,int y,int dev_id);
    58             ~SimuCameraGL();
    59             //! Sets the value of the near clipping plane. (default: 1.0f)
    60             /** \param zn: New z near value. */
    61             void setNearValue(float zn);
     41namespace flair {
     42namespace sensor {
     43/*! \class SimuCameraGL
     44* \brief Class for a simulation camera
     45*
     46*/
     47class SimuCameraGL : public SimuCamera,
     48                     public SensorGL,
     49                     public irr::scene::ISceneNodeAnimator {
     50public:
     51  // top left origin
     52  SimuCameraGL(const simulator::Model *parent, std::string name, int width,
     53               int height, int x, int y, int dev_id);
     54  ~SimuCameraGL();
     55  //! Sets the value of the near clipping plane. (default: 1.0f)
     56  /** \param zn: New z near value. */
     57  void setNearValue(float zn);
    6258
    63             //! Sets the value of the far clipping plane (default: 2000.0f)
    64             /** \param zf: New z far value. */
    65             void setFarValue(float zf);
     59  //! Sets the value of the far clipping plane (default: 2000.0f)
     60  /** \param zf: New z far value. */
     61  void setFarValue(float zf);
    6662
    67         private:
    68             void UpdateFrom(const core::io_data *data);
    69             void animateNode(irr::scene::ISceneNode* node, irr::u32 timeMs);
    70             ISceneNodeAnimator* createClone(irr::scene::ISceneNode* node,irr::scene::ISceneManager* newManager=0);
    71             void getImage(void);
    72             void putImage(char* pixels);
    73             irr::scene::ICameraSceneNode* camera;
    74             irr::scene::ISceneManager* smgr;
    75             gui::Vector3DSpinBox *position,*direction,*up;
    76             gui::DoubleSpinBox *fov;
    77             int width,height,x,y;
    78             char* buffer;
    79             bool use_pbo,invert_pixel,disable_output;
    80             GLuint *pboIds;
    81             int index ;
    82     };
     63private:
     64  void UpdateFrom(const core::io_data *data);
     65  void animateNode(irr::scene::ISceneNode *node, irr::u32 timeMs);
     66  ISceneNodeAnimator *createClone(irr::scene::ISceneNode *node,
     67                                  irr::scene::ISceneManager *newManager = 0);
     68  void getImage(void);
     69  void putImage(char *pixels);
     70  irr::scene::ICameraSceneNode *camera;
     71  irr::scene::ISceneManager *smgr;
     72  gui::Vector3DSpinBox *position, *direction, *up;
     73  gui::DoubleSpinBox *fov;
     74  int width, height, x, y;
     75  char *buffer;
     76  bool use_pbo, invert_pixel, disable_output;
     77  GLuint *pboIds;
     78  int index;
     79};
    8380} // end namespace simulator
    8481} // end namespace flair

  • trunk/lib/FlairSimulator/src/SimuLaserGL.cpp

    r10 r15  
    3636using namespace flair::simulator;
    3737
    38 namespace flair
    39 {
    40 namespace sensor
    41 {
    42 SimuLaserGL::SimuLaserGL(const Model* parent,std::string name,int dev_id) :SimuLaser(parent,name,dev_id),SensorGL(parent)
    43 {
    44     Tab* setup_tab=new Tab(parent->GetTabWidget(),name);
    45             position=new Vector3DSpinBox(setup_tab->NewRow(),"position",-2,2,.01);
    46             direction=new Vector3DSpinBox(setup_tab->NewRow(),"direction",-2,2,.01);
    47             range=new DoubleSpinBox(setup_tab->NewRow(),"range:",0,30,1);
     38namespace flair {
     39namespace sensor {
     40SimuLaserGL::SimuLaserGL(const Model *parent, std::string name, int dev_id)
     41    : SimuLaser(parent, name, dev_id), SensorGL(parent) {
     42  Tab *setup_tab = new Tab(parent->GetTabWidget(), name);
     43  position = new Vector3DSpinBox(setup_tab->NewRow(), "position", -2, 2, .01);
     44  direction = new Vector3DSpinBox(setup_tab->NewRow(), "direction", -2, 2, .01);
     45  range = new DoubleSpinBox(setup_tab->NewRow(), "range:", 0, 30, 1);
    4846}
    4947
    50 SimuLaserGL::~SimuLaserGL()
    51 {
     48SimuLaserGL::~SimuLaserGL() {}
    5249
    53 }
     50void SimuLaserGL::UpdateFrom(const io_data *data) {
     51  float value[360];
    5452
    55 void SimuLaserGL::UpdateFrom(const io_data *data)
    56 {
    57     float value[360];
     53  if (noGui() == false && data == NULL) {
     54    for (int i = 0; i < 360; i++) {
     55      line3d<f32> ray_laser;         // rayon provenant de l'ultra son
     56      vector3df intersection_laser;  // point intersection us avec le sol
     57      triangle3df hitTriangle_laser; // triangle intersection us avec le sol
    5858
    59     if(noGui()==false && data==NULL)
    60     {
    61         for (int i=0; i<360; i++)
    62         {
    63             line3d<f32> ray_laser;//rayon provenant de l'ultra son
    64             vector3df intersection_laser;//point intersection us avec le sol
    65             triangle3df hitTriangle_laser;//triangle intersection us avec le sol
     59      // get rotation matrix of node - Zeuss must be getRotation not
     60      // getRelativeTransformation
     61      matrix4 m;
     62      matrix4 M;
     63      m.setRotationDegrees(Node()->getRotation());
    6664
    67              //get rotation matrix of node - Zeuss must be getRotation not getRelativeTransformation
    68             matrix4 m;
    69             matrix4 M;
    70             m.setRotationDegrees(Node()->getRotation());
     65      // Matrice de rotation pour balayage du laser, angle i
     66      M.setRotationDegrees(vector3df(0, 0, i));
     67      // transform forward vector of us
     68      vector3df frv = ToIrrlichtCoordinates(direction->Value());
     69      M.transformVect(frv);
     70      m.transformVect(frv);
     71      frv.normalize();
    7172
    72             //Matrice de rotation pour balayage du laser, angle i
    73             M.setRotationDegrees(vector3df(0,0,i));
    74             // transform forward vector of us
    75             vector3df frv =ToIrrlichtCoordinates(direction->Value());
    76             M.transformVect(frv);
    77             m.transformVect(frv);
    78             frv.normalize();
     73      // transform pos vector of us
     74      vector3df pos = ToIrrlichtCoordinates(position->Value());
     75      m.transformVect(pos);
    7976
     77      ray_laser.start = Node()->getPosition() + pos;
     78      ray_laser.end = ray_laser.start + ToIrrlichtScale(range->Value()) * frv;
    8079
    81              // transform pos vector of us
    82             vector3df pos =ToIrrlichtCoordinates(position->Value());
    83             m.transformVect(pos);
    84 
    85             ray_laser.start =Node()->getPosition() + pos;
    86             ray_laser.end = ray_laser.start + ToIrrlichtScale(range->Value())*frv;
    87 
    88             scene::ISceneNode * selectedSceneNode =
    89                 CollMan()->getSceneNodeAndCollisionPointFromRay(ray_laser,intersection_laser,hitTriangle_laser);
    90     // //////////////////////////////////////////
    91             if(selectedSceneNode) //
    92             {
    93                 value[i]=ToSimulatorScale(ray_laser.start.getDistanceFrom(intersection_laser));
    94             }
    95             else
    96             {
    97                 value[i]=-1;
    98             }
    99 
    100         }
    101         shmem->Write((char*)value,360*sizeof(float));
     80      scene::ISceneNode *selectedSceneNode =
     81          CollMan()->getSceneNodeAndCollisionPointFromRay(
     82              ray_laser, intersection_laser, hitTriangle_laser);
     83      // //////////////////////////////////////////
     84      if (selectedSceneNode) //
     85      {
     86        value[i] = ToSimulatorScale(
     87            ray_laser.start.getDistanceFrom(intersection_laser));
     88      } else {
     89        value[i] = -1;
     90      }
    10291    }
     92    shmem->Write((char *)value, 360 * sizeof(float));
     93  }
    10394}
    10495

  • trunk/lib/FlairSimulator/src/SimuLaserGL.h

    r10 r15  
    1717#include <SensorGL.h>
    1818
    19 namespace flair
    20 {
    21     namespace gui
    22     {
    23         class DoubleSpinBox;
    24         class Vector3DSpinBox;
    25     }
     19namespace flair {
     20namespace gui {
     21class DoubleSpinBox;
     22class Vector3DSpinBox;
     23}
    2624}
    2725
    28 namespace flair
    29 {
    30     namespace simulator
    31     {
    32         class Model;
    33     }
     26namespace flair {
     27namespace simulator {
     28class Model;
     29}
    3430}
    3531
    36 namespace flair
    37 {
    38 namespace sensor
    39 {
    40     /*! \class SimuUsGL
    41     * \brief Class for a simulation us
    42     *
    43     */
    44     class SimuLaserGL : public SimuLaser, public SensorGL
    45     {
    46         public:
    47             SimuLaserGL(const simulator::Model* parent,std::string name,int dev_id);
    48             ~SimuLaserGL();
     32namespace flair {
     33namespace sensor {
     34/*! \class SimuUsGL
     35* \brief Class for a simulation us
     36*
     37*/
     38class SimuLaserGL : public SimuLaser, public SensorGL {
     39public:
     40  SimuLaserGL(const simulator::Model *parent, std::string name, int dev_id);
     41  ~SimuLaserGL();
    4942
    50         private:
    51             void UpdateFrom(const core::io_data *data);
    52             gui::DoubleSpinBox *range;
    53             gui::Vector3DSpinBox *position,*direction;
    54     };
     43private:
     44  void UpdateFrom(const core::io_data *data);
     45  gui::DoubleSpinBox *range;
     46  gui::Vector3DSpinBox *position, *direction;
     47};
    5548} // end namespace sensor
    5649} // end namespace flair

  • trunk/lib/FlairSimulator/src/SimuUsGL.cpp

    r10 r15  
    3636using namespace flair::simulator;
    3737
    38 namespace flair
    39 {
    40 namespace sensor
    41 {
    42 SimuUsGL::SimuUsGL(const Model* parent,std::string name,int dev_id) :SimuUs(parent,name,dev_id),SensorGL(parent)
    43 {
    44     Tab* setup_tab=new Tab(parent->GetTabWidget(),name);
    45             position=new Vector3DSpinBox(setup_tab->NewRow(),"position",-2,2,.01);
    46             direction=new Vector3DSpinBox(setup_tab->NewRow(),"direction",-2,2,.01);
    47             range=new DoubleSpinBox(setup_tab->NewRow(),"range:",0,6,1);
     38namespace flair {
     39namespace sensor {
     40SimuUsGL::SimuUsGL(const Model *parent, std::string name, int dev_id)
     41    : SimuUs(parent, name, dev_id), SensorGL(parent) {
     42  Tab *setup_tab = new Tab(parent->GetTabWidget(), name);
     43  position = new Vector3DSpinBox(setup_tab->NewRow(), "position", -2, 2, .01);
     44  direction = new Vector3DSpinBox(setup_tab->NewRow(), "direction", -2, 2, .01);
     45  range = new DoubleSpinBox(setup_tab->NewRow(), "range:", 0, 6, 1);
    4846}
    4947
    50 SimuUsGL::~SimuUsGL()
    51 {
     48SimuUsGL::~SimuUsGL() {}
    5249
    53 }
     50void SimuUsGL::UpdateFrom(const io_data *data) {
     51  float value;
     52#ifdef GL
     53  if (noGui() == true) {
     54#endif
     55    // todo: utiliser le placement de l'us dans le drone et sa portée
     56    cvmatrix *input = (cvmatrix *)data;
     57    value = input->Value(9, 0);
     58    shmem->Write((char *)&value, sizeof(float));
     59#ifdef GL
     60  }
    5461
    55 void SimuUsGL::UpdateFrom(const io_data *data)
    56 {
    57     float value;
    58 #ifdef GL
    59     if(noGui()==true)
    60     {
    61 #endif
    62         //todo: utiliser le placement de l'us dans le drone et sa portée
    63         cvmatrix *input=(cvmatrix*)data;
    64         value=input->Value(9,0);
    65         shmem->Write((char*)&value,sizeof(float));
    66 #ifdef GL
     62  if (noGui() == false && data == NULL) {
     63    line3d<f32> ray_us;         // rayon provenant de l'ultra son
     64    vector3df intersection_us;  // point intersection us avec le sol
     65    triangle3df hitTriangle_us; // triangle intersection us avec le sol
     66
     67    // get rotation matrix of node - Zeuss must be getRotation not
     68    // getRelativeTransformation
     69    matrix4 m;
     70    m.setRotationDegrees(Node()->getRotation());
     71
     72    // transform forward vector of us
     73    vector3df frv = ToIrrlichtCoordinates(direction->Value());
     74    m.transformVect(frv);
     75    frv.normalize();
     76
     77    // transform pos vector of us
     78    vector3df pos = ToIrrlichtCoordinates(position->Value());
     79    m.transformVect(pos);
     80
     81    ray_us.start = Node()->getPosition() + pos;
     82    ray_us.end = ray_us.start + ToIrrlichtScale(range->Value()) * frv;
     83
     84    scene::ISceneNode *selectedSceneNode =
     85        CollMan()->getSceneNodeAndCollisionPointFromRay(ray_us, intersection_us,
     86                                                        hitTriangle_us);
     87
     88    if (selectedSceneNode) {
     89      float value =
     90          ToSimulatorScale(ray_us.start.getDistanceFrom(intersection_us));
     91      shmem->Write((char *)&value, sizeof(float));
    6792    }
    68 
    69     if(noGui()==false && data==NULL)
    70     {
    71         line3d<f32> ray_us;//rayon provenant de l'ultra son
    72         vector3df intersection_us;//point intersection us avec le sol
    73         triangle3df hitTriangle_us;//triangle intersection us avec le sol
    74 
    75          //get rotation matrix of node - Zeuss must be getRotation not getRelativeTransformation
    76         matrix4 m;
    77         m.setRotationDegrees(Node()->getRotation());
    78 
    79         // transform forward vector of us
    80         vector3df frv =ToIrrlichtCoordinates(direction->Value());
    81         m.transformVect(frv);
    82         frv.normalize();
    83 
    84          // transform pos vector of us
    85         vector3df pos =ToIrrlichtCoordinates(position->Value());
    86         m.transformVect(pos);
    87 
    88         ray_us.start =Node()->getPosition() +pos;
    89         ray_us.end = ray_us.start + ToIrrlichtScale(range->Value())*frv;
    90 
    91         scene::ISceneNode * selectedSceneNode =
    92             CollMan()->getSceneNodeAndCollisionPointFromRay(ray_us,intersection_us,hitTriangle_us);
    93 
    94         if(selectedSceneNode)
    95         {
    96             float value=ToSimulatorScale(ray_us.start.getDistanceFrom(intersection_us));
    97             shmem->Write((char*)&value,sizeof(float));
    98         }
    99     }
     93  }
    10094#endif
    10195}

  • trunk/lib/FlairSimulator/src/SimuUsGL.h

    r10 r15  
    1717#include <SensorGL.h>
    1818
    19 namespace flair
    20 {
    21     namespace gui
    22     {
    23         class DoubleSpinBox;
    24         class Vector3DSpinBox;
    25     }
     19namespace flair {
     20namespace gui {
     21class DoubleSpinBox;
     22class Vector3DSpinBox;
     23}
    2624}
    2725
    28 namespace flair
    29 {
    30     namespace simulator
    31     {
    32         class Model;
    33     }
     26namespace flair {
     27namespace simulator {
     28class Model;
     29}
    3430}
    3531
    36 namespace flair
    37 {
    38 namespace sensor
    39 {
    40     /*! \class SimuUsGL
    41     * \brief Class for a simulation us
    42     *
    43     */
    44     class SimuUsGL : public SimuUs, public SensorGL
    45     {
    46         public:
    47             SimuUsGL(const simulator::Model* parent,std::string name,int dev_id);
    48             ~SimuUsGL();
     32namespace flair {
     33namespace sensor {
     34/*! \class SimuUsGL
     35* \brief Class for a simulation us
     36*
     37*/
     38class SimuUsGL : public SimuUs, public SensorGL {
     39public:
     40  SimuUsGL(const simulator::Model *parent, std::string name, int dev_id);
     41  ~SimuUsGL();
    4942
    50         private:
    51             void UpdateFrom(const core::io_data *data);
    52             gui::DoubleSpinBox *range;
    53             gui::Vector3DSpinBox *position,*direction;
    54     };
     43private:
     44  void UpdateFrom(const core::io_data *data);
     45  gui::DoubleSpinBox *range;
     46  gui::Vector3DSpinBox *position, *direction;
     47};
    5548} // end namespace sensor
    5649} // end namespace flair

  • trunk/lib/FlairSimulator/src/Simulator.cpp

    r10 r15  
    2727using namespace flair::core;
    2828
    29 namespace
    30 {
    31     flair::simulator::Simulator* simu=NULL;
     29namespace {
     30flair::simulator::Simulator *simu = NULL;
    3231}
    3332
    34 namespace flair
    35 {
    36 namespace simulator
    37 {
     33namespace flair {
     34namespace simulator {
    3835
    39 Simulator* getSimulator(void) {
    40     return simu;
     36Simulator *getSimulator(void) { return simu; }
     37
     38Simulator::Simulator(string name, int optitrack_mstime, float yaw_deg)
     39    : FrameworkManager(name) {
     40  if (simu != NULL)
     41    Err("Simulator should be instanced only one time\n");
     42
     43  pimpl_ = new Simulator_impl(this, optitrack_mstime, yaw_deg);
     44  simu = this;
    4145}
    4246
    43 Simulator::Simulator(string name,int optitrack_mstime,float yaw_deg): FrameworkManager(name)
    44 {
    45     if(simu!=NULL) Err("Simulator should be instanced only one time\n");
    46 
    47     pimpl_=new Simulator_impl(this,optitrack_mstime,yaw_deg);
    48     simu=this;
    49 }
    50 
    51 Simulator::~Simulator()
    52 {
    53     delete pimpl_;
    54 }
     47Simulator::~Simulator() { delete pimpl_; }
    5548
    5649Quaternion Simulator::ToVRPNReference(Quaternion quat_in) {
    57     Quaternion yaw_rot_quat;
    58     Euler yaw_rot_euler(0,0,-pimpl_->yaw_rad);//yaw_rad is vrpn rotation in earth reference
    59     yaw_rot_euler.ToQuaternion(yaw_rot_quat);
     50  Quaternion yaw_rot_quat;
     51  Euler yaw_rot_euler(
     52      0, 0, -pimpl_->yaw_rad); // yaw_rad is vrpn rotation in earth reference
     53  yaw_rot_euler.ToQuaternion(yaw_rot_quat);
    6054
    61     return yaw_rot_quat*quat_in;
     55  return yaw_rot_quat * quat_in;
    6256}
    6357
    6458Vector3D Simulator::ToVRPNReference(Vector3D point_in) {
    65     Quaternion yaw_rot_quat;
    66     Euler yaw_rot_euler(0,0,-pimpl_->yaw_rad);//yaw_rad is vrpn rotation in earth reference
    67     yaw_rot_euler.ToQuaternion(yaw_rot_quat);
    68     point_in.Rotate(yaw_rot_quat);
     59  Quaternion yaw_rot_quat;
     60  Euler yaw_rot_euler(
     61      0, 0, -pimpl_->yaw_rad); // yaw_rad is vrpn rotation in earth reference
     62  yaw_rot_euler.ToQuaternion(yaw_rot_quat);
     63  point_in.Rotate(yaw_rot_quat);
    6964
    70     return point_in;
     65  return point_in;
    7166}
    7267
    73 float Simulator::Yaw(void) const
    74 {
    75     return pimpl_->yaw_rad;
    76 }
     68float Simulator::Yaw(void) const { return pimpl_->yaw_rad; }
    7769
    78 void Simulator::RunSimu(void)
    79 {
    80     pimpl_->RunSimu();
    81 }
     70void Simulator::RunSimu(void) { pimpl_->RunSimu(); }
    8271
    8372} // end namespace simulator

  • trunk/lib/FlairSimulator/src/Simulator.h

    r10 r15  
    2424
    2525namespace flair {
    26     namespace core {
    27         class Quaternion;
    28         class Vector3D;
    29     }
     26namespace core {
     27class Quaternion;
     28class Vector3D;
     29}
    3030}
    3131
    32 namespace flair
    33 {
    34 namespace simulator
    35 {
    36     class Model;
    37     class Gui;
     32namespace flair {
     33namespace simulator {
     34class Model;
     35class Gui;
    3836
    39     class Simulator: public core::FrameworkManager
    40     {
    41         friend class Model;
    42         friend class Gui;
    43         friend class GenericObject;
     37class Simulator : public core::FrameworkManager {
     38  friend class Model;
     39  friend class Gui;
     40  friend class GenericObject;
    4441
    45         public:
    46             //yaw_deg: rotation of the vrpn coordinate with respect to the earth coordinate, around z axis
    47             Simulator(std::string name,int optitrack_mstime=10,float yaw_deg=30);
    48             ~Simulator();
    49             void RunSimu(void);
    50             //get rotation of the vrpn coordinate with respect to the earth coordinate, around z axis; in radians
    51             float Yaw(void) const;
    52             //compute rotation of the vrpn coordinate with respect to the earth coordinate, around z axis
    53             core::Quaternion ToVRPNReference(core::Quaternion quat_in);
    54             core::Vector3D ToVRPNReference(core::Vector3D point_in);
     42public:
     43  // yaw_deg: rotation of the vrpn coordinate with respect to the earth
     44  // coordinate, around z axis
     45  Simulator(std::string name, int optitrack_mstime = 10, float yaw_deg = 30);
     46  ~Simulator();
     47  void RunSimu(void);
     48  // get rotation of the vrpn coordinate with respect to the earth coordinate,
     49  // around z axis; in radians
     50  float Yaw(void) const;
     51  // compute rotation of the vrpn coordinate with respect to the earth
     52  // coordinate, around z axis
     53  core::Quaternion ToVRPNReference(core::Quaternion quat_in);
     54  core::Vector3D ToVRPNReference(core::Vector3D point_in);
    5555
    56         private:
    57             Simulator_impl* pimpl_;
    58     };
     56private:
     57  Simulator_impl *pimpl_;
     58};
    5959
    60     /*!
    61     * \brief get Simulator
    62     *
    63     * \return the Simulator
    64     */
    65     Simulator* getSimulator(void);
     60/*!
     61* \brief get Simulator
     62*
     63* \return the Simulator
     64*/
     65Simulator *getSimulator(void);
    6666
    6767} // end namespace simulator

  • trunk/lib/FlairSimulator/src/Simulator_impl.cpp

    r10 r15  
    2929using namespace flair::simulator;
    3030
    31 Simulator_impl::Simulator_impl(Simulator* self,int optitrack_mstime,float yaw_deg): vrpn_Connection_IP(), Thread(self,"simulator",1)
    32 {
    33     this->self=self;
    34     this->optitrack_mstime=optitrack_mstime;
    35     yaw_rad=Euler::ToRadian(yaw_deg);
     31Simulator_impl::Simulator_impl(Simulator *self, int optitrack_mstime,
     32                               float yaw_deg)
     33    : vrpn_Connection_IP(), Thread(self, "simulator", 1) {
     34  this->self = self;
     35  this->optitrack_mstime = optitrack_mstime;
     36  yaw_rad = Euler::ToRadian(yaw_deg);
    3637}
    3738
    38 Simulator_impl::~Simulator_impl()
    39 {
    40     //printf("del Simulator_impl\n");
     39Simulator_impl::~Simulator_impl() {
     40  // printf("del Simulator_impl\n");
    4141
    42     SafeStop();
    43     Join();
     42  SafeStop();
     43  Join();
    4444
    45     for(size_t i=0;i<models.size();i++)
    46     {
    47         models.at(i)->pimpl_->SafeStop();
    48         models.at(i)->pimpl_->Join();
    49         delete models.at(i);
    50     }
     45  for (size_t i = 0; i < models.size(); i++) {
     46    models.at(i)->pimpl_->SafeStop();
     47    models.at(i)->pimpl_->Join();
     48    delete models.at(i);
     49  }
    5150
    5251#ifdef GL
    53     if(getGui()!=NULL) delete getGui();
     52  if (getGui() != NULL)
     53    delete getGui();
    5454#endif
    5555
    56      //printf("del Simulator_impl ok\n");
     56  // printf("del Simulator_impl ok\n");
    5757}
    5858
    59 void Simulator_impl::Run(void)
    60 {
    61     SetPeriodMS(optitrack_mstime);
     59void Simulator_impl::Run(void) {
     60  SetPeriodMS(optitrack_mstime);
    6261
    63     while(ToBeStopped()==false)
    64         {
    65             WaitPeriod();
    66 //printf("%lld\n",GetTime());
    67         mainloop();
    68         for(size_t i=0;i<models.size();i++)
    69         {
    70             models.at(i)->pimpl_->mainloop();
    71         }
    72         }
     62  while (ToBeStopped() == false) {
     63    WaitPeriod();
     64    // printf("%lld\n",GetTime());
     65    mainloop();
     66    for (size_t i = 0; i < models.size(); i++) {
     67      models.at(i)->pimpl_->mainloop();
     68    }
     69  }
    7370}
    7471
     72void Simulator_impl::RunSimu(void) {
     73  if (models.size() == 0) {
     74    self->Err("No model to run\n");
     75    return;
     76  }
    7577
    76 void Simulator_impl::RunSimu(void)
    77 {
    78     if(models.size()==0)
    79     {
    80         self->Err("No model to run\n");
    81         return;
    82     }
     78  for (size_t i = 0; i < models.size(); i++) {
     79    models.at(i)->pimpl_->Start();
     80  }
    8381
    84     for(size_t i=0;i<models.size();i++)
    85     {
    86         models.at(i)->pimpl_->Start();
    87     }
    88 
    89     Start();
     82  Start();
    9083
    9184#ifdef GL
    92     if(getGui()!=NULL)
    93     {
    94         getGui()->pimpl_->RunGui(models,objects);
    95     }
    96     else
     85  if (getGui() != NULL) {
     86    getGui()->pimpl_->RunGui(models, objects);
     87  } else
    9788#endif
    98     {
    99         models.at(0)->pimpl_->Join();
    100     }
     89  {
     90    models.at(0)->pimpl_->Join();
     91  }
    10192
    102     SafeStop();
    103     Join();
     93  SafeStop();
     94  Join();
    10495}

  • trunk/lib/FlairSimulator/src/X4.cpp

    r10 r15  
    3131#endif
    3232
    33 #define K_MOT 0.4f //blade animation
    34 #define G (float)9.81 //gravity ( N/(m/s²) )
     33#define K_MOT 0.4f    // blade animation
     34#define G (float)9.81 // gravity ( N/(m/s²) )
    3535
    3636#ifdef GL
    … …  
    4343using namespace flair::actuator;
    4444
    45 namespace flair
    46 {
    47 namespace simulator
    48 {
    49 
    50 X4::X4(const Simulator* parent,std::string name, int dev_id): Model(parent,name)
    51 {
    52     Tab *setup_tab=new Tab(GetTabWidget(),"model");
    53         m=new DoubleSpinBox(setup_tab->NewRow(),"mass (kg):",0,20,0.1);
    54         arm_length=new DoubleSpinBox(setup_tab->LastRowLastCol(),"arm length (m):",0,2,0.1);
    55         //l_cg=new DoubleSpinBox(setup_tab,"position G (m):",0,2,-0.5,0.5,0.02);//position du centre de gravité/centre de poussé
    56         k_mot=new DoubleSpinBox(setup_tab->NewRow(),"k_mot:",0,1,0.001,3);// vitesse rotation² (unité arbitraire) -> force (N)
    57         c_mot=new DoubleSpinBox(setup_tab->LastRowLastCol(),"c_mot:",0,1,0.001,3);// vitesse rotation moteur -> couple (N.m/unité arbitraire)
    58         f_air_vert=new DoubleSpinBox(setup_tab->NewRow(),"f_air_vert:",0,10,1);//frottements air depl. vertical, aussi utilisé pour les rotations ( N/(m/s) ) (du aux helices en rotation)
    59         f_air_lat=new DoubleSpinBox(setup_tab->LastRowLastCol(),"f_air_lat:",0,10,1);//frottements air deplacements lateraux ( N/(m/s) )
    60         j_roll=new DoubleSpinBox(setup_tab->NewRow(),"j_roll:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
    61         j_pitch=new DoubleSpinBox(setup_tab->LastRowLastCol(),"j_pitch:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
    62         j_yaw=new DoubleSpinBox(setup_tab->LastRowLastCol(),"j_yaw:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
    63 
    64     motors=new SimuBldc(this,name,4,dev_id);
    65 }
    66 
    67 X4::~X4()
    68 {
    69     //les objets irrlicht seront automatiquement detruits (moteurs, helices, pales) par parenté
    70 }
    71 
    72 
    73 #ifdef GL
    74 
    75 void X4::Draw(void)
    76 {
    77     //create unite (1m=100cm) UAV; scale will be adapted according to arm_length parameter
    78     //note that the frame used is irrlicht one:
    79     //left handed, North East Up
    80     const IGeometryCreator *geo;
    81     geo=getGui()->getSceneManager()->getGeometryCreator();
    82 
    83     //cylinders are aligned with y axis
    84     red_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 255, 0, 0));
    85     black_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 128, 128, 128));
    86     motor=geo->createCylinderMesh(7.5,15,16);//,SColor(0, 128, 128, 128));
    87     //geo->drop();
    88 
    89     ITexture* texture=getGui()->getTexture("carbone.jpg");
    90     fl_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-135));
    91     fr_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-45));
    92     rl_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,135),texture);
    93     rr_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,45),texture);
    94 
    95     texture=getGui()->getTexture("metal047.jpg");
    96     fl_motor=new MeshSceneNode(this, motor, vector3df(70.71,-70.71,2.5),vector3df(90,0,0),texture);
    97     fr_motor=new MeshSceneNode(this, motor ,vector3df(70.71,70.71,2.5),vector3df(90,0,0),texture);
    98     rl_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,-70.71,2.5),vector3df(90,0,0),texture);
    99     rr_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,70.71,2.5),vector3df(90,0,0),texture);
    100 
    101     fl_blade=new Blade(this, vector3df(70.71,-70.71,17.5));
    102     fr_blade=new Blade(this, vector3df(70.71,70.71,17.5),true);
    103     rl_blade=new Blade(this, vector3df(-70.71,-70.71,17.5),true);
    104     rr_blade=new Blade(this, vector3df(-70.71,70.71,17.5));
    105 
    106     motor_speed_mutex=new Mutex(this);
    107     for(int i=0;i<4;i++) motor_speed[i]=0;
    108     ExtraDraw();
    109 }
    110 
    111 void X4::AnimateModel(void)
    112 {
    113     motor_speed_mutex->GetMutex();
    114     fl_blade->SetRotationSpeed(K_MOT*motor_speed[0]);
    115     fr_blade->SetRotationSpeed(-K_MOT*motor_speed[1]);
    116     rl_blade->SetRotationSpeed(-K_MOT*motor_speed[2]);
    117     rr_blade->SetRotationSpeed(K_MOT*motor_speed[3]);
    118     motor_speed_mutex->ReleaseMutex();
    119 
    120     //adapt UAV size
    121     if(arm_length->ValueChanged()==true)
    122     {
    123         setScale(arm_length->Value());
    124     }
    125 }
    126 
    127 size_t X4::dbtSize(void) const
    128 {
    129     return 6*sizeof(float)+4*sizeof(float);//6ddl+4helices
    130 }
    131 
    132 void X4::WritedbtBuf(char* dbtbuf)
    133 {/*
    134     float *buf=(float*)dbtbuf;
    135     vector3df vect=getPosition();
    136     memcpy(buf,&vect.X,sizeof(float));
    137     buf++;
    138     memcpy(buf,&vect.Y,sizeof(float));
    139     buf++;
    140     memcpy(buf,&vect.Z,sizeof(float));
    141     buf++;
    142     vect=getRotation();
    143     memcpy(buf,&vect.X,sizeof(float));
    144     buf++;
    145     memcpy(buf,&vect.Y,sizeof(float));
    146     buf++;
    147     memcpy(buf,&vect.Z,sizeof(float));
    148     buf++;
    149     memcpy(buf,&motors,sizeof(rtsimu_motors));*/
    150 }
    151 
    152 void X4::ReaddbtBuf(char* dbtbuf)
    153 {/*
    154     float *buf=(float*)dbtbuf;
    155     vector3df vect;
    156     memcpy(&vect.X,buf,sizeof(float));
    157     buf++;
    158     memcpy(&vect.Y,buf,sizeof(float));
    159     buf++;
    160     memcpy(&vect.Z,buf,sizeof(float));
    161     buf++;
    162     setPosition(vect);
    163     memcpy(&vect.X,buf,sizeof(float));
    164     buf++;
    165     memcpy(&vect.Y,buf,sizeof(float));
    166     buf++;
    167     memcpy(&vect.Z,buf,sizeof(float));
    168     buf++;
    169     ((ISceneNode*)(this))->setRotation(vect);
    170     memcpy(&motors,buf,sizeof(rtsimu_motors));
    171     AnimateModele();*/
    172 }
    173 #endif //GL
    174 
    175 //states are computed on fixed frame NED
    176 //x north
    177 //y east
    178 //z down
    179 void X4::CalcModel(void)
    180 {
    181     float fl_speed,fr_speed,rl_speed,rr_speed;
    182     float u_roll,u_pitch,u_yaw,u_thrust;
    183 #ifdef GL
    184     motor_speed_mutex->GetMutex();
    185 #endif //GL
    186     motors->GetSpeeds(motor_speed);
    187 #ifdef GL
    188     motor_speed_mutex->ReleaseMutex();
    189 #endif //GL
    190         fl_speed=motor_speed[0];
    191         fr_speed=motor_speed[1];
    192         rl_speed=motor_speed[2];
    193         rr_speed=motor_speed[3];
    194 
    195     /*
    196         ** ===================================================================
    197         **    u roll: roll torque
    198         **
    199         ** ===================================================================
    200         */
    201         u_roll=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+rl_speed*rl_speed-fr_speed*fr_speed-rr_speed*rr_speed)*sqrtf(2)/2;
    202 
    203     /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed of the quadri in the body frame). It is a discrete integrator
    204         state[0].W.x=(dT()/j_roll->Value())*((j_yaw->Value()-j_pitch->Value())*state[-1].W.y*state[-1].W.z + u_roll) +state[-1].W.x;
    205 
    206         //u_roll=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+rl_speed*rl_speed-fr_speed*fr_speed-rr_speed*rr_speed)*sqrtf(2)/2;
    207         //state[0].W.x=(dT()/j_roll->Value())*(u_roll-m->Value()*G*l_cg->Value()*sinf(state[-2].W.x)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.x)+state[-1].W.x;
    208 
    209         /*
    210         ** ===================================================================
    211         **   u pitch : pitch torque
    212         **
    213         ** ===================================================================
    214         */
    215         u_pitch=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+fr_speed*fr_speed-rl_speed*rl_speed-rr_speed*rr_speed)*sqrtf(2)/2;
    216 
    217     /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed of the quadri in the body frame). It is a discrete integrator
    218         state[0].W.y=(dT()/j_pitch->Value())*((j_roll->Value()-j_yaw->Value())*state[-1].W.x*state[-1].W.z + u_pitch)+state[-1].W.y;
    219 
    220         //u_pitch=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+fr_speed*fr_speed-rl_speed*rl_speed-rr_speed*rr_speed)*sqrtf(2)/2;
    221         //state[0].W.y=(dT()/j_pitch->Value())*(u_pitch-m->Value()*G*l_cg->Value()*sinf(state[-2].W.y)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.y)+state[-1].W.y;
    222 
    223         /*
    224         ** ===================================================================
    225         **    u yaw : yaw torque
    226         **
    227         ** ===================================================================
    228         */
    229         u_yaw=c_mot->Value()*(fl_speed*fl_speed+rr_speed*rr_speed-fr_speed*fr_speed-rl_speed*rl_speed);
    230 
    231     /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed of the quadri in the body frame). It is a discrete integrator
    232         state[0].W.z=(dT()/j_yaw->Value())* u_yaw +state[-1].W.z;
    233 
    234         //u_yaw=c_mot->Value()*(fl_speed*fl_speed+rr_speed*rr_speed-fr_speed*fr_speed-rl_speed*rl_speed);
    235         //state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
    236 
    237     // compute quaternion from W
    238     // Quaternion derivative: dQ = 0.5*(Q*Qw)
    239     Quaternion dQ=state[-1].Quat.GetDerivative(state[0].W);
    240 
    241     // Quaternion integration
    242     state[0].Quat = state[-1].Quat +dQ*dT();
    243     state[0].Quat.Normalize();
    244 
    245     // Calculation of the thrust from the reference speed of motors
    246         u_thrust=k_mot->Value()*(fl_speed*fl_speed+fr_speed*fr_speed+rl_speed*rl_speed+rr_speed*rr_speed);
    247     Vector3D vect(0,0,-u_thrust);
    248     vect.Rotate(state[0].Quat);
    249 
    250     /*
    251         ** ===================================================================
    252         **     x double integrator
    253         **
    254         ** ===================================================================
    255         */
    256         state[0].Pos.x=(dT()*dT()/m->Value())*(vect.x-f_air_lat->Value()*(state[-1].Pos.x-state[-2].Pos.x)/dT())+2*state[-1].Pos.x-state[-2].Pos.x;
    257     state[0].Vel.x=(state[0].Pos.x-state[-1].Pos.x)/dT();
    258 
    259         /*
    260         ** ===================================================================
    261         **     y double integrator
    262         **
    263         ** ===================================================================
    264         */
    265         state[0].Pos.y=(dT()*dT()/m->Value())*(vect.y-f_air_lat->Value()*(state[-1].Pos.y-state[-2].Pos.y)/dT())+2*state[-1].Pos.y-state[-2].Pos.y;
    266     state[0].Vel.y=(state[0].Pos.y-state[-1].Pos.y)/dT();
    267 
    268         /*
    269         ** ===================================================================
    270         **     z double integrator
    271         **
    272         ** ===================================================================
    273         */
    274         state[0].Pos.z=(dT()*dT()/m->Value())*(vect.z+f_air_vert->Value()*(state[-1].Pos.z-state[-2].Pos.z)/dT()+m->Value()*G)+2*state[-1].Pos.z-state[-2].Pos.z;
    275     state[0].Vel.z=(state[0].Pos.z-state[-1].Pos.z)/dT();
     45namespace flair {
     46namespace simulator {
     47
     48X4::X4(const Simulator *parent, std::string name, int dev_id)
     49    : Model(parent, name) {
     50  Tab *setup_tab = new Tab(GetTabWidget(), "model");
     51  m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1);
     52  arm_length = new DoubleSpinBox(setup_tab->LastRowLastCol(), "arm length (m):",
     53                                 0, 2, 0.1);
     54  // l_cg=new DoubleSpinBox(setup_tab,"position G
     55  // (m):",0,2,-0.5,0.5,0.02);//position du centre de gravité/centre de poussé
     56  k_mot =
     57      new DoubleSpinBox(setup_tab->NewRow(), "k_mot:", 0, 1, 0.001,
     58                        3); // vitesse rotation² (unité arbitraire) -> force (N)
     59  c_mot = new DoubleSpinBox(
     60      setup_tab->LastRowLastCol(), "c_mot:", 0, 1, 0.001,
     61      3); // vitesse rotation moteur -> couple (N.m/unité arbitraire)
     62  f_air_vert = new DoubleSpinBox(setup_tab->NewRow(), "f_air_vert:", 0, 10,
     63                                 1); // frottements air depl. vertical, aussi
     64                                     // utilisé pour les rotations ( N/(m/s) )
     65                                     // (du aux helices en rotation)
     66  f_air_lat =
     67      new DoubleSpinBox(setup_tab->LastRowLastCol(), "f_air_lat:", 0, 10,
     68                        1); // frottements air deplacements lateraux ( N/(m/s) )
     69  j_roll = new DoubleSpinBox(setup_tab->NewRow(), "j_roll:", 0, 1, 0.001,
     70                             5); // moment d'inertie d'un axe (N.m.s²/rad)
     71  j_pitch =
     72      new DoubleSpinBox(setup_tab->LastRowLastCol(), "j_pitch:", 0, 1, 0.001,
     73                        5); // moment d'inertie d'un axe (N.m.s²/rad)
     74  j_yaw = new DoubleSpinBox(setup_tab->LastRowLastCol(), "j_yaw:", 0, 1, 0.001,
     75                            5); // moment d'inertie d'un axe (N.m.s²/rad)
     76
     77  motors = new SimuBldc(this, name, 4, dev_id);
     78}
     79
     80X4::~X4() {
     81  // les objets irrlicht seront automatiquement detruits (moteurs, helices,
     82  // pales) par parenté
     83}
     84
     85#ifdef GL
     86
     87void X4::Draw(void) {
     88  // create unite (1m=100cm) UAV; scale will be adapted according to arm_length
     89  // parameter
     90  // note that the frame used is irrlicht one:
     91  // left handed, North East Up
     92  const IGeometryCreator *geo;
     93  geo = getGui()->getSceneManager()->getGeometryCreator();
     94
     95  // cylinders are aligned with y axis
     96  red_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 255, 0, 0));
     97  black_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 128, 128, 128));
     98  motor = geo->createCylinderMesh(7.5, 15, 16); //,SColor(0, 128, 128, 128));
     99  // geo->drop();
     100
     101  ITexture *texture = getGui()->getTexture("carbone.jpg");
     102  fl_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
     103                             vector3df(0, 0, -135));
     104  fr_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
     105                             vector3df(0, 0, -45));
     106  rl_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
     107                             vector3df(0, 0, 135), texture);
     108  rr_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
     109                             vector3df(0, 0, 45), texture);
     110
     111  texture = getGui()->getTexture("metal047.jpg");
     112  fl_motor = new MeshSceneNode(this, motor, vector3df(70.71, -70.71, 2.5),
     113                               vector3df(90, 0, 0), texture);
     114  fr_motor = new MeshSceneNode(this, motor, vector3df(70.71, 70.71, 2.5),
     115                               vector3df(90, 0, 0), texture);
     116  rl_motor = new MeshSceneNode(this, motor, vector3df(-70.71, -70.71, 2.5),
     117                               vector3df(90, 0, 0), texture);
     118  rr_motor = new MeshSceneNode(this, motor, vector3df(-70.71, 70.71, 2.5),
     119                               vector3df(90, 0, 0), texture);
     120
     121  fl_blade = new Blade(this, vector3df(70.71, -70.71, 17.5));
     122  fr_blade = new Blade(this, vector3df(70.71, 70.71, 17.5), true);
     123  rl_blade = new Blade(this, vector3df(-70.71, -70.71, 17.5), true);
     124  rr_blade = new Blade(this, vector3df(-70.71, 70.71, 17.5));
     125
     126  motor_speed_mutex = new Mutex(this);
     127  for (int i = 0; i < 4; i++)
     128    motor_speed[i] = 0;
     129  ExtraDraw();
     130}
     131
     132void X4::AnimateModel(void) {
     133  motor_speed_mutex->GetMutex();
     134  fl_blade->SetRotationSpeed(K_MOT * motor_speed[0]);
     135  fr_blade->SetRotationSpeed(-K_MOT * motor_speed[1]);
     136  rl_blade->SetRotationSpeed(-K_MOT * motor_speed[2]);
     137  rr_blade->SetRotationSpeed(K_MOT * motor_speed[3]);
     138  motor_speed_mutex->ReleaseMutex();
     139
     140  // adapt UAV size
     141  if (arm_length->ValueChanged() == true) {
     142    setScale(arm_length->Value());
     143  }
     144}
     145
     146size_t X4::dbtSize(void) const {
     147  return 6 * sizeof(float) + 4 * sizeof(float); // 6ddl+4helices
     148}
     149
     150void X4::WritedbtBuf(
     151    char *dbtbuf) { /*
     152                       float *buf=(float*)dbtbuf;
     153                       vector3df vect=getPosition();
     154                       memcpy(buf,&vect.X,sizeof(float));
     155                       buf++;
     156                       memcpy(buf,&vect.Y,sizeof(float));
     157                       buf++;
     158                       memcpy(buf,&vect.Z,sizeof(float));
     159                       buf++;
     160                       vect=getRotation();
     161                       memcpy(buf,&vect.X,sizeof(float));
     162                       buf++;
     163                       memcpy(buf,&vect.Y,sizeof(float));
     164                       buf++;
     165                       memcpy(buf,&vect.Z,sizeof(float));
     166                       buf++;
     167                       memcpy(buf,&motors,sizeof(rtsimu_motors));*/
     168}
     169
     170void X4::ReaddbtBuf(
     171    char *dbtbuf) { /*
     172                       float *buf=(float*)dbtbuf;
     173                       vector3df vect;
     174                       memcpy(&vect.X,buf,sizeof(float));
     175                       buf++;
     176                       memcpy(&vect.Y,buf,sizeof(float));
     177                       buf++;
     178                       memcpy(&vect.Z,buf,sizeof(float));
     179                       buf++;
     180                       setPosition(vect);
     181                       memcpy(&vect.X,buf,sizeof(float));
     182                       buf++;
     183                       memcpy(&vect.Y,buf,sizeof(float));
     184                       buf++;
     185                       memcpy(&vect.Z,buf,sizeof(float));
     186                       buf++;
     187                       ((ISceneNode*)(this))->setRotation(vect);
     188                       memcpy(&motors,buf,sizeof(rtsimu_motors));
     189                       AnimateModele();*/
     190}
     191#endif // GL
     192
     193// states are computed on fixed frame NED
     194// x north
     195// y east
     196// z down
     197void X4::CalcModel(void) {
     198  float fl_speed, fr_speed, rl_speed, rr_speed;
     199  float u_roll, u_pitch, u_yaw, u_thrust;
     200#ifdef GL
     201  motor_speed_mutex->GetMutex();
     202#endif // GL
     203  motors->GetSpeeds(motor_speed);
     204#ifdef GL
     205  motor_speed_mutex->ReleaseMutex();
     206#endif // GL
     207  fl_speed = motor_speed[0];
     208  fr_speed = motor_speed[1];
     209  rl_speed = motor_speed[2];
     210  rr_speed = motor_speed[3];
     211
     212  /*
     213      ** ===================================================================
     214      **    u roll: roll torque
     215      **
     216      ** ===================================================================
     217      */
     218  u_roll = arm_length->Value() * k_mot->Value() *
     219           (fl_speed * fl_speed + rl_speed * rl_speed - fr_speed * fr_speed -
     220            rr_speed * rr_speed) *
     221           sqrtf(2) / 2;
     222
     223  /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed
     224  /// of the quadri in the body frame). It is a discrete integrator
     225  state[0].W.x =
     226      (dT() / j_roll->Value()) *
     227          ((j_yaw->Value() - j_pitch->Value()) * state[-1].W.y * state[-1].W.z +
     228           u_roll) +
     229      state[-1].W.x;
     230
     231  // u_roll=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+rl_speed*rl_speed-fr_speed*fr_speed-rr_speed*rr_speed)*sqrtf(2)/2;
     232  // state[0].W.x=(dT()/j_roll->Value())*(u_roll-m->Value()*G*l_cg->Value()*sinf(state[-2].W.x)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.x)+state[-1].W.x;
     233
     234  /*
     235  ** ===================================================================
     236  **   u pitch : pitch torque
     237  **
     238  ** ===================================================================
     239  */
     240  u_pitch = arm_length->Value() * k_mot->Value() *
     241            (fl_speed * fl_speed + fr_speed * fr_speed - rl_speed * rl_speed -
     242             rr_speed * rr_speed) *
     243            sqrtf(2) / 2;
     244
     245  /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed
     246  /// of the quadri in the body frame). It is a discrete integrator
     247  state[0].W.y =
     248      (dT() / j_pitch->Value()) *
     249          ((j_roll->Value() - j_yaw->Value()) * state[-1].W.x * state[-1].W.z +
     250           u_pitch) +
     251      state[-1].W.y;
     252
     253  // u_pitch=arm_length->Value()*k_mot->Value()*(fl_speed*fl_speed+fr_speed*fr_speed-rl_speed*rl_speed-rr_speed*rr_speed)*sqrtf(2)/2;
     254  // state[0].W.y=(dT()/j_pitch->Value())*(u_pitch-m->Value()*G*l_cg->Value()*sinf(state[-2].W.y)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.y)+state[-1].W.y;
     255
     256  /*
     257  ** ===================================================================
     258  **    u yaw : yaw torque
     259  **
     260  ** ===================================================================
     261  */
     262  u_yaw = c_mot->Value() * (fl_speed * fl_speed + rr_speed * rr_speed -
     263                            fr_speed * fr_speed - rl_speed * rl_speed);
     264
     265  /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed
     266  /// of the quadri in the body frame). It is a discrete integrator
     267  state[0].W.z = (dT() / j_yaw->Value()) * u_yaw + state[-1].W.z;
     268
     269  // u_yaw=c_mot->Value()*(fl_speed*fl_speed+rr_speed*rr_speed-fr_speed*fr_speed-rl_speed*rl_speed);
     270  // state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
     271
     272  // compute quaternion from W
     273  // Quaternion derivative: dQ = 0.5*(Q*Qw)
     274  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
     275
     276  // Quaternion integration
     277  state[0].Quat = state[-1].Quat + dQ * dT();
     278  state[0].Quat.Normalize();
     279
     280  // Calculation of the thrust from the reference speed of motors
     281  u_thrust = k_mot->Value() * (fl_speed * fl_speed + fr_speed * fr_speed +
     282                               rl_speed * rl_speed + rr_speed * rr_speed);
     283  Vector3D vect(0, 0, -u_thrust);
     284  vect.Rotate(state[0].Quat);
     285
     286  /*
     287      ** ===================================================================
     288      **     x double integrator
     289      **
     290      ** ===================================================================
     291      */
     292  state[0].Pos.x =
     293      (dT() * dT() / m->Value()) *
     294          (vect.x -
     295           f_air_lat->Value() * (state[-1].Pos.x - state[-2].Pos.x) / dT()) +
     296      2 * state[-1].Pos.x - state[-2].Pos.x;
     297  state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
     298
     299  /*
     300  ** ===================================================================
     301  **     y double integrator
     302  **
     303  ** ===================================================================
     304  */
     305  state[0].Pos.y =
     306      (dT() * dT() / m->Value()) *
     307          (vect.y -
     308           f_air_lat->Value() * (state[-1].Pos.y - state[-2].Pos.y) / dT()) +
     309      2 * state[-1].Pos.y - state[-2].Pos.y;
     310  state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
     311
     312  /*
     313  ** ===================================================================
     314  **     z double integrator
     315  **
     316  ** ===================================================================
     317  */
     318  state[0].Pos.z =
     319      (dT() * dT() / m->Value()) *
     320          (vect.z +
     321           f_air_vert->Value() * (state[-1].Pos.z - state[-2].Pos.z) / dT() +
     322           m->Value() * G) +
     323      2 * state[-1].Pos.z - state[-2].Pos.z;
     324  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
    276325
    277326#ifndef GL
    278     if(state[0].Pos.z<0) state[0].Pos.z=0;
     327  if (state[0].Pos.z < 0)
     328    state[0].Pos.z = 0;
    279329#endif
    280 
    281330}
    282331

  • trunk/lib/FlairSimulator/src/X4.h

    r10 r15  
    2222#include <stdio.h>
    2323
    24 namespace flair
    25 {
    26     namespace core
    27     {
    28         class Mutex;
    29     }
    30     namespace gui
    31     {
    32         class DoubleSpinBox;
    33     }
    34     namespace actuator
    35     {
    36         class SimuBldc;
    37     }
     24namespace flair {
     25namespace core {
     26class Mutex;
     27}
     28namespace gui {
     29class DoubleSpinBox;
     30}
     31namespace actuator {
     32class SimuBldc;
     33}
    3834}
    3935
    4036#ifdef GL
    41 namespace irr
    42 {
    43     namespace scene
    44     {
    45         class IMesh;
    46     }
     37namespace irr {
     38namespace scene {
     39class IMesh;
     40}
    4741}
    4842#endif
    4943
    50 namespace flair
    51 {
    52 namespace simulator
    53 {
    54     class Simulator;
    55     class Blade;
    56     class MeshSceneNode;
     44namespace flair {
     45namespace simulator {
     46class Simulator;
     47class Blade;
     48class MeshSceneNode;
    5749
    58     class X4 : public Model
    59     {
    60         public:
    61             X4(const Simulator* parent,std::string name, int dev_id);
    62             ~X4();
     50class X4 : public Model {
     51public:
     52  X4(const Simulator *parent, std::string name, int dev_id);
     53  ~X4();
    6354#ifdef GL
    64             virtual void Draw(void);
    65             virtual void ExtraDraw(void){};
     55  virtual void Draw(void);
     56  virtual void ExtraDraw(void){};
    6657
    67         protected:
    68                     irr::scene::IMesh *red_arm,*black_arm,*motor;
    69                     MeshSceneNode *fl_arm,*fr_arm,*rl_arm,*rr_arm;
    70                     MeshSceneNode *fl_motor,*fr_motor,*rl_motor,*rr_motor;
    71                     Blade *fl_blade,*fr_blade,*rl_blade,*rr_blade;
    72                     core::Mutex *motor_speed_mutex;
     58protected:
     59  irr::scene::IMesh *red_arm, *black_arm, *motor;
     60  MeshSceneNode *fl_arm, *fr_arm, *rl_arm, *rr_arm;
     61  MeshSceneNode *fl_motor, *fr_motor, *rl_motor, *rr_motor;
     62  Blade *fl_blade, *fr_blade, *rl_blade, *rr_blade;
     63  core::Mutex *motor_speed_mutex;
    7364#endif
    74         private:
    75             void CalcModel(void);
    76     #ifdef GL
    77             bool OnEvent(const irr::SEvent& event){};
    78             void AnimateModel(void);
    79             size_t dbtSize(void) const;
    80             void WritedbtBuf(char* dbtbuf);
    81             void ReaddbtBuf(char* dbtbuf);
    82     #endif
     65private:
     66  void CalcModel(void);
     67#ifdef GL
     68  bool OnEvent(const irr::SEvent &event){};
     69  void AnimateModel(void);
     70  size_t dbtSize(void) const;
     71  void WritedbtBuf(char *dbtbuf);
     72  void ReaddbtBuf(char *dbtbuf);
     73#endif
    8374
    84             actuator::SimuBldc *motors;
    85             float motor_speed[4];
    86             gui::DoubleSpinBox *m,*arm_length,*l_cg;
    87             gui::DoubleSpinBox *k_mot,*c_mot;
    88             gui::DoubleSpinBox *f_air_vert,*f_air_lat;
    89             gui::DoubleSpinBox *j_roll,*j_pitch,*j_yaw;
    90     };
     75  actuator::SimuBldc *motors;
     76  float motor_speed[4];
     77  gui::DoubleSpinBox *m, *arm_length, *l_cg;
     78  gui::DoubleSpinBox *k_mot, *c_mot;
     79  gui::DoubleSpinBox *f_air_vert, *f_air_lat;
     80  gui::DoubleSpinBox *j_roll, *j_pitch, *j_yaw;
     81};
    9182} // end namespace simulator
    9283} // end namespace flair

  • trunk/lib/FlairSimulator/src/X8.cpp

    r10 r15  
    3131#endif
    3232
    33 #define K_MOT 0.4f //blade animation
    34 #define G (float)9.81 //gravity ( N/(m/s²) )
     33#define K_MOT 0.4f    // blade animation
     34#define G (float)9.81 // gravity ( N/(m/s²) )
    3535
    3636#ifdef GL
    … …  
    4343using namespace flair::actuator;
    4444
    45 namespace flair
    46 {
    47 namespace simulator
    48 {
    49 
    50 X8::X8(const Simulator* parent,std::string name, int dev_id): Model(parent,name)
    51 {
    52     Tab *setup_tab=new Tab(GetTabWidget(),"model");
    53         m=new DoubleSpinBox(setup_tab->NewRow(),"mass (kg):",0,20,0.1);
    54         arm_length=new DoubleSpinBox(setup_tab->LastRowLastCol(),"arm length (m):",0,2,0.1);
    55         l_cg=new DoubleSpinBox(setup_tab->LastRowLastCol(),"position G (m):",-0.5,0.5,0.02);//position du centre de gravité/centre de poussé
    56         k_mot=new DoubleSpinBox(setup_tab->NewRow(),"k_mot:",0,1,0.001,3);// vitesse rotation² (unité arbitraire) -> force (N)
    57         c_mot=new DoubleSpinBox(setup_tab->LastRowLastCol(),"c_mot:",0,1,0.001,3);// vitesse rotation moteur -> couple (N.m/unité arbitraire)
    58         f_air_vert=new DoubleSpinBox(setup_tab->NewRow(),"f_air_vert:",0,10,1);//frottements air depl. vertical, aussi utilisé pour les rotations ( N/(m/s) ) (du aux helices en rotation)
    59         f_air_lat=new DoubleSpinBox(setup_tab->LastRowLastCol(),"f_air_lat:",0,10,1);//frottements air deplacements lateraux ( N/(m/s) )
    60         j_roll=new DoubleSpinBox(setup_tab->NewRow(),"j_roll:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
    61         j_pitch=new DoubleSpinBox(setup_tab->LastRowLastCol(),"j_pitch:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
    62         j_yaw=new DoubleSpinBox(setup_tab->LastRowLastCol(),"j_yaw:",0,1,0.001,5); //moment d'inertie d'un axe (N.m.s²/rad)
    63         j_r=new DoubleSpinBox(setup_tab->NewRow(),"j_r:",0,1,0.001);// moment des helices (N.m.s²/rad)
    64         sigma=new DoubleSpinBox(setup_tab->LastRowLastCol(),"sigma:",0,1,0.1); // coefficient de perte d efficacite aerodynamique (sans unite)
    65         S=new DoubleSpinBox(setup_tab->LastRowLastCol(),"S:",1,2,0.1); // coefficient de forme des helices 1<S=1+Ss/Sprop<2 (sans unite)
    66 
    67     motors=new SimuBldc(this,name,8,dev_id);
    68 }
    69 
    70 void X8::Draw(){
    71 #ifdef GL
    72 
    73         //create unite (1m=100cm) UAV; scale will be adapted according to arm_length parameter
    74     //note that the frame used is irrlicht one:
    75     //left handed, North East Up
    76 
    77     const IGeometryCreator *geo;
    78     geo=getGui()->getSceneManager()->getGeometryCreator();
    79 
    80     //cylinders are aligned with y axis
    81     red_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 255, 0, 0));
    82     black_arm=geo->createCylinderMesh(2.5,100,16,SColor(0, 128, 128, 128));
    83     motor=geo->createCylinderMesh(7.5,15,16);//,SColor(0, 128, 128, 128));
    84     //geo->drop();
    85 
    86     ITexture* texture=getGui()->getTexture("carbone.jpg");
    87     fl_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-135));
    88     fr_arm=new MeshSceneNode(this, red_arm, vector3df(0,0,0),vector3df(0,0,-45));
    89     rl_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,135),texture);
    90     rr_arm=new MeshSceneNode(this, black_arm, vector3df(0,0,0),vector3df(0,0,45),texture);
    91 
    92     texture=getGui()->getTexture("metal047.jpg");
    93     tfl_motor=new MeshSceneNode(this, motor, vector3df(70.71,-70.71,2.5),vector3df(90,0,0),texture);
    94     tfr_motor=new MeshSceneNode(this, motor ,vector3df(70.71,70.71,2.5),vector3df(90,0,0),texture);
    95     trl_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,-70.71,2.5),vector3df(90,0,0),texture);
    96     trr_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,70.71,2.5),vector3df(90,0,0),texture);
    97 
    98     bfl_motor=new MeshSceneNode(this, motor, vector3df(70.71,-70.71,-17.5),vector3df(90,0,0),texture);
    99     bfr_motor=new MeshSceneNode(this, motor ,vector3df(70.71,70.71,-17.5),vector3df(90,0,0),texture);
    100     brl_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,-70.71,-17.5),vector3df(90,0,0),texture);
    101     brr_motor=new MeshSceneNode(this, motor ,vector3df(-70.71,70.71,-17.5),vector3df(90,0,0),texture);
    102 
    103     tfl_blade=new Blade(this, vector3df(70.71,-70.71,17.5));
    104     tfr_blade=new Blade(this, vector3df(70.71,70.71,17.5),true);
    105     trl_blade=new Blade(this, vector3df(-70.71,-70.71,17.5),true);
    106     trr_blade=new Blade(this, vector3df(-70.71,70.71,17.5));
    107 
    108     bfl_blade=new Blade(this, vector3df(70.71,-70.71,-17.5));
    109     bfr_blade=new Blade(this, vector3df(70.71,70.71,-17.5),true);
    110     brl_blade=new Blade(this, vector3df(-70.71,-70.71,-17.5),true);
    111     brr_blade=new Blade(this, vector3df(-70.71,70.71,-17.5));
    112 
    113     motor_speed_mutex=new Mutex(this);
    114     for(int i=0;i<8;i++) motor_speed[i]=0;
    115     ExtraDraw();
    116     #endif
    117 }
    118 
    119 X8::~X8()
    120 {
    121     //les objets irrlicht seront automatiquement detruits (moteurs, helices, pales) par parenté
    122 }
    123 
    124 #ifdef GL
    125 void X8::AnimateModel(void)
    126 {
    127     motor_speed_mutex->GetMutex();
    128     tfl_blade->SetRotationSpeed(K_MOT*motor_speed[0]);
    129     tfr_blade->SetRotationSpeed(-K_MOT*motor_speed[1]);
    130     trl_blade->SetRotationSpeed(-K_MOT*motor_speed[2]);
    131     trr_blade->SetRotationSpeed(K_MOT*motor_speed[3]);
    132 
    133     bfl_blade->SetRotationSpeed(-K_MOT*motor_speed[4]);
    134     bfr_blade->SetRotationSpeed(K_MOT*motor_speed[5]);
    135     brl_blade->SetRotationSpeed(K_MOT*motor_speed[6]);
    136     brr_blade->SetRotationSpeed(-K_MOT*motor_speed[7]);
    137     motor_speed_mutex->ReleaseMutex();
    138 
    139     //adapt UAV size
    140     if(arm_length->ValueChanged()==true)
    141     {
    142         setScale(arm_length->Value());
    143     }
    144 }
    145 
    146 size_t X8::dbtSize(void) const
    147 {
    148     return 6*sizeof(float)+4*sizeof(float);//6ddl+4helices
    149 }
    150 
    151 void X8::WritedbtBuf(char* dbtbuf)
    152 {/*
    153     float *buf=(float*)dbtbuf;
    154     vector3df vect=getPosition();
    155     memcpy(buf,&vect.X,sizeof(float));
    156     buf++;
    157     memcpy(buf,&vect.Y,sizeof(float));
    158     buf++;
    159     memcpy(buf,&vect.Z,sizeof(float));
    160     buf++;
    161     vect=getRotation();
    162     memcpy(buf,&vect.X,sizeof(float));
    163     buf++;
    164     memcpy(buf,&vect.Y,sizeof(float));
    165     buf++;
    166     memcpy(buf,&vect.Z,sizeof(float));
    167     buf++;
    168     memcpy(buf,&motors,sizeof(rtsimu_motors));*/
    169 }
    170 
    171 void X8::ReaddbtBuf(char* dbtbuf)
    172 {/*
    173     float *buf=(float*)dbtbuf;
    174     vector3df vect;
    175     memcpy(&vect.X,buf,sizeof(float));
    176     buf++;
    177     memcpy(&vect.Y,buf,sizeof(float));
    178     buf++;
    179     memcpy(&vect.Z,buf,sizeof(float));
    180     buf++;
    181     setPosition(vect);
    182     memcpy(&vect.X,buf,sizeof(float));
    183     buf++;
    184     memcpy(&vect.Y,buf,sizeof(float));
    185     buf++;
    186     memcpy(&vect.Z,buf,sizeof(float));
    187     buf++;
    188     ((ISceneNode*)(this))->setRotation(vect);
    189     memcpy(&motors,buf,sizeof(rtsimu_motors));
    190     AnimateModele();*/
    191 }
    192 #endif //GL
    193 
    194 //states are computed on fixed frame NED
    195 //x north
    196 //y east
    197 //z down
    198 void X8::CalcModel(void)
    199 {
    200     float tfl_speed,tfr_speed,trl_speed,trr_speed;
    201     float bfl_speed,bfr_speed,brl_speed,brr_speed;
    202     float u_roll,u_pitch,u_yaw,u_thrust;
    203     float omega;
    204 #ifdef GL
    205     motor_speed_mutex->GetMutex();
    206 #endif //GL
    207     motors->GetSpeeds(motor_speed);
    208 #ifdef GL
    209     motor_speed_mutex->ReleaseMutex();
    210 #endif //GL
    211         tfl_speed=motor_speed[0];
    212         tfr_speed=motor_speed[1];
    213         trl_speed=motor_speed[2];
    214         trr_speed=motor_speed[3];
    215         bfl_speed=motor_speed[4];
    216         bfr_speed=motor_speed[5];
    217         brl_speed=motor_speed[6];
    218         brr_speed=motor_speed[7];
    219 
    220         omega=tfl_speed+brl_speed+trr_speed+bfr_speed-bfl_speed-trl_speed-brr_speed-tfr_speed;
    221 
    222 
    223     /*
    224         ** ===================================================================
    225         **    u roll: roll torque
    226         **
    227         ** ===================================================================
    228         */
    229 
    230     u_roll=arm_length->Value()*k_mot->Value()*(sigma->Value()*tfl_speed*tfl_speed+bfl_speed*bfl_speed
    231                                                 +sigma->Value()*trl_speed*trl_speed+brl_speed*brl_speed
    232                                                 -sigma->Value()*tfr_speed*tfr_speed-bfr_speed*bfr_speed
    233                                                 -sigma->Value()*trr_speed*trr_speed-brr_speed*brr_speed)*sqrtf(2)/2;
    234 
    235     /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed of the quadri in the body frame). It is a discrete integrator
    236         //state[0].W.x=(dT()/j_roll->Value())*((j_yaw->Value()-j_pitch->Value())*state[-1].W.y*state[-1].W.z-j_r->Value()*state[-1].W.y*omega + u_roll) +state[-1].W.x;//Osamah
    237     state[0].W.x=(dT()/j_roll->Value())*((j_pitch->Value()-j_yaw->Value())*state[-1].W.y*state[-1].W.z-j_r->Value()*state[-1].W.y*omega + u_roll) +state[-1].W.x;//Majd
    238 
    239         //state[0].W.x=(dT()/j_roll->Value())*(u_roll-m->Value()*G*l_cg->Value()*sinf(state[-2].W.x)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.x)+state[-1].W.x;
    240 
    241         /*
    242         ** ===================================================================
    243         **   u pitch : pitch torque
    244         **
    245         ** ===================================================================
    246         */
    247         u_pitch=arm_length->Value()*k_mot->Value()*(sigma->Value()*tfl_speed*tfl_speed+bfl_speed*bfl_speed
    248                                              +sigma->Value()*tfr_speed*tfr_speed+bfr_speed*bfr_speed
    249                                             -sigma->Value()*trl_speed*trl_speed-brl_speed*brl_speed
    250                                              -sigma->Value()*trr_speed*trr_speed-brr_speed*brr_speed)*sqrtf(2)/2;
    251 
    252     /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed of the quadri in the body frame). It is a discrete integrator
    253         //state[0].W.y=(dT()/j_pitch->Value())*((j_roll->Value()-j_yaw->Value())*state[-1].W.x*state[-1].W.z-j_r->Value()*state[-1].W.x*omega + u_pitch)+state[-1].W.y;//Osamah
    254     state[0].W.y=(dT()/j_pitch->Value())*((j_yaw->Value()-j_roll->Value())*state[-1].W.x*state[-1].W.z-j_r->Value()*state[-1].W.x*omega + u_pitch)+state[-1].W.y;//Majd
    255 
    256    //state[0].W.y=(dT()/j_pitch->Value())*(u_pitch-m->Value()*G*l_cg->Value()*sinf(state[-2].W.y)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.y)+state[-1].W.y;
    257 
    258         /*
    259         ** ===================================================================
    260         **    u yaw : yaw torque
    261         **
    262         ** ===================================================================
    263         */
    264         u_yaw=c_mot->Value()*(tfl_speed*tfl_speed-bfl_speed*bfl_speed
    265                         +trr_speed*trr_speed-brr_speed*brr_speed
    266                         -tfr_speed*tfr_speed+bfr_speed*bfr_speed
    267                         -trl_speed*trl_speed+brl_speed*brl_speed);
    268 
    269     /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed of the quadri in the body frame). It is a discrete integrator
    270         //state[0].W.z=(dT()/j_yaw->Value())* u_yaw +state[-1].W.z;//Osamah
    271         state[0].W.z=(dT()/j_yaw->Value())*((j_roll->Value()-j_pitch->Value())*state[-1].W.x*state[-1].W.y+u_yaw )+state[-1].W.z;//Majd
    272 
    273         //state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
    274 
    275     // compute quaternion from W
    276     // Quaternion derivative: dQ = 0.5*(Q*Qw)
    277     Quaternion dQ=state[-1].Quat.GetDerivative(state[0].W);
    278 
    279     // Quaternion integration
    280     state[0].Quat =state[-1].Quat +dQ*dT();
    281     state[0].Quat.Normalize();
    282 
    283     // Calculation of the thrust from the reference speed of motors
    284     u_thrust=k_mot->Value()*S->Value()*
    285        (sigma->Value()*tfl_speed*tfl_speed+sigma->Value()*tfr_speed*tfr_speed+sigma->Value()*trl_speed*trl_speed+sigma->Value()*trr_speed*trr_speed
    286        +bfl_speed*bfl_speed+bfr_speed*bfr_speed+brl_speed*brl_speed+brr_speed*brr_speed);
    287     Vector3D vect(0,0,-u_thrust);
    288     vect.Rotate(state[0].Quat);
    289 
    290     /*
    291         ** ===================================================================
    292         **     x double integrator
    293         **
    294         ** ===================================================================
    295         */
    296         state[0].Pos.x=(dT()*dT()/m->Value())*(vect.x-f_air_lat->Value()*(state[-1].Pos.x-state[-2].Pos.x)/dT())+2*state[-1].Pos.x-state[-2].Pos.x;
    297     state[0].Vel.x=(state[0].Pos.x-state[-1].Pos.x)/dT();
    298 
    299     /*
    300         ** ===================================================================
    301         **     y double integrator
    302         **
    303         ** ===================================================================
    304         */
    305         state[0].Pos.y=(dT()*dT()/m->Value())*(vect.y-f_air_lat->Value()*(state[-1].Pos.y-state[-2].Pos.y)/dT())+2*state[-1].Pos.y-state[-2].Pos.y;
    306     state[0].Vel.y=(state[0].Pos.y-state[-1].Pos.y)/dT();
    307 
    308         /*
    309         ** ===================================================================
    310         **     z double integrator
    311         **
    312         ** ===================================================================
    313         */
    314         state[0].Pos.z=(dT()*dT()/m->Value())*(vect.z+f_air_vert->Value()*(state[-1].Pos.z-state[-2].Pos.z)/dT()+m->Value()*G)+2*state[-1].Pos.z-state[-2].Pos.z;
    315     state[0].Vel.z=(state[0].Pos.z-state[-1].Pos.z)/dT();
     45namespace flair {
     46namespace simulator {
     47
     48X8::X8(const Simulator *parent, std::string name, int dev_id)
     49    : Model(parent, name) {
     50  Tab *setup_tab = new Tab(GetTabWidget(), "model");
     51  m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1);
     52  arm_length = new DoubleSpinBox(setup_tab->LastRowLastCol(), "arm length (m):",
     53                                 0, 2, 0.1);
     54  l_cg = new DoubleSpinBox(
     55      setup_tab->LastRowLastCol(), "position G (m):", -0.5, 0.5,
     56      0.02); // position du centre de gravité/centre de poussé
     57  k_mot =
     58      new DoubleSpinBox(setup_tab->NewRow(), "k_mot:", 0, 1, 0.001,
     59                        3); // vitesse rotation² (unité arbitraire) -> force (N)
     60  c_mot = new DoubleSpinBox(
     61      setup_tab->LastRowLastCol(), "c_mot:", 0, 1, 0.001,
     62      3); // vitesse rotation moteur -> couple (N.m/unité arbitraire)
     63  f_air_vert = new DoubleSpinBox(setup_tab->NewRow(), "f_air_vert:", 0, 10,
     64                                 1); // frottements air depl. vertical, aussi
     65                                     // utilisé pour les rotations ( N/(m/s) )
     66                                     // (du aux helices en rotation)
     67  f_air_lat =
     68      new DoubleSpinBox(setup_tab->LastRowLastCol(), "f_air_lat:", 0, 10,
     69                        1); // frottements air deplacements lateraux ( N/(m/s) )
     70  j_roll = new DoubleSpinBox(setup_tab->NewRow(), "j_roll:", 0, 1, 0.001,
     71                             5); // moment d'inertie d'un axe (N.m.s²/rad)
     72  j_pitch =
     73      new DoubleSpinBox(setup_tab->LastRowLastCol(), "j_pitch:", 0, 1, 0.001,
     74                        5); // moment d'inertie d'un axe (N.m.s²/rad)
     75  j_yaw = new DoubleSpinBox(setup_tab->LastRowLastCol(), "j_yaw:", 0, 1, 0.001,
     76                            5); // moment d'inertie d'un axe (N.m.s²/rad)
     77  j_r = new DoubleSpinBox(setup_tab->NewRow(), "j_r:", 0, 1,
     78                          0.001); // moment des helices (N.m.s²/rad)
     79  sigma = new DoubleSpinBox(
     80      setup_tab->LastRowLastCol(), "sigma:", 0, 1,
     81      0.1); // coefficient de perte d efficacite aerodynamique (sans unite)
     82  S = new DoubleSpinBox(
     83      setup_tab->LastRowLastCol(), "S:", 1, 2,
     84      0.1); // coefficient de forme des helices 1<S=1+Ss/Sprop<2 (sans unite)
     85
     86  motors = new SimuBldc(this, name, 8, dev_id);
     87}
     88
     89void X8::Draw() {
     90#ifdef GL
     91
     92  // create unite (1m=100cm) UAV; scale will be adapted according to arm_length
     93  // parameter
     94  // note that the frame used is irrlicht one:
     95  // left handed, North East Up
     96
     97  const IGeometryCreator *geo;
     98  geo = getGui()->getSceneManager()->getGeometryCreator();
     99
     100  // cylinders are aligned with y axis
     101  red_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 255, 0, 0));
     102  black_arm = geo->createCylinderMesh(2.5, 100, 16, SColor(0, 128, 128, 128));
     103  motor = geo->createCylinderMesh(7.5, 15, 16); //,SColor(0, 128, 128, 128));
     104  // geo->drop();
     105
     106  ITexture *texture = getGui()->getTexture("carbone.jpg");
     107  fl_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
     108                             vector3df(0, 0, -135));
     109  fr_arm = new MeshSceneNode(this, red_arm, vector3df(0, 0, 0),
     110                             vector3df(0, 0, -45));
     111  rl_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
     112                             vector3df(0, 0, 135), texture);
     113  rr_arm = new MeshSceneNode(this, black_arm, vector3df(0, 0, 0),
     114                             vector3df(0, 0, 45), texture);
     115
     116  texture = getGui()->getTexture("metal047.jpg");
     117  tfl_motor = new MeshSceneNode(this, motor, vector3df(70.71, -70.71, 2.5),
     118                                vector3df(90, 0, 0), texture);
     119  tfr_motor = new MeshSceneNode(this, motor, vector3df(70.71, 70.71, 2.5),
     120                                vector3df(90, 0, 0), texture);
     121  trl_motor = new MeshSceneNode(this, motor, vector3df(-70.71, -70.71, 2.5),
     122                                vector3df(90, 0, 0), texture);
     123  trr_motor = new MeshSceneNode(this, motor, vector3df(-70.71, 70.71, 2.5),
     124                                vector3df(90, 0, 0), texture);
     125
     126  bfl_motor = new MeshSceneNode(this, motor, vector3df(70.71, -70.71, -17.5),
     127                                vector3df(90, 0, 0), texture);
     128  bfr_motor = new MeshSceneNode(this, motor, vector3df(70.71, 70.71, -17.5),
     129                                vector3df(90, 0, 0), texture);
     130  brl_motor = new MeshSceneNode(this, motor, vector3df(-70.71, -70.71, -17.5),
     131                                vector3df(90, 0, 0), texture);
     132  brr_motor = new MeshSceneNode(this, motor, vector3df(-70.71, 70.71, -17.5),
     133                                vector3df(90, 0, 0), texture);
     134
     135  tfl_blade = new Blade(this, vector3df(70.71, -70.71, 17.5));
     136  tfr_blade = new Blade(this, vector3df(70.71, 70.71, 17.5), true);
     137  trl_blade = new Blade(this, vector3df(-70.71, -70.71, 17.5), true);
     138  trr_blade = new Blade(this, vector3df(-70.71, 70.71, 17.5));
     139
     140  bfl_blade = new Blade(this, vector3df(70.71, -70.71, -17.5));
     141  bfr_blade = new Blade(this, vector3df(70.71, 70.71, -17.5), true);
     142  brl_blade = new Blade(this, vector3df(-70.71, -70.71, -17.5), true);
     143  brr_blade = new Blade(this, vector3df(-70.71, 70.71, -17.5));
     144
     145  motor_speed_mutex = new Mutex(this);
     146  for (int i = 0; i < 8; i++)
     147    motor_speed[i] = 0;
     148  ExtraDraw();
     149#endif
     150}
     151
     152X8::~X8() {
     153  // les objets irrlicht seront automatiquement detruits (moteurs, helices,
     154  // pales) par parenté
     155}
     156
     157#ifdef GL
     158void X8::AnimateModel(void) {
     159  motor_speed_mutex->GetMutex();
     160  tfl_blade->SetRotationSpeed(K_MOT * motor_speed[0]);
     161  tfr_blade->SetRotationSpeed(-K_MOT * motor_speed[1]);
     162  trl_blade->SetRotationSpeed(-K_MOT * motor_speed[2]);
     163  trr_blade->SetRotationSpeed(K_MOT * motor_speed[3]);
     164
     165  bfl_blade->SetRotationSpeed(-K_MOT * motor_speed[4]);
     166  bfr_blade->SetRotationSpeed(K_MOT * motor_speed[5]);
     167  brl_blade->SetRotationSpeed(K_MOT * motor_speed[6]);
     168  brr_blade->SetRotationSpeed(-K_MOT * motor_speed[7]);
     169  motor_speed_mutex->ReleaseMutex();
     170
     171  // adapt UAV size
     172  if (arm_length->ValueChanged() == true) {
     173    setScale(arm_length->Value());
     174  }
     175}
     176
     177size_t X8::dbtSize(void) const {
     178  return 6 * sizeof(float) + 4 * sizeof(float); // 6ddl+4helices
     179}
     180
     181void X8::WritedbtBuf(
     182    char *dbtbuf) { /*
     183                       float *buf=(float*)dbtbuf;
     184                       vector3df vect=getPosition();
     185                       memcpy(buf,&vect.X,sizeof(float));
     186                       buf++;
     187                       memcpy(buf,&vect.Y,sizeof(float));
     188                       buf++;
     189                       memcpy(buf,&vect.Z,sizeof(float));
     190                       buf++;
     191                       vect=getRotation();
     192                       memcpy(buf,&vect.X,sizeof(float));
     193                       buf++;
     194                       memcpy(buf,&vect.Y,sizeof(float));
     195                       buf++;
     196                       memcpy(buf,&vect.Z,sizeof(float));
     197                       buf++;
     198                       memcpy(buf,&motors,sizeof(rtsimu_motors));*/
     199}
     200
     201void X8::ReaddbtBuf(
     202    char *dbtbuf) { /*
     203                       float *buf=(float*)dbtbuf;
     204                       vector3df vect;
     205                       memcpy(&vect.X,buf,sizeof(float));
     206                       buf++;
     207                       memcpy(&vect.Y,buf,sizeof(float));
     208                       buf++;
     209                       memcpy(&vect.Z,buf,sizeof(float));
     210                       buf++;
     211                       setPosition(vect);
     212                       memcpy(&vect.X,buf,sizeof(float));
     213                       buf++;
     214                       memcpy(&vect.Y,buf,sizeof(float));
     215                       buf++;
     216                       memcpy(&vect.Z,buf,sizeof(float));
     217                       buf++;
     218                       ((ISceneNode*)(this))->setRotation(vect);
     219                       memcpy(&motors,buf,sizeof(rtsimu_motors));
     220                       AnimateModele();*/
     221}
     222#endif // GL
     223
     224// states are computed on fixed frame NED
     225// x north
     226// y east
     227// z down
     228void X8::CalcModel(void) {
     229  float tfl_speed, tfr_speed, trl_speed, trr_speed;
     230  float bfl_speed, bfr_speed, brl_speed, brr_speed;
     231  float u_roll, u_pitch, u_yaw, u_thrust;
     232  float omega;
     233#ifdef GL
     234  motor_speed_mutex->GetMutex();
     235#endif // GL
     236  motors->GetSpeeds(motor_speed);
     237#ifdef GL
     238  motor_speed_mutex->ReleaseMutex();
     239#endif // GL
     240  tfl_speed = motor_speed[0];
     241  tfr_speed = motor_speed[1];
     242  trl_speed = motor_speed[2];
     243  trr_speed = motor_speed[3];
     244  bfl_speed = motor_speed[4];
     245  bfr_speed = motor_speed[5];
     246  brl_speed = motor_speed[6];
     247  brr_speed = motor_speed[7];
     248
     249  omega = tfl_speed + brl_speed + trr_speed + bfr_speed - bfl_speed -
     250          trl_speed - brr_speed - tfr_speed;
     251
     252  /*
     253      ** ===================================================================
     254      **    u roll: roll torque
     255      **
     256      ** ===================================================================
     257      */
     258
     259  u_roll = arm_length->Value() * k_mot->Value() *
     260           (sigma->Value() * tfl_speed * tfl_speed + bfl_speed * bfl_speed +
     261            sigma->Value() * trl_speed * trl_speed + brl_speed * brl_speed -
     262            sigma->Value() * tfr_speed * tfr_speed - bfr_speed * bfr_speed -
     263            sigma->Value() * trr_speed * trr_speed - brr_speed * brr_speed) *
     264           sqrtf(2) / 2;
     265
     266  /// Classical Nonlinear model of a quadrotor ( This is the w_x angular speed
     267  /// of the quadri in the body frame). It is a discrete integrator
     268  // state[0].W.x=(dT()/j_roll->Value())*((j_yaw->Value()-j_pitch->Value())*state[-1].W.y*state[-1].W.z-j_r->Value()*state[-1].W.y*omega
     269  // + u_roll) +state[-1].W.x;//Osamah
     270  state[0].W.x =
     271      (dT() / j_roll->Value()) *
     272          ((j_pitch->Value() - j_yaw->Value()) * state[-1].W.y * state[-1].W.z -
     273           j_r->Value() * state[-1].W.y * omega + u_roll) +
     274      state[-1].W.x; // Majd
     275
     276  // state[0].W.x=(dT()/j_roll->Value())*(u_roll-m->Value()*G*l_cg->Value()*sinf(state[-2].W.x)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.x)+state[-1].W.x;
     277
     278  /*
     279  ** ===================================================================
     280  **   u pitch : pitch torque
     281  **
     282  ** ===================================================================
     283  */
     284  u_pitch = arm_length->Value() * k_mot->Value() *
     285            (sigma->Value() * tfl_speed * tfl_speed + bfl_speed * bfl_speed +
     286             sigma->Value() * tfr_speed * tfr_speed + bfr_speed * bfr_speed -
     287             sigma->Value() * trl_speed * trl_speed - brl_speed * brl_speed -
     288             sigma->Value() * trr_speed * trr_speed - brr_speed * brr_speed) *
     289            sqrtf(2) / 2;
     290
     291  /// Classical Nonlinear model of a quadrotor ( This is the w_y angular speed
     292  /// of the quadri in the body frame). It is a discrete integrator
     293  // state[0].W.y=(dT()/j_pitch->Value())*((j_roll->Value()-j_yaw->Value())*state[-1].W.x*state[-1].W.z-j_r->Value()*state[-1].W.x*omega
     294  // + u_pitch)+state[-1].W.y;//Osamah
     295  state[0].W.y =
     296      (dT() / j_pitch->Value()) *
     297          ((j_yaw->Value() - j_roll->Value()) * state[-1].W.x * state[-1].W.z -
     298           j_r->Value() * state[-1].W.x * omega + u_pitch) +
     299      state[-1].W.y; // Majd
     300
     301  // state[0].W.y=(dT()/j_pitch->Value())*(u_pitch-m->Value()*G*l_cg->Value()*sinf(state[-2].W.y)-f_air_vert->Value()*arm_length->Value()*arm_length->Value()*state[-1].W.y)+state[-1].W.y;
     302
     303  /*
     304  ** ===================================================================
     305  **    u yaw : yaw torque
     306  **
     307  ** ===================================================================
     308  */
     309  u_yaw = c_mot->Value() * (tfl_speed * tfl_speed - bfl_speed * bfl_speed +
     310                            trr_speed * trr_speed - brr_speed * brr_speed -
     311                            tfr_speed * tfr_speed + bfr_speed * bfr_speed -
     312                            trl_speed * trl_speed + brl_speed * brl_speed);
     313
     314  /// Classical Nonlinear model of a quadrotor ( This is the w_z angular speed
     315  /// of the quadri in the body frame). It is a discrete integrator
     316  // state[0].W.z=(dT()/j_yaw->Value())* u_yaw +state[-1].W.z;//Osamah
     317  state[0].W.z =
     318      (dT() / j_yaw->Value()) * ((j_roll->Value() - j_pitch->Value()) *
     319                                     state[-1].W.x * state[-1].W.y +
     320                                 u_yaw) +
     321      state[-1].W.z; // Majd
     322
     323  // state[0].W.z=(dT()/j_yaw->Value())*(u_yaw-f_air_lat->Value()*state[-1].W.z)+state[-1].W.z;
     324
     325  // compute quaternion from W
     326  // Quaternion derivative: dQ = 0.5*(Q*Qw)
     327  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
     328
     329  // Quaternion integration
     330  state[0].Quat = state[-1].Quat + dQ * dT();
     331  state[0].Quat.Normalize();
     332
     333  // Calculation of the thrust from the reference speed of motors
     334  u_thrust =
     335      k_mot->Value() * S->Value() *
     336      (sigma->Value() * tfl_speed * tfl_speed +
     337       sigma->Value() * tfr_speed * tfr_speed +
     338       sigma->Value() * trl_speed * trl_speed +
     339       sigma->Value() * trr_speed * trr_speed + bfl_speed * bfl_speed +
     340       bfr_speed * bfr_speed + brl_speed * brl_speed + brr_speed * brr_speed);
     341  Vector3D vect(0, 0, -u_thrust);
     342  vect.Rotate(state[0].Quat);
     343
     344  /*
     345      ** ===================================================================
     346      **     x double integrator
     347      **
     348      ** ===================================================================
     349      */
     350  state[0].Pos.x =
     351      (dT() * dT() / m->Value()) *
     352          (vect.x -
     353           f_air_lat->Value() * (state[-1].Pos.x - state[-2].Pos.x) / dT()) +
     354      2 * state[-1].Pos.x - state[-2].Pos.x;
     355  state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
     356
     357  /*
     358      ** ===================================================================
     359      **     y double integrator
     360      **
     361      ** ===================================================================
     362      */
     363  state[0].Pos.y =
     364      (dT() * dT() / m->Value()) *
     365          (vect.y -
     366           f_air_lat->Value() * (state[-1].Pos.y - state[-2].Pos.y) / dT()) +
     367      2 * state[-1].Pos.y - state[-2].Pos.y;
     368  state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
     369
     370  /*
     371  ** ===================================================================
     372  **     z double integrator
     373  **
     374  ** ===================================================================
     375  */
     376  state[0].Pos.z =
     377      (dT() * dT() / m->Value()) *
     378          (vect.z +
     379           f_air_vert->Value() * (state[-1].Pos.z - state[-2].Pos.z) / dT() +
     380           m->Value() * G) +
     381      2 * state[-1].Pos.z - state[-2].Pos.z;
     382  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
    316383
    317384#ifndef GL
    318     if(state[0].Pos.z<0) state[0].Pos.z=0;
     385  if (state[0].Pos.z < 0)
     386    state[0].Pos.z = 0;
    319387#endif
    320 
    321388}
    322389

  • trunk/lib/FlairSimulator/src/X8.h

    r10 r15  
    2121#include <stdint.h>
    2222
    23 namespace flair
    24 {
    25     namespace core
    26     {
    27         class Mutex;
    28     }
    29     namespace gui
    30     {
    31         class DoubleSpinBox;
    32     }
    33     namespace actuator
    34     {
    35         class SimuBldc;
    36     }
     23namespace flair {
     24namespace core {
     25class Mutex;
     26}
     27namespace gui {
     28class DoubleSpinBox;
     29}
     30namespace actuator {
     31class SimuBldc;
     32}
    3733}
    3834
    3935#ifdef GL
    40 namespace irr
    41 {
    42     namespace scene
    43     {
    44         class IMesh;
    45     }
     36namespace irr {
     37namespace scene {
     38class IMesh;
     39}
    4640}
    4741#endif
    4842
    49 namespace flair
    50 {
    51 namespace simulator
    52 {
    53     class Simulator;
    54     class Blade;
    55     class MeshSceneNode;
     43namespace flair {
     44namespace simulator {
     45class Simulator;
     46class Blade;
     47class MeshSceneNode;
    5648
    57     class X8 : public Model
    58     {
    59         public:
    60             X8(const Simulator* parent,std::string name, int dev_id);
    61             ~X8();
    62             virtual void Draw(void);
    63                         virtual void ExtraDraw(void){};
    64         protected:
     49class X8 : public Model {
     50public:
     51  X8(const Simulator *parent, std::string name, int dev_id);
     52  ~X8();
     53  virtual void Draw(void);
     54  virtual void ExtraDraw(void){};
     55
     56protected:
    6557#ifdef GL
    66             irr::scene::IMesh *red_arm,*black_arm,*motor;
    67                         MeshSceneNode *fl_arm,*fr_arm,*rl_arm,*rr_arm;
    68                         MeshSceneNode *tfl_motor,*tfr_motor,*trl_motor,*trr_motor;
    69                         MeshSceneNode *bfl_motor,*bfr_motor,*brl_motor,*brr_motor;
    70                         Blade *tfl_blade,*tfr_blade,*trl_blade,*trr_blade;
    71                         Blade *bfl_blade,*bfr_blade,*brl_blade,*brr_blade;
     58  irr::scene::IMesh *red_arm, *black_arm, *motor;
     59  MeshSceneNode *fl_arm, *fr_arm, *rl_arm, *rr_arm;
     60  MeshSceneNode *tfl_motor, *tfr_motor, *trl_motor, *trr_motor;
     61  MeshSceneNode *bfl_motor, *bfr_motor, *brl_motor, *brr_motor;
     62  Blade *tfl_blade, *tfr_blade, *trl_blade, *trr_blade;
     63  Blade *bfl_blade, *bfr_blade, *brl_blade, *brr_blade;
    7264#endif
    73         private:
    74             void CalcModel(void);
    75     #ifdef GL
    76             bool OnEvent(const irr::SEvent& event){};
    77             void AnimateModel(void);
    78             size_t dbtSize(void) const;
    79             void WritedbtBuf(char* dbtbuf);
    80             void ReaddbtBuf(char* dbtbuf);
    81             core::Mutex *motor_speed_mutex;
    82     #endif
     65private:
     66  void CalcModel(void);
     67#ifdef GL
     68  bool OnEvent(const irr::SEvent &event){};
     69  void AnimateModel(void);
     70  size_t dbtSize(void) const;
     71  void WritedbtBuf(char *dbtbuf);
     72  void ReaddbtBuf(char *dbtbuf);
     73  core::Mutex *motor_speed_mutex;
     74#endif
    8375
    84             actuator::SimuBldc *motors;
    85             float motor_speed[8];
    86             gui::DoubleSpinBox *m,*arm_length,*l_cg;
    87             gui::DoubleSpinBox *k_mot,*c_mot;
    88             gui::DoubleSpinBox *f_air_vert,*f_air_lat;
    89             gui::DoubleSpinBox *j_roll,*j_pitch,*j_yaw;
    90             gui::DoubleSpinBox *j_r,*sigma,*S;
    91     };
     76  actuator::SimuBldc *motors;
     77  float motor_speed[8];
     78  gui::DoubleSpinBox *m, *arm_length, *l_cg;
     79  gui::DoubleSpinBox *k_mot, *c_mot;
     80  gui::DoubleSpinBox *f_air_vert, *f_air_lat;
     81  gui::DoubleSpinBox *j_roll, *j_pitch, *j_yaw;
     82  gui::DoubleSpinBox *j_r, *sigma, *S;
     83};
    9284} // end namespace simulator
    9385} // end namespace flair

  • trunk/lib/FlairSimulator/src/unexported/AnimPoursuite.h

    r10 r15  
    2222#include <vector3d.h>
    2323
    24 namespace flair
    25 {
    26 namespace simulator
    27 {
     24namespace flair {
     25namespace simulator {
    2826
    29     class AnimPoursuite : public irr::scene::ISceneNodeAnimator
    30     {
    31         public:
    32             AnimPoursuite(const irr::scene::ISceneNode* parent,float rotateSpeed = -500.0f, float zoomSpeed = 4.0f);
    33             ~AnimPoursuite();
     27class AnimPoursuite : public irr::scene::ISceneNodeAnimator {
     28public:
     29  AnimPoursuite(const irr::scene::ISceneNode *parent,
     30                float rotateSpeed = -500.0f, float zoomSpeed = 4.0f);
     31  ~AnimPoursuite();
    3432
    35             void animateNode(irr::scene::ISceneNode* node, irr::u32 timeMs);
    36             ISceneNodeAnimator* createClone(irr::scene::ISceneNode* node,irr::scene::ISceneManager* newManager=0);
    37             bool MouseMoved(const irr::SEvent& event,irr::core::position2df MousePos);
    38             void setPositionOffset(irr::core::vector3df newpos);
    39             void setTargetOffset(irr::core::vector3df newpos);
     33  void animateNode(irr::scene::ISceneNode *node, irr::u32 timeMs);
     34  ISceneNodeAnimator *createClone(irr::scene::ISceneNode *node,
     35                                  irr::scene::ISceneManager *newManager = 0);
     36  bool MouseMoved(const irr::SEvent &event, irr::core::position2df MousePos);
     37  void setPositionOffset(irr::core::vector3df newpos);
     38  void setTargetOffset(irr::core::vector3df newpos);
    4039
    41         private:
    42             virtual bool isEventReceiverEnabled(void) const
    43             {
    44                 return true;
    45             }
     40private:
     41  virtual bool isEventReceiverEnabled(void) const { return true; }
    4642
    47             irr::core::vector3df pos_offset,target_offset;
    48             bool LMouseKey;
    49             irr::core::position2df RotateStart;
    50             irr::core::position2df MousePos;
    51             const irr::scene::ISceneNode* parent;
    52             bool Rotating;
    53             float RotY,RotZ;
    54             float rotateSpeed;
    55             float zoomSpeed;
    56             float currentZoom;
    57             float sat(float value);
    58     };
     43  irr::core::vector3df pos_offset, target_offset;
     44  bool LMouseKey;
     45  irr::core::position2df RotateStart;
     46  irr::core::position2df MousePos;
     47  const irr::scene::ISceneNode *parent;
     48  bool Rotating;
     49  float RotY, RotZ;
     50  float rotateSpeed;
     51  float zoomSpeed;
     52  float currentZoom;
     53  float sat(float value);
     54};
    5955
    6056} // end namespace simulator

  • trunk/lib/FlairSimulator/src/unexported/GenericObject.h

    r10 r15  
    3535#include <Vector3D.h>
    3636
    37 
    3837#include <aabbox3d.h>
    3938#include <IMeshSceneNode.h>
    4039
    41 namespace irr
    42 {
    43     class SEvent;
    44     namespace scene
    45     {
    46         class IMesh;
    47         class ISceneManager;
    48         class ITriangleSelector;
    49         class IMetaTriangleSelector;
    50         class ISceneNodeAnimatorCollisionResponse;
    51     }
     40namespace irr {
     41class SEvent;
     42namespace scene {
     43class IMesh;
     44class ISceneManager;
     45class ITriangleSelector;
     46class IMetaTriangleSelector;
     47class ISceneNodeAnimatorCollisionResponse;
     48}
    5249}
    5350
    … …  
    5552class Simulator_impl;
    5653
    57 namespace flair
    58 {
    59 namespace core
    60 {
     54namespace flair {
     55namespace core {
    6156class ConditionVariable;
    6257}
    63 namespace simulator
    64 {
     58namespace simulator {
    6559class Simulator;
    6660class AnimPoursuite;
    6761
    68 class GenericObject : public irr::scene::IMeshSceneNode
    69 {
    70     friend class ::Simulator_impl;
     62class GenericObject : public irr::scene::IMeshSceneNode {
     63  friend class ::Simulator_impl;
    7164
    7265public:
    73     GenericObject(Simulator* parent,std::string name, irr::scene::ISceneManager* sceneManager);
    74     virtual ~GenericObject();
     66  GenericObject(Simulator *parent, std::string name,
     67                irr::scene::ISceneManager *sceneManager);
     68  virtual ~GenericObject();
    7569
    76     //FROM IMPL
    77     irr::scene::ITriangleSelector* TriangleSelector(void);
    78     irr::core::aabbox3d<irr::f32> box;
    79     //END FROM IMPL
     70  // FROM IMPL
     71  irr::scene::ITriangleSelector *TriangleSelector(void);
     72  irr::core::aabbox3d<irr::f32> box;
     73  // END FROM IMPL
    8074
    81     irr::scene::ISceneNode* getSceneNode();
    82     virtual const irr::core::aabbox3d<irr::f32>& getBoundingBox(void) const
    83     {
    84         return box;
    85     }
     75  irr::scene::ISceneNode *getSceneNode();
     76  virtual const irr::core::aabbox3d<irr::f32> &getBoundingBox(void) const {
     77    return box;
     78  }
    8679
    87     void setPosition(irr::core::vector3df position);
    88     void setScale(float value);
    89     void setScale(irr::core::vector3df scale);
    90     void setRotation(irr::core::vector3df rotation);
    91     void OnRegisterSceneNode(void);
    92     void setMesh(irr::scene::IMesh* mesh);
    93     irr::scene::IMesh* getMesh(void);
    94     void render(void);
    95     virtual void setReadOnlyMaterials(bool readonly){};
    96     virtual bool isReadOnlyMaterials(void) const
    97     {
    98         return false;
    99     }
    100     virtual irr::scene::IShadowVolumeSceneNode* addShadowVolumeSceneNode(const irr::scene::IMesh* shadowMesh=0,
    101             irr::s32 id=-1, bool zfailmethod=true, irr::f32 infinity=1000.0f)
    102     {
    103         return NULL;
    104     }
     80  void setPosition(irr::core::vector3df position);
     81  void setScale(float value);
     82  void setScale(irr::core::vector3df scale);
     83  void setRotation(irr::core::vector3df rotation);
     84  void OnRegisterSceneNode(void);
     85  void setMesh(irr::scene::IMesh *mesh);
     86  irr::scene::IMesh *getMesh(void);
     87  void render(void);
     88  virtual void setReadOnlyMaterials(bool readonly){};
     89  virtual bool isReadOnlyMaterials(void) const { return false; }
     90  virtual irr::scene::IShadowVolumeSceneNode *
     91  addShadowVolumeSceneNode(const irr::scene::IMesh *shadowMesh = 0,
     92                           irr::s32 id = -1, bool zfailmethod = true,
     93                           irr::f32 infinity = 1000.0f) {
     94    return NULL;
     95  }
    10596
    10697private:
    107     void UpdateFrom(core::io_data *data) {};
    108     irr::scene::IMesh *mesh;
    109     irr::scene::ITriangleSelector* selector;
    110     flair::core::ConditionVariable* cond;
    111     irr::video::SMaterial Material;
    112 
     98  void UpdateFrom(core::io_data *data){};
     99  irr::scene::IMesh *mesh;
     100  irr::scene::ITriangleSelector *selector;
     101  flair::core::ConditionVariable *cond;
     102  irr::video::SMaterial Material;
    113103};
    114104} // end namespace simulator

  • trunk/lib/FlairSimulator/src/unexported/Gui_impl.h

    r10 r15  
    2424#include <vector3d.h>
    2525
    26 namespace irr
    27 {
    28     class IrrlichtDevice;
    29     namespace scene
    30     {
    31         class ISceneManager;
    32         class IMeshSceneNode;
    33         class ITriangleSelector;
    34     }
    35     namespace video
    36     {
    37         class IVideoDriver;
    38     }
    39     namespace gui
    40     {
    41         class IGUIFont;
    42         class IGUIEnvironment;
    43     }
     26namespace irr {
     27class IrrlichtDevice;
     28namespace scene {
     29class ISceneManager;
     30class IMeshSceneNode;
     31class ITriangleSelector;
     32}
     33namespace video {
     34class IVideoDriver;
     35}
     36namespace gui {
     37class IGUIFont;
     38class IGUIEnvironment;
     39}
    4440}
    4541
    46 namespace flair
    47 {
    48     namespace core
    49     {
    50         class Object;
    51     }
    52     namespace simulator
    53     {
    54         class Model;
    55         class GenericObject;
    56         class Gui;
    57     }
     42namespace flair {
     43namespace core {
     44class Object;
     45}
     46namespace simulator {
     47class Model;
     48class GenericObject;
     49class Gui;
     50}
    5851}
    5952
    6053class MyEventReceiver;
    6154
    62 class Gui_impl
    63 {
    64     public:
    65         Gui_impl(flair::simulator::Gui* self,int app_width, int app_height,int scene_width, int scene_height,std::string media_path,irr::video::E_DRIVER_TYPE driver_type=irr::video::EDT_OPENGL);
    66         ~Gui_impl();
    67         void RunGui(std::vector<flair::simulator::Model*> modeles,std::vector<flair::simulator::GenericObject*> objects);
    68         void setMesh(std::string file,irr::core::vector3df position = irr::core::vector3df(0,0,0),irr::core::vector3df rotation= irr::core::vector3df(0,0,0),irr::core::vector3df scale= irr::core::vector3df(1,1,1));
     55class Gui_impl {
     56public:
     57  Gui_impl(flair::simulator::Gui *self, int app_width, int app_height,
     58           int scene_width, int scene_height, std::string media_path,
     59           irr::video::E_DRIVER_TYPE driver_type = irr::video::EDT_OPENGL);
     60  ~Gui_impl();
     61  void RunGui(std::vector<flair::simulator::Model *> modeles,
     62              std::vector<flair::simulator::GenericObject *> objects);
     63  void setMesh(std::string file,
     64               irr::core::vector3df position = irr::core::vector3df(0, 0, 0),
     65               irr::core::vector3df rotation = irr::core::vector3df(0, 0, 0),
     66               irr::core::vector3df scale = irr::core::vector3df(1, 1, 1));
    6967
    70         irr::scene::IMeshSceneNode* node;
    71         irr::IrrlichtDevice *device;
    72         std::string media_path;
    73         irr::video::IVideoDriver* driver;
    74         irr::scene::ISceneManager* smgr;
    75         int scene_width,scene_height;
     68  irr::scene::IMeshSceneNode *node;
     69  irr::IrrlichtDevice *device;
     70  std::string media_path;
     71  irr::video::IVideoDriver *driver;
     72  irr::scene::ISceneManager *smgr;
     73  int scene_width, scene_height;
    7674
    77     private:
    78         MyEventReceiver *receiver;
    79         irr::scene::ITriangleSelector* selector;
    80         irr::gui::IGUIFont* font;
    81         irr::gui::IGUIEnvironment* env;
    82         void setWindowCaption(flair::core::Object* object, int fps);
    83         void takeScreenshot(void);
    84         hdfile_t *dbtFile_r,*dbtFile_w;
    85         size_t dbtSize(std::vector<flair::simulator::Model*> modeles);
    86         char* dbtbuf;
    87         flair::simulator::Gui *self;
     75private:
     76  MyEventReceiver *receiver;
     77  irr::scene::ITriangleSelector *selector;
     78  irr::gui::IGUIFont *font;
     79  irr::gui::IGUIEnvironment *env;
     80  void setWindowCaption(flair::core::Object *object, int fps);
     81  void takeScreenshot(void);
     82  hdfile_t *dbtFile_r, *dbtFile_w;
     83  size_t dbtSize(std::vector<flair::simulator::Model *> modeles);
     84  char *dbtbuf;
     85  flair::simulator::Gui *self;
    8886};
    8987

  • trunk/lib/FlairSimulator/src/unexported/Model_impl.h

    r10 r15  
    2323#include "Quaternion.h"
    2424
    25 namespace flair
    26 {
    27     namespace core
    28     {
    29         class cvmatrix;
    30         class Mutex;
    31         class ConditionVariable;
    32     }
    33     namespace gui
    34     {
    35         class TabWidget;
    36         class CheckBox;
    37         class DoubleSpinBox;
    38         class SpinBox;
    39         class Vector3DSpinBox;
    40     }
    41     namespace simulator
    42     {
    43         class Simulator;
    44         class AnimPoursuite;
    45     }
     25namespace flair {
     26namespace core {
     27class cvmatrix;
     28class Mutex;
     29class ConditionVariable;
     30}
     31namespace gui {
     32class TabWidget;
     33class CheckBox;
     34class DoubleSpinBox;
     35class SpinBox;
     36class Vector3DSpinBox;
     37}
     38namespace simulator {
     39class Simulator;
     40class AnimPoursuite;
     41}
    4642}
    4743
    … …  
    5046#include <IMeshSceneNode.h>
    5147
    52 namespace irr
    53 {
    54     namespace scene
    55     {
    56         class ISceneManager;
    57         class ITriangleSelector;
    58         class IMetaTriangleSelector;
    59         class ISceneNodeAnimatorCollisionResponse;
    60         class ICameraSceneNode;
    61     }
     48namespace irr {
     49namespace scene {
     50class ISceneManager;
     51class ITriangleSelector;
     52class IMetaTriangleSelector;
     53class ISceneNodeAnimatorCollisionResponse;
     54class ICameraSceneNode;
     55}
    6256}
    6357#endif
    6458
    6559#ifdef GL
    66 class Model_impl : public irr::scene::ISceneNode,public flair::core::Thread,private vrpn_Tracker
     60class Model_impl : public irr::scene::ISceneNode,
     61                   public flair::core::Thread,
     62                   private vrpn_Tracker
    6763#else
    68 class Model_impl : public flair::core::Thread, private vrpn_Tracker
     64class Model_impl : public flair::core::Thread,
     65                   private vrpn_Tracker
    6966#endif
    70 {
    71     public:
     67                   {
     68public:
    7269#ifdef GL
    73         Model_impl(flair::simulator::Model* self,std::string name,irr::scene::ISceneManager* scenemanager,vrpn_Connection_IP* vrpn);
     70  Model_impl(flair::simulator::Model *self, std::string name,
     71             irr::scene::ISceneManager *scenemanager, vrpn_Connection_IP *vrpn);
    7472#else
    75         Model_impl(flair::simulator::Model* self,std::string name,vrpn_Connection_IP* vrpn);
     73  Model_impl(flair::simulator::Model *self, std::string name,
     74             vrpn_Connection_IP *vrpn);
    7675#endif
    77         ~Model_impl();
     76  ~Model_impl();
    7877
    7978#ifdef GL
    80         void OnRegisterSceneNode(void);
    81         void render(void);
    82         void Draw(void){printf("CA MARCHE PAS PUNAISE\r\n");ExtraDraw();};
    83         void ExtraDraw(void){printf("nope\r\n");};
     79  void OnRegisterSceneNode(void);
     80  void render(void);
     81  void Draw(void) {
     82    printf("CA MARCHE PAS PUNAISE\r\n");
     83    ExtraDraw();
     84  };
     85  void ExtraDraw(void) { printf("nope\r\n"); };
    8486
    85         const irr::core::aabbox3d<irr::f32>& getBoundingBox(void) const
    86         {
    87             return box;
    88         }
    89         void UpdatePos(void);
    90         void CheckCollision(void);
    91         irr::scene::ITriangleSelector* TriangleSelector(void);
    92         irr::scene::IMetaTriangleSelector* MetaTriangleSelector(void);
    93         irr::core::aabbox3d<irr::f32> box;
    94         void SynchronizationPoint();
    95         irr::scene::ICameraSceneNode* camera;
    96         flair::simulator::AnimPoursuite* animator;
    97         irr::scene::ITriangleSelector* selector;
     87  const irr::core::aabbox3d<irr::f32> &getBoundingBox(void) const {
     88    return box;
     89  }
     90  void UpdatePos(void);
     91  void CheckCollision(void);
     92  irr::scene::ITriangleSelector *TriangleSelector(void);
     93  irr::scene::IMetaTriangleSelector *MetaTriangleSelector(void);
     94  irr::core::aabbox3d<irr::f32> box;
     95  void SynchronizationPoint();
     96  irr::scene::ICameraSceneNode *camera;
     97  flair::simulator::AnimPoursuite *animator;
     98  irr::scene::ITriangleSelector *selector;
    9899#endif
    99         void mainloop(void);
     100  void mainloop(void);
    100101
    101         flair::gui::TabWidget *tabwidget;
    102         flair::gui::DoubleSpinBox *dT;
     102  flair::gui::TabWidget *tabwidget;
     103  flair::gui::DoubleSpinBox *dT;
    103104
    104     private:
    105         flair::gui::Vector3DSpinBox* pos_init;
    106         flair::gui::SpinBox *yaw_init;
    107         flair::gui::CheckBox *enable_opti;
    108         flair::simulator::Model* self;
    109         flair::core::cvmatrix* output;
    110         flair::core::Mutex *states_mutex;
     105private:
     106  flair::gui::Vector3DSpinBox *pos_init;
     107  flair::gui::SpinBox *yaw_init;
     108  flair::gui::CheckBox *enable_opti;
     109  flair::simulator::Model *self;
     110  flair::core::cvmatrix *output;
     111  flair::core::Mutex *states_mutex;
    111112
    112         struct timeval _timestamp;
    113         void Run(void);
    114         flair::core::Quaternion ComputeInitRotation(flair::core::Quaternion quat_in);
     113  struct timeval _timestamp;
     114  void Run(void);
     115  flair::core::Quaternion ComputeInitRotation(flair::core::Quaternion quat_in);
    115116#ifdef GL
    116         void CollisionHandler(void);
     117  void CollisionHandler(void);
    117118
    118         irr::scene::IMetaTriangleSelector* meta_selector;
    119         irr::scene::ISceneNodeAnimatorCollisionResponse* anim;
     119  irr::scene::IMetaTriangleSelector *meta_selector;
     120  irr::scene::ISceneNodeAnimatorCollisionResponse *anim;
    120121
    121         bool position_init;
     122  bool position_init;
    122123
    123         flair::core::ConditionVariable* cond;
    124         int sync_count;
     124  flair::core::ConditionVariable *cond;
     125  int sync_count;
    125126
    126         flair::core::Mutex *collision_mutex;
    127         bool collision_occured;
    128         flair::core::Vector3D collision_point;
     127  flair::core::Mutex *collision_mutex;
     128  bool collision_occured;
     129  flair::core::Vector3D collision_point;
    129130#endif
    130 
    131131};
    132132

  • trunk/lib/FlairSimulator/src/unexported/Simulator_impl.h

    r10 r15  
    2121#include <Thread.h>
    2222
    23 
    24 namespace flair
    25 {
    26     namespace simulator
    27     {
    28         class Simulator;
    29         class Model;
    30         class GenericObject;
    31     }
     23namespace flair {
     24namespace simulator {
     25class Simulator;
     26class Model;
     27class GenericObject;
     28}
    3229}
    3330
    34 class Simulator_impl: public vrpn_Connection_IP, private flair::core::Thread
    35 {
    36     friend class flair::simulator::Model;
    37     friend class flair::simulator::GenericObject;
     31class Simulator_impl : public vrpn_Connection_IP, private flair::core::Thread {
     32  friend class flair::simulator::Model;
     33  friend class flair::simulator::GenericObject;
    3834
    39     public:
    40         Simulator_impl(flair::simulator::Simulator* self,int optitrack_mstime=10,float yaw_deg=30);
    41         ~Simulator_impl();
     35public:
     36  Simulator_impl(flair::simulator::Simulator *self, int optitrack_mstime = 10,
     37                 float yaw_deg = 30);
     38  ~Simulator_impl();
    4239
    43         void RunSimu(void);
    44         float yaw_rad;
     40  void RunSimu(void);
     41  float yaw_rad;
    4542
    46     private:
    47         void Run(void);
    48         flair::simulator::Simulator* self;
    49         std::vector<flair::simulator::Model*> models;
    50         std::vector<flair::simulator::GenericObject*> objects;
    51         int optitrack_mstime;
     43private:
     44  void Run(void);
     45  flair::simulator::Simulator *self;
     46  std::vector<flair::simulator::Model *> models;
     47  std::vector<flair::simulator::GenericObject *> objects;
     48  int optitrack_mstime;
    5249};
    5350
Note: See TracChangeset for help on using the changeset viewer.