Applicazione correzione rotazione bacino

src/collections/methods/gamba.cpp

@@ -76,7 +76,7 @@ float Gamba::getZ_Acc(){
         totZ_Acc += i->getZ_Acc();
     }*/
     
-    totZ_Acc = sensori[0]->getZ_Acc();// + sensori[1]->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

@@ -77,8 +77,8 @@ bool Lower_Body::setTransparency(float alpha){
 }
 
 void Lower_Body::update(sf::Clock cl){
-    float sxAcc = sx->getZ_Acc() * 10;
-    float dxAcc = dx->getZ_Acc();
+    float sxAcc = sx->getZ_Acc() ;
+    float dxAcc = dx->getZ_Acc() ;
 
     //float totAcc = sxAcc + dxAcc;
     //t->body.setTanAcc({0,totAcc,0}); // non funziona, cambio sistema
@@ -86,7 +86,7 @@ void Lower_Body::update(sf::Clock cl){
     /* Posso considerare lo spostamento come A*sin(alpha)*/
     /* Mi calcolo le velocità totali sull'asse z */
 
-    printf("Acc: %f\n", dxAcc);
+    //printf("Acc: %f\n", dxAcc);
 
     int64_t Dtime = cl.getElapsedTime().asMicroseconds();
     if (prevT == 0) prevT = Dtime;
@@ -96,14 +96,14 @@ void Lower_Body::update(sf::Clock cl){
     float tmpVelS = sxAcc*dt;
     float tmpVelD = dxAcc*dt;
 
-    printf("Vel: %f\n", tmpVelD);
+    //printf("Vel: %f\n", tmpVelD);
 
 
-    float tmpPosD = velD * 100 * dt + tmpVelD *dt *100 ;
-    float tmpPosS = velS * 100 * dt + tmpVelD * 100 *dt;
+    float tmpPosD =  tmpVelD *dt *500 + velD * 500 * dt; 
+    float tmpPosS =  tmpVelS * 500 *dt + velS * 500 * dt;
 
 
-    printf("DPos: %f\n\n", tmpPosD);
+    //printf("DPos: %f\n\n", tmpPosD);
 
     velD += tmpVelD; 
     velS += tmpVelS;
@@ -111,10 +111,17 @@ void Lower_Body::update(sf::Clock cl){
     posS += tmpPosS;
 
     // PosD + PosS + Z = 0
-    float alpha = atan(tmpPosD/60.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) ;
+
+    printf("Svel: %f\n", velS);
+    printf("Dvel: %f\n\n", velD);
 
     t->body.setRot({alpha,0,0});
     auto tPos = t->body.getPos();
-    //t->body.setPos({tPos[0],tPos[1],tPos[2]+posD});
+    //t->body.setPos({tPos[0],tPos[1],tPos[2]+posD-posS});
 
 }

src/sfml_util.cpp

@@ -82,9 +82,10 @@ void State::update(){
     std::vector<PieceInterface*> collPieces;
     std::vector<JointInterface*> collJoints;
     */
-
-    for (auto i : collections){
-        i->update(PieceClock);
+    if (play){
+        for (auto i : collections){
+            i->update(PieceClock);
+        }
     }
 
     for (auto i : createdColl){