Modifica README

CMakeLists.txt

@@ -53,7 +53,7 @@ target_compile_options(common INTERFACE
 
 set(METHODS_PATH "./src/*/methods/*.cpp")
 
-set(VERSION "V6")
+set(VERSION "V9")
 
 file(GLOB_RECURSE METHODS_SRC "${METHODS_PATH}")
 add_executable(main${VERSION} ./src/testMain.cpp ${METHODS_SRC} )

README.md

@@ -43,10 +43,25 @@ Per spostare l'intera scena si tiene premuto il tasto centrale del mouse.
 - Modifica di sfml_util per gestire le collezioni
 - Aggiunta modalità debug  
 
+## Nella versione v0.7
+- Aggiunta impostazione di trasparenza dei pezzi
+- Aggiustato cambio direzione della gamba (sulla visualizzazione dei piani XZ e -XZ)
+- Aggiunto controllo trasparenza delle collezioni
+- Modificato lower_body per gestire la trasparenza della gamba più lontana
+- Ridimensionato bacino per migiore visualizzazione
+
+## Nella versione v0.8
+- Aggiunta oscillazione bacino 
+- Aggiustato calcolo posizione con clock dedicato
+
+## Nella versione v0.9
+- Modificata la funzione update di pezzi e collezioni per implementare controllo sul tempo
+- Aggiunta finestra con slider per selezione moltiplicatore del tempo
+
 # Per compilare:
 
     cmake --build
 
 # Per lanciare:
 
-    ./build/bin/mainV6
+    ./build/bin/mainV9

build/bin/imgui.ini

@@ -3,7 +3,7 @@ Pos=60,60
 Size=400,400
 
 [Window][Dear ImGui Demo]
-Pos=781,47
+Pos=487,44
 Size=455,873
 
 [Window][Hello, world!]
@@ -42,3 +42,7 @@ Size=353,1005
 Pos=400,0
 Size=400,30
 
+[Window][Set time multiplier]
+Pos=400,640
+Size=400,30
+

src/collections/headers/collection_interface.hpp

@@ -13,6 +13,8 @@ struct collection{
 class CollectionInterface{
     public:
         virtual collection create(ReferencePlane plane) = 0;
+        virtual void update(sf::Clock cl, float multiplier) = 0;
+        virtual bool setTransparency(float alpha) = 0; 
         virtual ~CollectionInterface(){};
 };
 

src/collections/headers/gamba.hpp

@@ -6,13 +6,17 @@
 
 class Gamba : public CollectionInterface {
     protected:
-        std::vector<PieceInterface*> sensori;
+        std::vector<Sensore*> sensori;
         std::vector<PieceInterface*> pezzi;
         std::vector<JointInterface*> joints;
     public:
         Gamba(rb::Vector3 pos, unsigned int* dataPos, std::string cosciaData, std::string cavigliaData);
         collection create(ReferencePlane plane) override;
         PieceInterface* getJointPiece();
+        void setDirection(Direction dir);
+        bool setTransparency(float alpha) override;
+        void update(sf::Clock cl, float multiplier)override {};
+        float getZ_Acc();
 };
 
 #endif

src/collections/headers/lower_body.hpp

@@ -12,6 +12,13 @@ struct gamba_data{
 
 
 class Lower_Body : public CollectionInterface{
+private:
+    int64_t prevT = 0;
+    float velD = 0;
+    float velS = 0;
+    float posS = 0;
+    float posD = 0;
+
 protected:
     Gamba* sx;
     Gamba* dx;
@@ -26,9 +33,9 @@ protected:
 public:
     Lower_Body(rb::Vector3 pos, std::vector<gamba_data> data);
     ~Lower_Body();
-
+    void update(sf::Clock cl, float multiplier) override;
     void setVisibility(bool c);
-    void setTransparency(float t);
+    bool setTransparency(float alpha) override;
     collection create(ReferencePlane plane) override;
 };
 

src/collections/methods/gamba.cpp

@@ -15,8 +15,8 @@ Gamba::Gamba(rb::Vector3 pos, unsigned int* dataPos, std::string cosciaData, std
         sensori.push_back(new Sensore (rb::Vector3{pos[0],pos[1],pos[2]+200},_Float16( 0.2 ),dataPos,caviglia));
 
         // modifico la rotazione relativa della gamba
-        sensori[0]->body.setRot({_Float16 (1.3),_Float16 (1.7),0});
-        sensori[1]->body.setRot({_Float16 (1.8),_Float16 (1.7),0});
+        sensori[0]->body.setRot({_Float16 (1.5708),_Float16 (1.5708),0});
+        sensori[1]->body.setRot({_Float16 (1.5708),_Float16 (1.5708),0});
 
         joints.push_back(new RigidJoint(sensori[0], {pezzi[0]}));
         joints.push_back(new PivotJoint(sensori[0], {sensori[1]}, rb::Vector3{0,0,100}));
@@ -47,4 +47,37 @@ collection Gamba::create(ReferencePlane plane){
 
 PieceInterface* Gamba::getJointPiece(){
     return sensori[0];
+}
+
+void Gamba::setDirection(Direction dir){
+    for (auto i : pezzi){
+        i->setDirection(dir);
+    }
+    for (auto i : sensori){
+        i->setDirection(dir);
+    }
+}
+
+bool Gamba::setTransparency(float alpha){
+    for (auto i : pezzi){
+        if (!i->setTransparency(alpha)) return false;
+    }
+    for (auto i : sensori){
+        if (!i->setTransparency(alpha)) return false;
+    }
+    return true;
+}
+
+float Gamba::getZ_Acc(){
+    
+    float totZ_Acc = 0;
+    /*
+    for (auto i : sensori){
+        totZ_Acc += i->getZ_Acc();
+    }*/
+    
+    totZ_Acc = sensori[0]->getZ_Acc() + sensori[1]->getZ_Acc();
+    //printf("TotAccGamba %f\n", totZ_Acc);
+
+    return totZ_Acc;
 }

src/collections/methods/lower_body.cpp

@@ -3,28 +3,47 @@
 Lower_Body::Lower_Body(rb::Vector3 pos,std::vector<gamba_data> data){
     if (data.size() != 2) throw "Lower_Body_Error: data vector size must be 2";
 
-    sx = new Gamba({pos[0],pos[1],pos[2]+150},data[0].dataPos,data[0].cosciaData,data[0].cavigliaData);
-    dx = new Gamba({pos[0],pos[1]+200,pos[2]+150},data[1].dataPos,data[1].cosciaData,data[1].cavigliaData);
-    t = new Torso({pos[0],pos[1]+100,pos[2]},_Float16(3.0));
+    sx = new Gamba({pos[0],pos[1]-60,pos[2]+150},data[0].dataPos,data[0].cosciaData,data[0].cavigliaData);
+    dx = new Gamba({pos[0],pos[1]+60,pos[2]+150},data[1].dataPos,data[1].cosciaData,data[1].cavigliaData);
+    t = new Torso({pos[0],pos[1],pos[2]},_Float16(3.0));
 
     PieceInterface* psx = sx->getJointPiece();
     PieceInterface* pdx = dx->getJointPiece();
 
-    jsx = new PivotJoint(t, {psx}, rb::Vector3{0,-100,50});
-    jdx = new PivotJoint(t, {pdx}, rb::Vector3{0,100,50});
+    jsx = new PivotJoint(t, {psx}, rb::Vector3{0,-60,50});
+    jdx = new PivotJoint(t, {pdx}, rb::Vector3{0,60,50});
 }
 
 
 collection Lower_Body::create(ReferencePlane plane){
     collection coll;
+    
+    sx->setTransparency(1);
+    dx->setTransparency(1);
+
+    
+    coll.joints.push_back(jsx);
+    coll.joints.push_back(jdx);
 
     switch (plane)
     {
-    case ReferencePlane::XZ: case ReferencePlane::XZN:
+    case ReferencePlane::XZN: 
+        dx->setTransparency(0.5);
+        dx->setDirection(Direction::L);
+        sx->setDirection(Direction::R);
+        coll = coll + dx->create(plane);
+        coll = coll + sx->create(plane);
+        break;
+    case ReferencePlane::XZ:
+        sx->setTransparency(0.5);
+        dx->setDirection(Direction::R);
+        sx->setDirection(Direction::L);
         coll = coll + sx->create(plane);
         coll = coll + dx->create(plane);
         break;
     case ReferencePlane::YZ:
+        sx->setDirection(Direction::R);
+        dx->setDirection(Direction::L);
         coll = coll + dx->create(plane);
         coll = coll + sx->create(plane);
         break;
@@ -32,10 +51,8 @@ collection Lower_Body::create(ReferencePlane plane){
     default:
         break;
     }
-    coll.pieces.push_back(t);
-    coll.joints.push_back(jsx);
-    coll.joints.push_back(jdx);
     
+    coll.pieces.push_back(t);
     
     return coll;
 }
@@ -50,4 +67,41 @@ Lower_Body::~Lower_Body(){
 
 void Lower_Body::setVisibility(bool c){
 
+}
+
+bool Lower_Body::setTransparency(float alpha){
+    if (!sx->setTransparency(alpha)) return false;
+    dx->setTransparency(alpha);
+    t->setTransparency(alpha);
+    return true;
+}
+
+void Lower_Body::update(sf::Clock cl, float multiplier){
+    float sxAcc = sx->getZ_Acc() ;
+    float dxAcc = dx->getZ_Acc() ;
+
+    int64_t Dtime = cl.getElapsedTime().asMicroseconds();
+    if (prevT == 0) prevT >= Dtime;
+    float dt = (float(Dtime) / 1000000.0) - (float(prevT) / 1000000.0);
+    prevT = Dtime;
+
+    float tmpVelS = sxAcc*dt;
+    float tmpVelD = dxAcc*dt;
+    float tmpPosD =  tmpVelD *dt *500 + velD * 500 * dt; 
+    float tmpPosS =  tmpVelS * 500 *dt + velS * 500 * dt;
+
+    velD += tmpVelD; 
+    velS += tmpVelS;
+
+    // PosD + PosS + Z = 0
+    float alpha = atan(tmpPosD/60.0 - tmpPosS/60); //il 60 è il raggio (dimesione del bacino)
+
+    //applico smoothing e ritorno a zero
+    velD -= velD * fabs(alpha);
+    velS -= velS * fabs(alpha) ;
+    
+    t->body.setRot({alpha,0,0});
+    auto tPos = t->body.getPos();
+    
+
 }

src/pieces/headers/caviglia.hpp

@@ -14,7 +14,7 @@ class Caviglia : public PieceInterface{
         Caviglia(rb::Vector3 coords, _Float16 mass);
         ~Caviglia();
 
-        void update(sf::Clock cl) override;
+        void update(sf::Clock cl, float multiplier) override;
         sf::Shape* draw(ReferencePlane plane) override;
 };
 

src/pieces/headers/coscia.hpp

@@ -15,7 +15,7 @@ class Coscia : public PieceInterface{
         Coscia(rb::Vector3 coords, _Float16 mass);
         ~Coscia();
 
-        void update(sf::Clock cl) override;
+        void update(sf::Clock cl, float multiplier) override;
         sf::Shape* draw(ReferencePlane plane) override;
 };
 

src/pieces/headers/piece_interface.hpp

@@ -13,6 +13,11 @@ enum class ReferencePlane {
     XZN
 };
 
+enum class Direction {
+    L,
+    R
+};
+
 //classi
 class PieceInterface{
     protected:
@@ -25,16 +30,25 @@ class PieceInterface{
             shapeXZ->setFillColor(color);
             shapeYZ->setFillColor(color);
         }
-
+        Direction direction = Direction::L;
     public:
         sf::Shape* shapeXZ, *shapeYZ;
         rb::Vector3 globalPos;
         rb::rigidbody body;
         sf::Color color;
+        float transparency = 1.0; //canale alpha del pezzo
 
-        virtual void update(sf::Clock cl) = 0;
+        virtual void update(sf::Clock cl, float multiplier) = 0;
         virtual sf::Shape* draw(ReferencePlane plane) = 0;
         virtual ~PieceInterface(){}
+        virtual void setDirection(Direction dir){
+            direction = dir;
+        }
+        virtual bool setTransparency(float alpha){
+            if (alpha < 0 || alpha > 1) return false;
+            transparency = alpha; 
+            return true;
+        }
 };
 
 

src/pieces/headers/sensore.hpp

@@ -15,6 +15,8 @@ class Sensore : public PieceInterface{
         std::vector<std::vector<float>> rotData;
         std::vector<float> timeData;
 
+        float gModule;
+
         //in che punto sto controllando il segnale 
         unsigned int* dataPos;
 
@@ -27,13 +29,13 @@ class Sensore : public PieceInterface{
         Sensore(rb::Vector3 coords, _Float16 mass, unsigned int* st, std::vector<std::vector<float>> data);
         ~Sensore();
 
-        void update(sf::Clock cl) override;
+        void update(sf::Clock cl,float multiplier) override;
         sf::Shape* draw(ReferencePlane plane) override;
 
         //funzioni specifiche
         void initCSV(std::vector<std::vector<float>> data);
-        void setIntervall(int min, int max);
-        void setPos(int &pos);
+
+        float getZ_Acc();
 };
 
 #endif

src/pieces/headers/torso.hpp

@@ -8,13 +8,13 @@
 
 class Torso : public PieceInterface{
     private:
-        const sf::Vector3f torso_Dim = {100, 100, 250};
+        const sf::Vector3f torso_Dim = {100, 100, 150};
         const sf::Color torso_Col = sf::Color::Red;
     public:
         Torso(rb::Vector3 coords, _Float16 mass);
         ~Torso();
 
-        void update(sf::Clock cl) override;
+        void update(sf::Clock cl, float multiplier) override;
         sf::Shape* draw(ReferencePlane plane) override;
 };
 

src/pieces/methods/caviglia_class.cpp

@@ -2,7 +2,7 @@
 
 Caviglia::Caviglia(rb::Vector3 coords, _Float16 mass){
     rb::Vector3 com = {caviglia_Dim.x/2,caviglia_Dim.x/2, caviglia_Dim.y/2};
-    body = rb::rigidbody(coords, com, mass);
+    body = rb::rigidbody(coords, com, mass, caviglia_Dim.x/2);
     color = caviglia_Col;
     globalPos = {0,0,0};
 
@@ -15,7 +15,7 @@ Caviglia::~Caviglia(){
     delete shapeYZ;
 }
 
-void Caviglia::update(sf::Clock cl){
+void Caviglia::update(sf::Clock cl,float multiplier){
     //body.step(cl);
 }
 
@@ -26,12 +26,13 @@ sf::Shape* Caviglia::draw(ReferencePlane plane){
 
     switch (plane)
     {
-    case ReferencePlane::XZ:
+    case ReferencePlane::XZ : case ReferencePlane::XZN:
         {
         sf::Shape* shape = shapeXZ;
         shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
         shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
         shape->setScale({1,cos(float(tmpRot[0]))});
+        shape->setFillColor(color*sf::Color(255,255,255,transparency*255));
         return shape;}
         break;
     
@@ -41,6 +42,7 @@ sf::Shape* Caviglia::draw(ReferencePlane plane){
         shape->setRotation(sf::Angle(sf::radians(tmpRot[0])));
         shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
         shape->setScale({1,cos(float(tmpRot[1]))});
+        shape->setFillColor(color*sf::Color(255,255,255,transparency*255));
         return shape;}
         break;
     

src/pieces/methods/coscia_class.cpp

@@ -2,7 +2,7 @@
 
 Coscia::Coscia(rb::Vector3 coords, _Float16 mass){
     rb::Vector3 com = {coscia_Dim.x/2,coscia_Dim.z/2,coscia_Dim.y/2};
-    body = rb::rigidbody(coords, com, mass);
+    body = rb::rigidbody(coords, com, mass, coscia_Dim.z/2);
     color = coscia_Col;
     globalPos = {0,0,0};
     initialize_shapes(coscia_Dim);
@@ -13,7 +13,7 @@ Coscia::~Coscia(){
     delete shapeYZ;
 }
 
-void Coscia::update(sf::Clock cl){
+void Coscia::update(sf::Clock cl, float multiplier){
     //body.step(cl);
 }
 
@@ -25,17 +25,13 @@ sf::Shape* Coscia::draw(ReferencePlane plane){
 
     switch (plane)
     {
-    case ReferencePlane::XZ:
+    case ReferencePlane::XZ : case ReferencePlane::XZN:
         {
         sf::Shape* shape = shapeXZ;
         shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
         shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
-
-        //calcolo ridimensionamento dato da cos(x)-> questo per definire l'ancoraggio corretto del pivot
         shape->setScale({1,cos(float(tmpRot[0]))});
-        //shape->setScale({1,(0.5* cos(float(tmpRot[0]*2)))+0.5});
-
-        
+        shape->setFillColor(color*sf::Color(255,255,255,transparency*255));
         return shape;}
         break;
     
@@ -44,10 +40,8 @@ sf::Shape* Coscia::draw(ReferencePlane plane){
         sf::Shape* shape = shapeYZ;
         shape->setRotation(sf::Angle(sf::radians(tmpRot[0])));
         shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
-
-        //calcolo ridimensionamento dato da cos(x) -> questo per definire l'ancoraggio corretto del pivot
+        shape->setFillColor(color*sf::Color(255,255,255,transparency*255));
         shape->setScale({1,cos(float(tmpRot[1]))});
-        //shape->setScale({1,(0.5* cos(float(tmpRot[1]*2)))+0.5});
         return shape;}
         break;
     

src/pieces/methods/sensore_class.cpp

@@ -3,7 +3,7 @@
 
 Sensore::Sensore(rb::Vector3 coords, _Float16 mass){
     rb::Vector3 com = {sensore_Dim.x/2,sensore_Dim.z/2, sensore_Dim.y/2};
-    body = rb::rigidbody(coords, com, mass);
+    body = rb::rigidbody(coords, com, mass, sensore_Dim.z/2);
     color = sensore_Col;
     globalPos = {0,0,0};
     initialize_shapes(sensore_Dim);
@@ -22,7 +22,7 @@ Sensore::~Sensore(){
 
 void Sensore::initCSV(std::vector<std::vector<float>> data){
     //timestamp_ns, wx, wy, wz, ax, ay, az, gx, gy, gz
-    if (data.size() < 1) throw "Sensor data empty";
+    if (data.size() < 10) throw "Sensor data empty";
     float stTime = int64_t( data[0][0] ) ;
 
     for (std::vector<float> row : data){
@@ -37,10 +37,19 @@ void Sensore::initCSV(std::vector<std::vector<float>> data){
         accData.push_back(tmpA);
         gData.push_back(tmpG);
     }
+
+    //trovo il modulo di g facendo la media del modulo nei primi 1000 campioni
+    gModule = 0;
+    int nCampioni = int(data.size())>1000 ? 1000 : 10;
+    for(int i = 0; i<nCampioni ;i++) {
+        gModule +=  sqrt(pow(gData[i][0],2)+pow(gData[i][1],2)+pow(gData[i][2],2));
+    }
+    gModule = gModule / 1000;
+
 }
 
 
-void Sensore::update(sf::Clock cl){
+void Sensore::update(sf::Clock cl, float multiplier){
    
     //calcolo la posizione e velocità
     if (*dataPos >= gData.size()) *dataPos = gData.size()-1;
@@ -49,7 +58,7 @@ void Sensore::update(sf::Clock cl){
 
 
     body.setAcc(rb::Vector3{accData[*dataPos]});
-    body.step(cl);
+    body.step(cl, multiplier);
    
 }
 
@@ -60,11 +69,13 @@ sf::Shape* Sensore::draw(ReferencePlane plane){
 
     switch (plane)
     {
-    case ReferencePlane::XZ:
+    case ReferencePlane::XZ : case ReferencePlane::XZN:
         {
+       
         sf::Shape* shape = shapeXZ;
         shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
         shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
+        shape->setFillColor(color*sf::Color(255,255,255,transparency*255));
         return shape;}
         break;
     
@@ -73,6 +84,7 @@ sf::Shape* Sensore::draw(ReferencePlane plane){
         sf::Shape* shape = shapeYZ;
         shape->setRotation(sf::Angle(sf::radians(tmpRot[0])));
         shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
+        shape->setFillColor(color*sf::Color(255,255,255,transparency*255));
         return shape;}
         break;
     
@@ -85,23 +97,37 @@ sf::Shape* Sensore::draw(ReferencePlane plane){
 
 
 void Sensore::calcRotWithG(unsigned int index){ // calcolo rotazione con valori della gravità
-
+    int dir = direction == Direction::R ? -1 : 1;
     std::vector<float> grav = gData[index];
-    float modG = sqrt(pow(grav[0],2)+pow(grav[1],2)+pow(grav[2],2));
 
     //x = mod * cosX -> mod = x/cosx -> cosx = x/mod
 
-    float tmpSinX = -grav[0] / modG;
-    float tmpSinY = -grav[1] / modG;
-    float tmpSinZ = -grav[2] / modG;
+    float tmpSinX = -grav[0] / gModule;
+    float tmpSinY = -grav[1] / gModule;
+    float tmpSinZ = -grav[2] / gModule;
 
-    float tmpAX = acos(tmpSinX);
-    float tmpAY = acos(tmpSinY);
+    float tmpAX = acos(dir*tmpSinX);
+    float tmpAY = acos(dir*tmpSinY);
     float tmpAZ = acos(tmpSinZ);
 
     body.setRot(rb::Vector3{tmpAY, tmpAX, tmpAZ });
 
 }
 
+float Sensore::getZ_Acc(){
+    int id = *dataPos;
+    float tmpAcc = 0;
+    
+    rb::Vector3 acc = body.getAcc();
+    rb::Vector3 rot = body.getRot();
 
+    float modAcc = sqrt(pow(acc[0],2)+pow(acc[1],2)+pow(acc[2],2));
+
+    float zAcc = cos(rot[2]) * modAcc;
+
+    //dipende se il sensore conta la gravità nell'accelerazione sugli assi
+    tmpAcc = zAcc - gModule;
+    //tmpAcc = gModule - sqrt(pow(gData[id][0],2)+pow(gData[id][1],2)+pow(gData[id][2],2));
+    return tmpAcc;
+}
 /////////////// cinematica inversa

src/pieces/methods/torso.cpp

@@ -2,7 +2,7 @@
 
 Torso::Torso(rb::Vector3 coords, _Float16 mass){
     rb::Vector3 com = {torso_Dim.x/2, torso_Dim.y/2, torso_Dim.z/2};
-    body = rb::rigidbody(coords,com, mass);
+    body = rb::rigidbody(coords,com, mass, torso_Dim.y/2);
     color = torso_Col;
     globalPos = {0,0,0};
 
@@ -14,7 +14,7 @@ Torso::~Torso(){
     delete shapeYZ;
 }
 
-void Torso::update(sf::Clock cl){
+void Torso::update(sf::Clock cl,float multiplier){
     
 }
 
@@ -26,11 +26,12 @@ sf::Shape* Torso::draw(ReferencePlane plane){
 
     switch (plane)
     {
-    case ReferencePlane::XZ:
+    case ReferencePlane::XZ: case ReferencePlane::XZN:
         {
         sf::Shape* shape = shapeXZ;
         shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
         shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
+        shape->setFillColor(color*sf::Color(255,255,255,transparency*255));
         return shape;}
         break;
     
@@ -39,6 +40,7 @@ sf::Shape* Torso::draw(ReferencePlane plane){
         sf::Shape* shape = shapeYZ;
         shape->setRotation(sf::Angle(sf::radians(tmpRot[0])));
         shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
+        shape->setFillColor(color*sf::Color(255,255,255,transparency*255));
         return shape;}
         break;
     

src/rigidbody/headers/rb.hpp

@@ -16,7 +16,9 @@
                 Vector3 acc = {0,0,0};
                 Vector3 rot = {0,0,0};
                 Vector3 tanAcc = {0,0,0};
+                Vector3 tanVel = {0,0,0};
 
+                float R = 1;
                 _Float16 mass = 1;
 
                 Vector3 coords = {0,0,0};
@@ -26,7 +28,7 @@
 
                 //funzioni
                 void calcVel(const float Dtime);
-                void calcRot(const time_t Dtime);
+                void calcRot(const float Dtime);
                 void calcAcc(const Vector3 Dacc);
                 void calcTanAcc(const Vector3 Dacc);
                 void calcPos(const float Dtime);
@@ -34,18 +36,20 @@
 
 
             public:
-                rigidbody(){}
-                rigidbody(Vector3 coords, Vector3 centerOfMass, _Float16 mass);
+                rigidbody(){ }
+                rigidbody(Vector3 coords, Vector3 centerOfMass, _Float16 mass, float radius);
                 ~rigidbody();
 
                 
                 Vector3 getPos();
                 Vector3 getRot();
+                Vector3 getAcc();
                 void setPos(const Vector3 Npos);
                 void setRot(const Vector3 Nrot);
                 void setVel(const Vector3 Nacc);
                 void setAcc(const Vector3 Nvel);
-                void step(const sf::Clock time);
+                void setTanAcc(const Vector3 Dacc);
+                void step(const sf::Clock time, float multiplier);
 
                 //complesso, deve definire accelerazione e accelerazione tangenziale 
                 void appForce(Vector3 f, Vector3 pos);

src/rigidbody/methods/rb_class.cpp

@@ -3,7 +3,7 @@
 
 using namespace rb ;
 
-rigidbody::rigidbody(Vector3 coords, Vector3 centerOfMass, _Float16 mass)
+rigidbody::rigidbody(Vector3 coords, Vector3 centerOfMass, _Float16 mass, float radius)
 {
     if (coords.size() != 3) throw "Coords must be 3";
     if (centerOfMass.size() != 3) throw "COM coords must be 3";
@@ -11,7 +11,7 @@ rigidbody::rigidbody(Vector3 coords, Vector3 centerOfMass, _Float16 mass)
     this->coords = coords;
     this->centerOfMass = centerOfMass;
     this->mass = mass;
-
+    this->R = radius;
 }
 
 rigidbody::~rigidbody()
@@ -37,7 +37,7 @@ void rigidbody::setPos(Vector3 Npos){
 }
 
 void rigidbody::setAcc(const Vector3 Nacc){
-    if (Nacc.size() != 3) throw "Vel vector must be 3 in lenght!";
+    if (Nacc.size() != 3) throw "Acc vector must be 3 in lenght!";
     
     int i = 0;
     for (float axis : Nacc){
@@ -85,16 +85,55 @@ void rigidbody::calcPos(const float Dtime){
     }
 }
 
-void rigidbody::step(const sf::Clock time){
+void rigidbody::step(const sf::Clock time, float multiplier){
     int64_t Dtime = time.getElapsedTime().asMicroseconds();
-    if (prevT == 0) prevT = Dtime;
+    if (prevT == 0) prevT >= Dtime;
 
 
-    float dt = (float(Dtime) / 1000000.0) - (float(prevT) / 1000000.0);
+    float dt = ((float(Dtime) / 1000000.0) - (float(prevT) / 1000000.0)) * multiplier;
     prevT = Dtime;
 
+    calcRot(dt);
     calcPos(dt);
     calcVel(dt);
 }
     
+void rigidbody::setTanAcc(const Vector3 Dacc){
+    if (Dacc.size() != 3) throw "Vel vector must be 3 in lenght!";
 
+    int i = 0;
+    for (float a : Dacc ){
+        tanAcc[i] = a;
+        i++;
+    }
+
+}
+
+
+void rigidbody::calcRot(const float Dtime){
+    // Ds = wt +1/2*at^2 -> l'accelerazione angolare la trovo ac = v^2/R
+    Vector3 tmpVel;
+    for (float a : tanAcc){
+        tmpVel.push_back( a*Dtime );
+    }
+    int i = 0;
+    for (float nv : tmpVel){
+        tanVel[i++] += nv;
+    }
+
+    Vector3 tmpTanAcc;
+    for (int i = 0; i<3; i++){
+        tmpTanAcc.push_back( pow(tanVel[i],2) / R );
+    }
+
+    i=0;
+    for (auto axes : rot){
+        rot[i] = axes + (0.5 * tmpTanAcc[i] * pow(Dtime,2));
+        i++;
+    }
+
+}
+
+rb::Vector3 rigidbody::getAcc(){
+    return Vector3(acc);
+}

src/sfml_util.cpp

@@ -29,10 +29,11 @@ struct State
     sf::Vector2f cameraOffset = {0.,0.};
 
     sf::Clock clock;
+    float* tMul; 
+    sf::Clock PieceClock;
     ReferencePlane selectedPlane = ReferencePlane::XZ;
 
     PieceInterface* selected = nullptr;
-
     bool rot_Piece = false;
     bool drag_Piece = false;
     bool drag = false;
@@ -50,6 +51,8 @@ struct State
         window = sf::RenderWindow(sf::VideoMode({w, h}), title);
         if (ImGui::SFML::Init(window)); // L'if è solo per togliere il warning, va aggiustato gestendo le eccezioni
         clock.restart();
+        tMul = new float(1.0);
+        PieceClock.restart();
         intervalMajLimit = maj;
         intervalMinLimit = min;
         this->pos = pos; 
@@ -66,6 +69,9 @@ struct State
             createdColl.push_back(c->create(selectedPlane));
         }
     }
+    ~State(){
+        delete tMul;
+    }
 };
 
 ///
@@ -80,11 +86,16 @@ void State::update(){
     std::vector<PieceInterface*> collPieces;
     std::vector<JointInterface*> collJoints;
     */
+    if (play){
+        for (auto i : collections){
+            i->update(PieceClock, *tMul);
+        }
+    }
 
     for (auto i : createdColl){
         if (play){
             for (auto j : i.pieces){
-                j->update(clock);
+                j->update(PieceClock, *tMul);
             }
         }
         for (auto j : i.joints){
@@ -94,7 +105,7 @@ void State::update(){
 
     if (play){
         for(PieceInterface* p : pieces){
-            p->update(clock);
+            p->update(PieceClock, *tMul);
         }
     }
     for(JointInterface* j : joints){
@@ -320,16 +331,34 @@ void doGUI(State &gs)
     ImGui::Begin("Set visualization plane",0,sdp_flags);
     const char* MyEnumNames[] = { "XZ", "YZ", "-XZ" };
     int currentPlane = (int)gs.selectedPlane;
-    if (ImGui::SliderInt("Selected Plane", &currentPlane,0,2,MyEnumNames[currentPlane])) gs.updateCollections();
-    gs.selectedPlane = (ReferencePlane)currentPlane;
+    if (ImGui::SliderInt("Selected Plane", &currentPlane,0,2,MyEnumNames[currentPlane])){
+        gs.selectedPlane = (ReferencePlane)currentPlane;
+        gs.updateCollections();
+    } 
     ImGui::End();
 
+   
+    //Finestra gestione velocità di riproduzione 
+    ImGui::Begin("Set time multiplier", 0,sdp_flags);
+    const float TimeMul[] = {0.5, 0.75, 1, 1.25, 1.5};
+    const char* TimeMulChar[] = {"0.5", "0.75", "1", "1.25", "1.5"};
+    static int Timeid = 2;
+    ImGui::SliderInt("Time", &Timeid,0,4,TimeMulChar[Timeid]);
+    *gs.tMul = TimeMul[Timeid]; 
+    ImGui::End();
+
+    //Finestra controllo sovrapposizione 
+    
+    
+
 
     sf::Vector2u wsize = gs.window.getSize();
     ImGui::SetWindowPos("Set data position",ImVec2(0,wsize.y - 30));
     ImGui::SetWindowSize("Set data position",ImVec2(wsize.x,30));
     ImGui::SetWindowPos("Set visualization plane",ImVec2(wsize.x-400,0));
     ImGui::SetWindowSize("Set visualization plane",ImVec2(400,30));
+    ImGui::SetWindowPos("Set time multiplier",ImVec2(wsize.x-400,wsize.y - 60));
+    ImGui::SetWindowSize("Set time multiplier",ImVec2(400,30));
     ImGui::SFML::Render(gs.window);
 }
 

src/testMain.cpp

@@ -43,8 +43,8 @@ int main() {
         const auto& coscia = processor.getData();
         gs.setIntervall(coscia.size());
 
-        /*
-        gs.pieces.push_back(new Coscia (rb::Vector3{300,300,300},2));
+        /*        
+        gs.pieces.push_back(new Coscia (rb::Vector3{0,0,0},2));
         gs.pieces.push_back(new Sensore (rb::Vector3{300,300,300},_Float16( 0.2 ),&pos,coscia));
         gs.pieces.push_back(new Caviglia (rb::Vector3{300,300,500},1));
 
@@ -68,11 +68,12 @@ int main() {
         gs.joints.push_back(new RigidJoint(gs.pieces[1], {gs.pieces[0]}));
         gs.joints.push_back(new PivotJoint(gs.pieces[1], {gs.pieces[3]}, rb::Vector3{0,0,100}));
         gs.joints.push_back(new RigidJoint(gs.pieces[3], {gs.pieces[2]}));
+        
+        gs.pieces[2]->setDirection(Direction::R);
         */
-
         //provo ad aggiungere una collection
         //gs.collections.push_back(new Gamba({220,0,220},&pos,"coscia_filt.csv","caviglia_filt.csv"));
-
+        
         std::vector<gamba_data> data;
         gamba_data d;
         d.dataPos = &pos;
@@ -85,7 +86,7 @@ int main() {
         data.push_back(d);
         data.push_back(s);
         gs.collections.push_back(new Lower_Body(rb::Vector3{200,200,100},data));
-
+        
         printf("Ho costruito tutto!\n");
     }
     catch(char* e){
@@ -105,7 +106,7 @@ int main() {
     sf::Clock mainClock;
     while (gs.window.isOpen()) 
     {
-        curTime += mainClock.restart().asMilliseconds();
+        curTime += mainClock.restart().asMilliseconds() *(*gs.tMul) ;
         if (curTime > T){
             if (gs.play && pos+curTime/T < maj) pos += curTime / T; 
             curTime = 0;