Aggiornato README

CMakeLists.txt

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

README.md

@@ -50,10 +50,14 @@ Per spostare l'intera scena si tiene premuto il tasto centrale del mouse.
 - 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
+
 # Per compilare:
 
     cmake --build
 
 # Per lanciare:
 
-    ./build/bin/mainV7
+    ./build/bin/mainV8

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!]

src/collections/headers/collection_interface.hpp

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

src/collections/headers/gamba.hpp

@@ -6,7 +6,7 @@
 
 class Gamba : public CollectionInterface {
     protected:
-        std::vector<PieceInterface*> sensori;
+        std::vector<Sensore*> sensori;
         std::vector<PieceInterface*> pezzi;
         std::vector<JointInterface*> joints;
     public:
@@ -15,6 +15,8 @@ class Gamba : public CollectionInterface {
         PieceInterface* getJointPiece();
         void setDirection(Direction dir);
         bool setTransparency(float alpha) override;
+        void update(sf::Clock cl)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,7 +33,7 @@ protected:
 public:
     Lower_Body(rb::Vector3 pos, std::vector<gamba_data> data);
     ~Lower_Body();
-
+    void update(sf::Clock cl) override;
     void setVisibility(bool c);
     bool setTransparency(float alpha) override;
     collection create(ReferencePlane plane) override;

src/collections/methods/gamba.cpp

@@ -67,3 +67,17 @@ bool Gamba::setTransparency(float alpha){
     }
     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

@@ -21,6 +21,10 @@ collection Lower_Body::create(ReferencePlane plane){
     sx->setTransparency(1);
     dx->setTransparency(1);
 
+    
+    coll.joints.push_back(jsx);
+    coll.joints.push_back(jdx);
+
     switch (plane)
     {
     case ReferencePlane::XZN: 
@@ -47,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;
 }
@@ -73,3 +75,33 @@ bool Lower_Body::setTransparency(float alpha){
     t->setTransparency(alpha);
     return true;
 }
+
+void Lower_Body::update(sf::Clock cl){
+    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/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;
 
@@ -32,8 +34,8 @@ class Sensore : public PieceInterface{
 
         //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/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};
 

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);

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,6 +37,15 @@ 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;
+
 }
 
 
@@ -90,13 +99,12 @@ 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(dir*tmpSinX);
     float tmpAY = acos(dir*tmpSinY);
@@ -106,5 +114,20 @@ void Sensore::calcRotWithG(unsigned int index){ // calcolo rotazione con valori
 
 }
 
+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};
 

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);
@@ -35,16 +37,18 @@
 
             public:
                 rigidbody(){ }
-                rigidbody(Vector3 coords, Vector3 centerOfMass, _Float16 mass);
+                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 setTanAcc(const Vector3 Dacc);
                 void step(const sf::Clock time);
 
                 //complesso, deve definire accelerazione e accelerazione tangenziale 

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){
@@ -93,8 +93,47 @@ void rigidbody::step(const sf::Clock time){
     float dt = (float(Dtime) / 1000000.0) - (float(prevT) / 1000000.0);
     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,6 +29,7 @@ struct State
     sf::Vector2f cameraOffset = {0.,0.};
 
     sf::Clock clock;
+    sf::Clock PieceClock;
     ReferencePlane selectedPlane = ReferencePlane::XZ;
 
     PieceInterface* selected = nullptr;
@@ -50,6 +51,7 @@ 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();
+        PieceClock.restart();
         intervalMajLimit = maj;
         intervalMinLimit = min;
         this->pos = pos; 
@@ -80,11 +82,16 @@ void State::update(){
     std::vector<PieceInterface*> collPieces;
     std::vector<JointInterface*> collJoints;
     */
+    if (play){
+        for (auto i : collections){
+            i->update(PieceClock);
+        }
+    }
 
     for (auto i : createdColl){
         if (play){
             for (auto j : i.pieces){
-                j->update(clock);
+                j->update(PieceClock);
             }
         }
         for (auto j : i.joints){
@@ -94,7 +101,7 @@ void State::update(){
 
     if (play){
         for(PieceInterface* p : pieces){
-            p->update(clock);
+            p->update(PieceClock);
         }
     }
     for(JointInterface* j : joints){
@@ -325,6 +332,13 @@ void doGUI(State &gs)
         gs.updateCollections();
     } 
 
+
+    /*//////   DA FARE  ///////*/
+
+    //Finestra gestione velocità di riproduzione 
+
+    //Finestra controllo sovrapposizione 
+    
     ImGui::End();