Index: trunk/lib/FlairSimulator/src/Model_impl.cpp
===================================================================
--- trunk/lib/FlairSimulator/src/Model_impl.cpp	(revision 372)
+++ trunk/lib/FlairSimulator/src/Model_impl.cpp	(revision 375)
@@ -229,6 +229,7 @@
   // TODO: setEllipsoidRadius should be called in Model::setScale
   // but we need to call recalculateBoundingBox
+  
   anim->setEllipsoidRadius(getTransformedBoundingBox().getExtent());
-
+ 
   if (anim->collisionOccurred() == true) {
     vector3df pos;
@@ -238,7 +239,7 @@
     nodePosition = getPosition();
     pos_rel = pos - nodePosition;
-    // printf("collision %f %f %f\n",pos.X,pos.Y,pos.Z);
-    // printf("object %f %f %f\n",nodePosition.X,nodePosition.Y,nodePosition.Z);
-    // printf("rel %f %f %f\n",pos_rel.X,pos_rel.Z,pos_rel.Y);
+     //printf("collision %f %f %f\n",pos.X,pos.Y,pos.Z);
+     //printf("object %f %f %f\n",nodePosition.X,nodePosition.Y,nodePosition.Z);
+     //printf("rel %f %f %f\n",pos_rel.X,pos_rel.Z,pos_rel.Y);
 
     collision_mutex->GetMutex();
Index: trunk/lib/FlairSimulator/src/SimuUgvControls.cpp
===================================================================
--- trunk/lib/FlairSimulator/src/SimuUgvControls.cpp	(revision 375)
+++ trunk/lib/FlairSimulator/src/SimuUgvControls.cpp	(revision 375)
@@ -0,0 +1,73 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+//  created:    2020/12/07
+//  filename:   SimuUgvControls.cpp
+//
+//  author:     Guillaume Sanahuja
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//  version:    $Id: $
+//
+//  purpose:    Class for a simulation ugv
+//
+//
+/*********************************************************************/
+#include "SimuUgvControls.h"
+#include <SharedMem.h>
+#include <sstream>
+#include <string.h>
+
+using std::string;
+using std::ostringstream;
+using namespace flair::core;
+
+namespace flair {
+namespace actuator {
+
+
+SimuUgvControls::SimuUgvControls(const Object *parent, string name,
+                   uint32_t modelId,uint32_t deviceId)
+    : IODevice(parent, name) {
+  shmem =new SharedMem(this, ShMemName(modelId, deviceId), 2 * sizeof(float)+sizeof(Time));
+      
+  buf=(char*)malloc(2 * sizeof(float)+sizeof(Time));
+  if(buf==NULL) {
+    Err("error creating buffer\n");
+    return;
+  } 
+  
+  // reset values
+  float *values=(float*)buf;
+  for (int i = 0; i < 2; i++) values[i] = 0;
+  Time time=GetTime();
+  memcpy(buf+2 * sizeof(float),&time,sizeof(Time));
+
+  shmem->Write(buf, 2 * sizeof(float)+sizeof(Time));
+  
+  SetIsReady(true);
+}
+
+SimuUgvControls::~SimuUgvControls() {
+  if(buf!=NULL) free(buf);
+}
+
+string SimuUgvControls::ShMemName(uint32_t modelId,uint32_t deviceId) {
+  ostringstream dev_name;
+  dev_name << "simu" <<  modelId << "_ugvcontrols_" << deviceId;
+  return dev_name.str().c_str();
+}
+
+
+void SimuUgvControls::GetControls(float *speed,float *turn,Time* time) const {
+  float *values=(float*)buf;
+  shmem->Read(buf, 2 * sizeof(float)+sizeof(Time));
+  memcpy(time,buf+2 * sizeof(float),sizeof(Time));
+
+  *speed = values[0];
+  *turn = values[1];
+}
+
+} // end namespace actuator
+} // end namespace flair
Index: trunk/lib/FlairSimulator/src/SimuUgvControls.h
===================================================================
--- trunk/lib/FlairSimulator/src/SimuUgvControls.h	(revision 375)
+++ trunk/lib/FlairSimulator/src/SimuUgvControls.h	(revision 375)
@@ -0,0 +1,77 @@
+// %flair:license{
+// This file is part of the Flair framework distributed under the
+// CECILL-C License, Version 1.0.
+// %flair:license}
+/*!
+ * \file SimuUgvControls.h
+ * \brief Class for a simulation ugv
+ * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
+ * \date 2020/12/07
+ * \version 4.0
+ */
+
+#ifndef SIMUUGVCONTROLS_H
+#define SIMUUGVCONTROLS_H
+
+#include <IODevice.h>
+
+namespace flair {
+namespace core {
+class SharedMem;
+}
+}
+
+namespace flair {
+namespace actuator {
+/*! \class SimuUgvControls
+*
+* \brief Class for a simulation ugv
+*
+*/
+class SimuUgvControls : public core::IODevice {
+public:
+  /*!
+  * \brief Constructor
+  *
+  * Construct a SimuUgvControls
+  *
+  * \param parent parent
+  * \param name name
+  * \param modelId Model id
+  * \param deviceId UgvControls id of the Model
+  */
+  SimuUgvControls(const core::Object *parent, std::string name,
+           uint32_t modelId,uint32_t deviceId);
+
+  /*!
+  * \brief Destructor
+  *
+  */
+  ~SimuUgvControls();
+
+  /*!
+  * \brief Get motors speeds.
+  *
+  *
+  * \param value array to store motors speeds
+  * \param time time when shared memory was written
+  */
+  void GetControls(float *speed,float *turn,core::Time* time) const;
+
+
+private:
+  /*!
+  * \brief Update using provided datas
+  *
+  * Reimplemented from IODevice.
+  *
+  * \param data data from the parent to process
+  */
+  void UpdateFrom(const core::io_data *data){};
+  std::string ShMemName(uint32_t modelId,uint32_t deviceId);
+  core::SharedMem *shmem;
+  char *buf;
+};
+} // end namespace actuator
+} // end namespace flair
+#endif // SIMUUGVCONTROLS_H
Index: trunk/lib/FlairSimulator/src/TwoWheelRobot.cpp
===================================================================
--- trunk/lib/FlairSimulator/src/TwoWheelRobot.cpp	(revision 372)
+++ trunk/lib/FlairSimulator/src/TwoWheelRobot.cpp	(revision 375)
@@ -22,4 +22,5 @@
 #include <GroupBox.h>
 #include <math.h>
+#include <SimuUgvControls.h>
 #ifdef GL
 #include <ISceneManager.h>
@@ -56,10 +57,11 @@
   bodyColorB = new SpinBox(visual_tab->LastRowLastCol(), "arm color (B):", 0, 255, 1,0);
   
+  controls = new SimuUgvControls(this, name, modelId,0);
+  
   SetIsReady(true);
 }
 
 TwoWheelRobot::~TwoWheelRobot() {
-  // les objets irrlicht seront automatiquement detruits (moteurs, helices,
-  // pales) par parenté
+  // les objets irrlicht seront automatiquement detruits par parenté
 }
 
@@ -75,4 +77,5 @@
 
   colored_body = geo->createCubeMesh(vector3df(100,100,100));
+  colored_body->setBoundingBox(aabbox3df(0,0,0,1,1,1));//bug with bounding box? workaround is to reduce it... we use only wheel box
   MeshSceneNode* mesh= new MeshSceneNode(this, colored_body);
   
@@ -80,5 +83,6 @@
   MeshSceneNode *l_wheel = new MeshSceneNode(this, wheel, vector3df(0, 50, -30),vector3df(0, 0, 0));
   MeshSceneNode *r_wheel = new MeshSceneNode(this, wheel, vector3df(0, -50-10, -30),vector3df(0, 0, 0));
-  ExtraDraw();
+  
+ ExtraDraw();
 }
 
@@ -147,10 +151,25 @@
 // z down
 void TwoWheelRobot::CalcModel(void) {
-  state[0].Pos.x=state[-1].Pos.x;
-  state[0].Pos.y=state[-1].Pos.y;
-  state[0].Vel.x = (state[0].Pos.x - state[-1].Pos.x) / dT();
-  state[0].Vel.y = (state[0].Pos.y - state[-1].Pos.y) / dT();
+  float speed=0.4, turn=0;
+  Time motorTime;
+
+  controls->GetControls(&speed,&turn,&motorTime);
+	  
+  // compute quaternion from W
+  // Quaternion derivative: dQ = 0.5*(  Q*Qw)
+  state[0].W.x=0;
+  state[0].W.y=0;
+  state[0].W.z=turn;
+  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
+
+  // Quaternion integration
+  state[0].Quat = state[-1].Quat + dQ * dT();
+  state[0].Quat.Normalize();
+
+  Vector3D<double> dir = Vector3D<double>(speed,0,0);
+  dir.Rotate(state[0].Quat);
+  state[0].Pos = state[-1].Pos + dT() * dir;
+  
  
-  //Printf("%f %f %f\n",state[0].Pos.x,state[-1].Pos.x,state[-2].Pos.x);
   /*
   ** ===================================================================
@@ -159,12 +178,5 @@
   ** ===================================================================
   */
-  state[0].Pos.z =
-      (dT() * dT() / m->Value()) * ( m->Value() * G) + 2 * state[-1].Pos.z - state[-2].Pos.z;
-  state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
-//Printf("%f %f %f\n",state[0].Pos.z,state[-1].Pos.z,state[-2].Pos.z);
-#ifndef GL
-  if (state[0].Pos.z < 0)
-    state[0].Pos.z = 0;
-#endif
+  state[0].Pos.z = (dT() * dT() / m->Value()) * ( m->Value() * G) + 2 * state[-1].Pos.z - state[-2].Pos.z;
   state[0].Vel.z = (state[0].Pos.z - state[-1].Pos.z) / dT();
 }
Index: trunk/lib/FlairSimulator/src/TwoWheelRobot.h
===================================================================
--- trunk/lib/FlairSimulator/src/TwoWheelRobot.h	(revision 372)
+++ trunk/lib/FlairSimulator/src/TwoWheelRobot.h	(revision 375)
@@ -29,5 +29,5 @@
 }
 namespace actuator {
-class SimuBldc;
+class SimuUgvControls;
 }
 }
@@ -65,4 +65,5 @@
   gui::SpinBox *bodyColorR,*bodyColorG,*bodyColorB;
   gui::DoubleSpinBox *size,*m;
+  actuator::SimuUgvControls *controls;
 
  
