Cambio sistema per rotazione bacino (non funziona)

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

src/collections/headers/gamba.hpp

@@ -15,7 +15,7 @@ class Gamba : public CollectionInterface {
         PieceInterface* getJointPiece();
         void setDirection(Direction dir);
         bool setTransparency(float alpha) override;
-        void update() {};
+        void update(sf::Clock cl)override {};
         float getZ_Acc();
 };
 

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() override;
+    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

@@ -71,9 +71,13 @@ bool Gamba::setTransparency(float alpha){
 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;
 }
@@ -74,11 +76,37 @@ bool Lower_Body::setTransparency(float alpha){
     return true;
 }
 
-void Lower_Body::update(){
-    float sxAcc = sx->getZ_Acc();
-    float dxAcc = dx->getZ_Acc();
+void Lower_Body::update(sf::Clock cl){
+    float sxAcc = sx->getZ_Acc() * 10;
+    float dxAcc = dx->getZ_Acc() * 10000;
 
-    float totAcc = sxAcc + dxAcc;
+    //float totAcc = sxAcc + dxAcc;
+    //t->body.setTanAcc({0,totAcc,0}); // non funziona, cambio sistema
+
+    /* Posso considerare lo spostamento come A*sin(alpha)*/
+    /* Mi calcolo le velocità totali sull'asse z */
+
+    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 =  velD *dt ;
+    float tmpPosS = velS * 100 * dt + tmpVelD * 100 *dt;
+
+    velD += tmpVelD; 
+    velS += tmpVelS;
+    posD += tmpPosD;
+    posS += tmpPosS;
+
+    // PosD + PosS + Z = 0
+    float alpha = asin(posD/60.0);
+
+    t->body.setRot({alpha,0,0});
+    //auto tPos = t->body.getPos();
+    //t->body.setPos({tPos[0],tPos[1],tPos[2]+tmpPosD});
 
-    t->body.setTanAcc({0,totAcc,0});
 }

src/pieces/methods/sensore_class.cpp

@@ -117,8 +117,17 @@ 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();
 
-    tmpAcc = gModule - sqrt(pow(gData[id][0],2)+pow(gData[id][1],2)+pow(gData[id][2],2));
+    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/rigidbody/headers/rb.hpp

@@ -43,6 +43,7 @@
                 
                 Vector3 getPos();
                 Vector3 getRot();
+                Vector3 getAcc();
                 void setPos(const Vector3 Npos);
                 void setRot(const Vector3 Nrot);
                 void setVel(const Vector3 Nacc);

src/rigidbody/methods/rb_class.cpp

@@ -133,3 +133,7 @@ void rigidbody::calcRot(const float Dtime){
     }
 
 }
+
+rb::Vector3 rigidbody::getAcc(){
+    return Vector3(acc);
+}

src/sfml_util.cpp

@@ -82,7 +82,7 @@ void State::update(){
     */
 
     for (auto i : collections){
-        i->update();
+        i->update(clock);
     }
 
     for (auto i : createdColl){