Aggiunta RELEASES da v0.1 a v1.1

RELEASES/FCG_VisualizzatoreCamminata-0.9/.gitignore

@@ -0,0 +1,23 @@
+CMakeLists.txt.user
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Testing
+Makefile
+cmake_install.cmake
+install_manifest.txt
+compile_commands.json
+CTestTestfile.cmake
+_deps
+CMakeUserPresets.json
+.cmake
+obj
+CPackConfig.cmake
+CPackSourceConfig.cmake
+
+# CLion
+#  JetBrains specific template is maintained in a separate JetBrains.gitignore that can
+#  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
+#  and can be added to the global gitignore or merged into this file.  For a more nuclear
+#  option (not recommended) you can uncomment the following to ignore the entire idea folder.
+#cmake-build-*

RELEASES/FCG_VisualizzatoreCamminata-0.9/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.28)
+project(CMakeSFMLProject LANGUAGES CXX)
+
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
+
+include(FetchContent)
+FetchContent_Declare(SFML
+    GIT_REPOSITORY https://github.com/SFML/SFML.git
+    GIT_TAG 3.0.0
+    GIT_SHALLOW ON
+    EXCLUDE_FROM_ALL
+    SYSTEM)
+FetchContent_MakeAvailable(SFML)
+
+FetchContent_Declare(
+	glm
+	GIT_REPOSITORY	https://github.com/g-truc/glm.git
+	GIT_TAG 	0af55ccecd98d4e5a8d1fad7de25ba429d60e863 #refs/tags/1.0.1
+)
+FetchContent_MakeAvailable(glm)
+
+# Aggiunta ImGUI
+
+
+FetchContent_Declare(ImGui
+    GIT_REPOSITORY https://github.com/ocornut/imgui
+    GIT_TAG v1.91.9b
+    GIT_SHALLOW ON
+    EXCLUDE_FROM_ALL
+    SYSTEM)
+FetchContent_MakeAvailable(ImGui)
+FetchContent_GetProperties(ImGui SOURCE_DIR IMGUI_DIR)
+
+set(IMGUI_SFML_FIND_SFML OFF)
+FetchContent_Declare(ImGui-SFML
+    GIT_REPOSITORY https://github.com/SFML/imgui-sfml
+    GIT_TAG v3.0
+    GIT_SHALLOW ON
+    EXCLUDE_FROM_ALL
+    SYSTEM)
+FetchContent_MakeAvailable(ImGui-SFML)
+
+add_library(common INTERFACE)
+target_compile_features(common INTERFACE cxx_std_17)
+target_compile_options(common INTERFACE
+  $<$<OR:$<CXX_COMPILER_ID:Clang>,$<CXX_COMPILER_ID:AppleClang>,$<CXX_COMPILER_ID:GNU>>: -Wall>
+  $<$<CXX_COMPILER_ID:MSVC>: /W4>
+)
+
+
+# Fine aggiunta
+
+
+set(METHODS_PATH "./src/*/methods/*.cpp")
+
+set(VERSION "V9")
+
+file(GLOB_RECURSE METHODS_SRC "${METHODS_PATH}")
+add_executable(main${VERSION} ./src/testMain.cpp ${METHODS_SRC} )
+target_link_libraries(main${VERSION} PRIVATE SFML::Graphics ImGui-SFML::ImGui-SFML common glm)
+target_compile_definitions(main${VERSION} PRIVATE $<$<CONFIG:Debug>:DEBUG_MODE>)

RELEASES/FCG_VisualizzatoreCamminata-0.9/README.md

@@ -0,0 +1,67 @@
+# FCG_VisualizzatoreCamminata
+
+
+## Nella versione versione v0.1 è presente la base del progetto.
+
+- La gerarchia dei file e delle classi
+
+    - Le classi sono divise in base allo scopo sotto directory diverse 
+
+- Definizione di pezzi (coscia, caviglia e sensori)
+
+- Definizione di joint (rigido e a pivot)
+
+- Semplice main di test
+
+Per questione di debug tutti i pezzi rappresentati si possono trascinare e ruotare con i rispettivi tasto sinistro e destro del mouse.
+
+Per spostare l'intera scena si tiene premuto il tasto centrale del mouse.
+
+## Nella versione v0.2:
+- Applicato refactoring di diverse classi
+- Aggiustato calcolo dei pivot implementando le rotazioni con algebra affine e glm
+- Aggiuta classe caviglia (per differenziarla dalla coscia) 
+
+## Nella versione v0.3:
+- Applicato refactoring delle classi pieces
+- Aggiunta pezzo torso
+- Nel testMain vengono agganciate caviglia e coscia al nuovo torso
+
+## Nella versione v0.4
+- Aggiunta vista frontale (con spazio si può camnbiare vista)
+- Modifica calcolo pivot per gestire spazio 3D
+- Aggiustati assi di riferimento (ora sono coerenti su tutte le classi)
+
+## Nella versione v0.5
+- Aggiunta ImGUI
+- Aggiunta selettore sulla posizione dei dati
+- Refactoring classe sensore e state per gestire posizione dati da gui
+
+## Nella versione v0.6
+- Aggiunta collezioni
+  - Servono a semplificare la struttura del main e il disegno degli elementi della gamba 
+- 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/mainV9

RELEASES/FCG_VisualizzatoreCamminata-0.9/build/bin/imgui.ini

@@ -0,0 +1,48 @@
+[Window][Debug##Default]
+Pos=60,60
+Size=400,400
+
+[Window][Dear ImGui Demo]
+Pos=487,44
+Size=455,873
+
+[Window][Hello, world!]
+Pos=11,12
+Size=212,56
+
+[Window][Dear ImGui Demo/##Basket_87771727]
+IsChild=1
+Size=478,260
+
+[Window][Dear ImGui Demo/##Basket_529977C1]
+IsChild=1
+Size=478,260
+
+[Window][Example: Custom rendering]
+Pos=68,96
+Size=740,472
+
+[Window][Set data position]
+Pos=0,670
+Size=800,30
+
+[Window][Dear ImGui Demo/ResizableChild_478B81A3]
+IsChild=1
+Size=499,136
+
+[Window][Dear ImGui Demo/Red_BEEF922B]
+IsChild=1
+Size=200,100
+
+[Window][Dear ImGui Style Editor]
+Pos=60,60
+Size=353,1005
+
+[Window][Set visualization plane]
+Pos=400,0
+Size=400,30
+
+[Window][Set time multiplier]
+Pos=400,640
+Size=400,30
+

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/collections/headers/collection_interface.hpp

@@ -0,0 +1,42 @@
+#include "../../pieces/headers/piece_interface.hpp"
+#include "../../joints/headers/joint_interface.hpp"
+
+#ifndef COLL_INTERFACE_H
+#define COLL_INTERFACE_H
+
+struct collection{
+    std::vector<PieceInterface*> pieces;
+    std::vector<JointInterface*> joints;
+};
+
+
+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(){};
+};
+
+
+
+
+inline collection operator+(collection c1, collection c2){
+    collection res;
+    for (PieceInterface* i : c1.pieces){
+        res.pieces.push_back(i);
+    }
+    for (PieceInterface* i : c2.pieces){
+        res.pieces.push_back(i);
+    }
+    for (JointInterface* i : c1.joints){
+        res.joints.push_back(i);
+    }
+    for (JointInterface* i : c2.joints){
+        res.joints.push_back(i);
+    }
+    
+    return res;
+}
+
+#endif

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/collections/headers/gamba.hpp

@@ -0,0 +1,22 @@
+#include "collection_interface.hpp"
+#include "include_pieces.hpp"
+
+#ifndef GAMBA_H
+#define GAMBA_H
+
+class Gamba : public CollectionInterface {
+    protected:
+        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

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/collections/headers/include_pieces.hpp

@@ -0,0 +1,7 @@
+#include "../../csv/headers/csv.hpp"
+#include "../../pieces/headers/caviglia.hpp"
+#include "../../pieces/headers/coscia.hpp"
+#include "../../pieces/headers/sensore.hpp"
+#include "../../joints/headers/rigid_joint.hpp"
+#include "../../joints/headers/pivot_joint.hpp"
+#include "../../pieces/headers/torso.hpp"

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/collections/headers/lower_body.hpp

@@ -0,0 +1,44 @@
+#include "gamba.hpp"
+#include "collection_interface.hpp"
+
+#ifndef LOWER_BODY_H
+#define LOWER_BODY_H
+
+struct gamba_data{
+    unsigned int* dataPos; 
+    std::string cosciaData; 
+    std::string cavigliaData;
+};
+
+
+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;
+    Torso* t;
+
+    PivotJoint* jsx;
+    PivotJoint* jdx;
+
+    bool visible = true;
+    float alpha = 1;
+
+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);
+    bool setTransparency(float alpha) override;
+    collection create(ReferencePlane plane) override;
+};
+
+
+
+#endif

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/collections/methods/gamba.cpp

@@ -0,0 +1,83 @@
+#include "../headers/gamba.hpp"
+
+Gamba::Gamba(rb::Vector3 pos, unsigned int* dataPos, std::string cosciaData, std::string cavigliaData){
+    CSVProcessor processor;
+    try {
+        pezzi.push_back(new Coscia(pos,2));
+        pezzi.push_back(new Caviglia(rb::Vector3{pos[0],pos[1],pos[2]+200},2));
+        
+        processor.readCSVFile (DATA_PATH + cosciaData);
+        const auto& coscia = processor.getData();
+        sensori.push_back(new Sensore(pos,_Float16( 0.2 ),dataPos,coscia));
+        
+        processor.readCSVFile(DATA_PATH + cavigliaData);
+        const auto& caviglia = processor.getData();
+        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.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}));
+        joints.push_back(new RigidJoint(sensori[1], {pezzi[1]}));
+
+    }
+    catch(char* e){
+        throw "Gamba ERROR : "+ std::string(e) +" \n" ;
+    }
+
+}
+
+collection Gamba::create(ReferencePlane plane){
+    collection coll;
+    for(auto e : pezzi){
+        coll.pieces.push_back(e);
+    }
+    for(auto e : sensori){
+        coll.pieces.push_back(e);
+    }
+
+    for(auto j : joints){
+        coll.joints.push_back(j);
+    }
+
+    return coll;
+}
+
+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;
+}

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/collections/methods/lower_body.cpp

@@ -0,0 +1,107 @@
+#include "../headers/lower_body.hpp"
+
+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]-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,-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::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;
+
+    default:
+        break;
+    }
+    
+    coll.pieces.push_back(t);
+    
+    return coll;
+}
+
+Lower_Body::~Lower_Body(){
+    delete sx;
+    delete dx;
+    delete t;
+    delete jdx;
+    delete jsx;
+}
+
+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();
+    
+
+}

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/csv/headers/csv.hpp

@@ -0,0 +1,27 @@
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <string>
+
+#ifndef CSV_H
+#define CSV_H
+
+#define DATA_PATH std::string("./../../data/")
+
+class CSVProcessor {
+private:
+    std::vector<std::string> headers;
+    std::vector<std::vector<float>> data;
+
+public:
+    // Method to read CSV file and store data in vectors
+    void readCSVFile(const std::string& filename);
+    // Getter for headers
+    const std::vector<std::string>& getHeaders() const;
+    // Getter for data
+    const std::vector<std::vector<float>>& getData() const;
+
+};
+
+#endif

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/csv/methods/csv_class.cpp

@@ -0,0 +1,53 @@
+#include "../headers/csv.hpp"
+
+void CSVProcessor::readCSVFile(const std::string& filename) {
+    std::ifstream file(filename);
+    if (!file.is_open()) {
+        throw std::runtime_error("Could not open file: " + filename);
+    }
+
+    std::string line;
+    // Read headers (first line)
+    if (std::getline(file, line)) {
+        std::stringstream ss(line);
+        std::string header;
+
+        while (std::getline(ss, header, ',')) {
+            headers.push_back(header);
+        }
+    }
+
+    data.clear();
+
+    // Read data
+    while (std::getline(file, line)) {
+        std::vector<float> row;
+        std::stringstream ss(line);
+        std::string value;
+
+        while (std::getline(ss, value, ',')) {
+            try {
+                row.push_back(std::stof(value));
+            } catch (const std::invalid_argument& e) {
+                // Handle non-integer values if needed
+                row.push_back(0); // or some other default/error value
+            }
+        }
+
+        data.push_back(row);
+    }
+
+    file.close();
+}
+
+// Getter for headers
+const std::vector<std::string>& CSVProcessor::getHeaders() const {
+    return headers;
+}
+
+// Getter for data
+const std::vector<std::vector<float>>& CSVProcessor::getData() const {
+    return data;
+}
+
+

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/include.hpp

@@ -0,0 +1,11 @@
+#include <iostream>
+#include "csv/headers/csv.hpp"
+#include "sfml_util.cpp"
+#include "pieces/headers/coscia.hpp"
+#include "pieces/headers/caviglia.hpp"
+#include "pieces/headers/sensore.hpp"
+#include "pieces/headers/torso.hpp"
+#include "joints/headers/rigid_joint.hpp"
+#include "joints/headers/pivot_joint.hpp"
+#include "collections/headers/gamba.hpp"
+#include "collections/headers/lower_body.hpp"

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/joints/headers/joint_interface.hpp

@@ -0,0 +1,34 @@
+#include "../../pieces/headers/piece_interface.hpp"
+#include <glm/glm.hpp>
+
+#ifndef JOINT_INTERFACE_H
+#define JOINT_INTERFACE_H
+
+/*
+1) il joint può essere tra più pezzi
+2) esistono 3 tipi di joint:
+   - completi / rigidi
+   - a pivot / 1 grado di libertà di rotazione
+   - spillo / completa libertà di rotazione 
+*/
+
+
+
+class JointInterface{
+    protected: 
+        virtual void rotate(unsigned int id) = 0;
+        virtual void traslate(unsigned int id) = 0;
+
+    public:
+        std::vector<rb::Vector3> offset;
+        std::vector<rb::Vector3> rotOffset;
+        PieceInterface* father;
+        std::vector<PieceInterface*> childs;
+
+        virtual ~JointInterface(){};
+        virtual void movechild() = 0;
+
+        
+};
+
+#endif

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/joints/headers/pivot_joint.hpp

@@ -0,0 +1,33 @@
+#include "joint_interface.hpp"
+
+#ifndef PIVOTJ_H
+#define PIVOTJ_H
+
+class PivotJoint : public JointInterface {
+    protected:
+        void rotate(unsigned int id) override;
+        void traslate(unsigned int id) override;
+        void updatePivot();
+
+        rb::Vector3 oldRot;
+        std::vector<rb::Vector3> oldCRot;
+        rb::Vector3 pivot;
+        rb::Vector3 stPivot;
+        
+        //angolo attuale pivot e offset
+        rb::Vector3 pivotAngle;
+        std::vector<rb::Vector3> offsetAngle;
+
+        //possono servire per calcolare l'offset rispetto alla posizione precedente
+        rb::Vector3 oldPos;
+        std::vector<rb::Vector3> oldCPos;
+        
+    public:
+        
+        void movechild() override;
+
+        PivotJoint(PieceInterface* father,std::vector<PieceInterface*> childs, rb::Vector3 pivotPoint);
+        ~PivotJoint();
+};
+
+#endif

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/joints/headers/rigid_joint.hpp

@@ -0,0 +1,23 @@
+#include "joint_interface.hpp"
+
+#ifndef RIGIDJ_H
+#define RIGIDJ_H
+
+class RigidJoint : public JointInterface {
+    protected:
+        void rotate(unsigned int id) override;
+        void traslate(unsigned int id) override;
+
+        //possono servire per calcolare l'offset rispetto alla posizione precedente
+        rb::Vector3 oldPos;
+        std::vector<rb::Vector3> oldCPos;
+        
+    public:
+        
+        void movechild() override;
+
+        RigidJoint(PieceInterface* father,std::vector<PieceInterface*> childs);
+        ~RigidJoint();
+};
+
+#endif

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/joints/methods/pivot_joint_class.cpp

@@ -0,0 +1,120 @@
+#include "../headers/pivot_joint.hpp"
+
+#define ZERO_INT 0.00001
+
+void PivotJoint::updatePivot(){
+    rb::Vector3 fRot = father->body.getRot();
+
+    pivotAngle = pivotAngle + (fRot - oldRot); // trovo angolo totale da angolo di partenza 
+    
+    float cosA = glm::cos(pivotAngle[0]);
+    float sinA = glm::sin(pivotAngle[0]);
+    float cosA1 = glm::cos(pivotAngle[1]);
+    float sinA1 = glm::sin(pivotAngle[1]);
+
+    glm::mat4 Rpx = glm::mat4{
+        1 , 0, 0, 0,
+        0, cosA, sinA, 0,
+        0, -sinA, cosA, 0,
+        0, 0, 0, 1
+    };
+
+    glm::mat4 Rpy = glm::mat4{
+        cosA1 , 0, sinA1, 0,
+        0, 1, 0, 0,
+        -sinA1, 0, cosA1, 0,
+        0, 0, 0, 1
+    };
+
+    glm::vec4 pivotN =  Rpy * Rpx * glm::vec4(stPivot[0], stPivot[1], stPivot[2],1);
+    pivot = rb::Vector3{pivotN[0],pivotN[1],pivotN[2]};
+
+    oldRot = fRot;
+}
+
+void PivotJoint::rotate(unsigned int id){
+    
+    rb::Vector3 fPos = father->body.getPos();
+    rb::Vector3 cRot = childs[id]->body.getRot();
+    
+    ////  sposto l'origine passivamente su tutti gli assi ////
+
+    offsetAngle[id] = offsetAngle[id] + (cRot - oldCRot[id]);
+
+    
+    float cosB = glm::cos(offsetAngle[id][0]);
+    float sinB = glm::sin(offsetAngle[id][0]);
+    
+    float cosB1 = glm::cos(offsetAngle[id][1]);
+    float sinB1 = glm::sin(offsetAngle[id][1]);
+    
+    glm::mat4 Rcx = glm::mat4{
+        1 , 0, 0, 0,
+        0, cosB, sinB, 0,
+        0, -sinB, cosB, 0,
+        0, 0, 0, 1
+    };
+
+    glm::mat4 Rcy = glm::mat4{
+        cosB1 , 0, sinB1, 0,
+        0, 1, 0, 0,
+        -sinB1, 0, cosB1, 0,
+        0, 0, 0, 1
+    };
+
+
+    glm::vec4 offN = Rcy * Rcx * glm::vec4(offset[id][0],offset[id][1],offset[id][2],1);
+    // offset[id] = rb::Vector3{offN[0],offN[1],offN[2]};
+
+    childs[id]->body.setPos(rb::Vector3{fPos[0]+offN[0]+pivot[0],fPos[1]+offN[1]+pivot[1],fPos[2]+offN[2]+pivot[2]});
+
+    //printf("Offset = %f %f %f \n" , offN[0], offN[1], offN[2]);
+
+    oldCRot[id] = cRot;
+
+}   
+
+void PivotJoint::traslate(unsigned int id){
+
+}
+
+
+PivotJoint::PivotJoint(PieceInterface* father,std::vector<PieceInterface*> childs, rb::Vector3 pivotPoint){
+    this->childs = childs;
+    this->father = father;
+    rb::Vector3 fCoords = father->globalPos + father->body.getPos();
+    rb::Vector3 fRot = father->body.getRot();
+
+    pivot = pivotPoint;
+    stPivot = pivotPoint;
+    rb::Vector3 pivotCenter = father->body.getPos() + stPivot;
+
+    pivotAngle = rb::Vector3{0,0,0};
+
+    oldRot = father->body.getRot();
+
+    //mi calcolo l'offset per ogni child rispetto al pivot
+    for(PieceInterface* c : childs){
+        rb::Vector3 tmpCoords;
+        rb::Vector3 cCoords = c->body.getPos();
+        tmpCoords = cCoords - pivotCenter;
+        oldCRot.push_back(c->body.getRot());
+        offset.push_back(tmpCoords);
+        offsetAngle.push_back(rb::Vector3{0,0,0});
+    }
+
+
+}
+
+PivotJoint::~PivotJoint(){
+
+}
+
+
+void PivotJoint::movechild(){
+    updatePivot();
+    for ( unsigned int i = 0; i < childs.size(); i++){
+        traslate(i);
+        rotate(i);
+    }
+}

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/joints/methods/rigid_joint_class.cpp

@@ -0,0 +1,102 @@
+#include "../headers/rigid_joint.hpp"
+
+#define ZERO_INT 0.00001
+
+//using namespace glm;
+
+void RigidJoint::rotate(unsigned int id){
+    rb::Vector3 fRot = father->body.getRot();
+    rb::Vector3 fRotOld = childs[id]->body.getRot() - rotOffset[id];
+    rb::Vector3 fPos = father->body.getPos();
+    rb::Vector3 cPos = childs[id]->body.getPos();
+
+    childs[id]->body.setRot(fRot + rotOffset[id]);
+
+    //passo a coordinate 3D per calcolare rotazione sul piano YZ oltre che al piano XZ
+    // calcolo alpha angolo 
+
+    float alpha = float (fRot[0] - fRotOld[0]);
+    float beta = float (fRot[1] - fRotOld[1]);
+
+    float cosA = glm::cos(alpha);
+    float sinA = glm::sin(alpha);
+    
+    float cosB = glm::cos(beta);
+    float sinB = glm::sin(beta);
+
+
+    glm::mat4 Rx = glm::mat4{
+        1 , 0, 0, 0,
+        0, cosA, sinA, 0,
+        0, -sinA, cosA, 0,
+        0, 0, 0, 1
+    };
+
+    glm::mat4 Ry = glm::mat4{
+        cosB , 0, sinB, 0,
+        0, 1, 0, 0,
+        -sinB, 0, cosB, 0,
+        0, 0, 0, 1
+    };
+
+    glm::mat4 T = glm::mat4{
+        1, 0, 0, 0, 
+        0, 1, 0, 0,
+        0 ,0, 1, 0,
+        fPos[0], fPos[1], fPos[2], 1
+    };
+
+    glm::vec4 resRot = Rx * Ry * glm::vec4(offset[id][0],offset[id][1],offset[id][2],1);
+    offset[id] = rb::Vector3{resRot[0], resRot[1], resRot[2]};
+
+    glm::vec4 resTransf = T * resRot;
+
+    childs[id]->body.setPos({resTransf[0],resTransf[1],resTransf[2]});
+    
+
+}   
+
+void RigidJoint::traslate(unsigned int id){
+    
+
+}
+
+RigidJoint::RigidJoint(PieceInterface* father,std::vector<PieceInterface*> childs){
+    this->childs = childs;
+    this->father = father;
+    rb::Vector3 fCoords = father->globalPos + father->body.getPos();
+    rb::Vector3 fRot = father->body.getRot();
+
+
+
+
+    //mi calcolo l'offset per ogni child rispetto al padre
+    for(PieceInterface* c : childs){
+        rb::Vector3 tmpCoords;
+        rb::Vector3 tmpRot;
+
+        rb::Vector3 cCoords = c->globalPos + c->body.getPos();
+
+        tmpCoords = cCoords - fCoords;
+        tmpRot = c->body.getRot() - fRot;
+
+        
+        offset.push_back(tmpCoords);
+        rotOffset.push_back(tmpRot);
+        
+    }
+
+
+}
+
+RigidJoint::~RigidJoint(){
+
+}
+
+void RigidJoint::movechild(){
+   
+    for ( unsigned int i = 0; i < childs.size(); i++){
+        traslate(i);
+        rotate(i);
+    }
+}

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/pieces/headers/caviglia.hpp

@@ -0,0 +1,21 @@
+#include "piece_interface.hpp"
+
+#ifndef CAVIGLIA_H
+#define CAVIGLIA_H
+
+
+class Caviglia : public PieceInterface{
+    private:
+        const sf::Vector3f caviglia_Dim = {60, 200, 60};
+        const sf::Color caviglia_Col = sf::Color(230,160,11,255);
+
+    public: 
+
+        Caviglia(rb::Vector3 coords, _Float16 mass);
+        ~Caviglia();
+
+        void update(sf::Clock cl, float multiplier) override;
+        sf::Shape* draw(ReferencePlane plane) override;
+};
+
+#endif

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/pieces/headers/coscia.hpp

@@ -0,0 +1,22 @@
+#include "piece_interface.hpp"
+
+#ifndef COSCIA_H
+#define COSCIA_H
+
+
+
+class Coscia : public PieceInterface{
+    private:
+        const sf::Vector3f coscia_Dim = {80, 200, 80};
+        const sf::Color coscia_Col = sf::Color::Yellow;
+        
+    public: 
+
+        Coscia(rb::Vector3 coords, _Float16 mass);
+        ~Coscia();
+
+        void update(sf::Clock cl, float multiplier) override;
+        sf::Shape* draw(ReferencePlane plane) override;
+};
+
+#endif

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/pieces/headers/joint_piece.hpp

@@ -0,0 +1 @@
+#include "piece_interface.hpp"

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/pieces/headers/piece_interface.hpp

@@ -0,0 +1,55 @@
+#include <SFML/Graphics.hpp>
+#include <math.h>
+#include "../../rigidbody/headers/rb.hpp"
+
+
+#ifndef PIECE_INTERFACE_H
+#define PIECE_INTERFACE_H
+
+
+enum class ReferencePlane {
+    XZ,
+    YZ,
+    XZN
+};
+
+enum class Direction {
+    L,
+    R
+};
+
+//classi
+class PieceInterface{
+    protected:
+        
+        void initialize_shapes(sf::Vector3f dim){
+            shapeXZ = new sf::RectangleShape({dim.x, dim.y});
+            shapeYZ = new sf::RectangleShape({dim.z, dim.y});
+            shapeXZ->setOrigin({dim.x/2,dim.y/2});
+            shapeYZ->setOrigin({dim.z/2,dim.y/2});
+            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, 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;
+        }
+};
+
+
+#endif

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/pieces/headers/sensore.hpp

@@ -0,0 +1,41 @@
+#include "piece_interface.hpp"
+
+#ifndef SENSORE_H
+#define SENSORE_H
+
+
+
+class Sensore : public PieceInterface{
+    private:
+        const sf::Vector3f sensore_Dim = {30, 60, 30};
+        const sf::Color sensore_Col = sf::Color::Red;
+        
+        std::vector<std::vector<float>> accData;
+        std::vector<std::vector<float>> gData;
+        std::vector<std::vector<float>> rotData;
+        std::vector<float> timeData;
+
+        float gModule;
+
+        //in che punto sto controllando il segnale 
+        unsigned int* dataPos;
+
+        //funzioni ausiliarie
+        void calcRotWithG(unsigned int index);
+        
+
+    public: 
+        Sensore(rb::Vector3 coords, _Float16 mass);
+        Sensore(rb::Vector3 coords, _Float16 mass, unsigned int* st, std::vector<std::vector<float>> data);
+        ~Sensore();
+
+        void update(sf::Clock cl,float multiplier) override;
+        sf::Shape* draw(ReferencePlane plane) override;
+
+        //funzioni specifiche
+        void initCSV(std::vector<std::vector<float>> data);
+
+        float getZ_Acc();
+};
+
+#endif

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/pieces/headers/torso.hpp

@@ -0,0 +1,21 @@
+#include "piece_interface.hpp"
+
+#ifndef TORSO_H
+#define TORSO_H
+
+
+
+
+class Torso : public PieceInterface{
+    private:
+        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, float multiplier) override;
+        sf::Shape* draw(ReferencePlane plane) override;
+};
+
+#endif

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/pieces/methods/caviglia_class.cpp

@@ -0,0 +1,54 @@
+#include "../headers/caviglia.hpp"
+
+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, caviglia_Dim.x/2);
+    color = caviglia_Col;
+    globalPos = {0,0,0};
+
+    initialize_shapes(caviglia_Dim);
+}
+
+
+Caviglia::~Caviglia(){
+    delete shapeXZ;
+    delete shapeYZ;
+}
+
+void Caviglia::update(sf::Clock cl,float multiplier){
+    //body.step(cl);
+}
+
+sf::Shape* Caviglia::draw(ReferencePlane plane){
+    
+    rb::Vector3 tmpPos = body.getPos();
+    rb::Vector3 tmpRot = body.getRot();
+
+    switch (plane)
+    {
+    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;
+    
+    case ReferencePlane::YZ:
+        {
+        sf::Shape* shape = shapeYZ;
+        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;
+    
+    default:
+        break;
+    }
+   
+    return nullptr;
+}

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/pieces/methods/coscia_class.cpp

@@ -0,0 +1,54 @@
+#include "../headers/coscia.hpp"
+
+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, coscia_Dim.z/2);
+    color = coscia_Col;
+    globalPos = {0,0,0};
+    initialize_shapes(coscia_Dim);
+}
+
+Coscia::~Coscia(){
+    delete shapeXZ;
+    delete shapeYZ;
+}
+
+void Coscia::update(sf::Clock cl, float multiplier){
+    //body.step(cl);
+}
+
+sf::Shape* Coscia::draw(ReferencePlane plane){
+    
+    rb::Vector3 tmpPos = body.getPos();
+    rb::Vector3 tmpRot = body.getRot();
+
+
+    switch (plane)
+    {
+    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;
+    
+    case ReferencePlane::YZ:
+        {
+        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));
+        shape->setScale({1,cos(float(tmpRot[1]))});
+        return shape;}
+        break;
+    
+    default:
+        break;
+    }
+
+   
+    return nullptr;
+}

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/pieces/methods/sensore_class.cpp

@@ -0,0 +1,133 @@
+#include "../headers/sensore.hpp"
+
+
+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, sensore_Dim.z/2);
+    color = sensore_Col;
+    globalPos = {0,0,0};
+    initialize_shapes(sensore_Dim);
+}
+
+Sensore::Sensore(rb::Vector3 coords, _Float16 mass, unsigned int* st, std::vector<std::vector<float>> data) : Sensore(coords, mass){
+        dataPos = st;
+        initCSV(data);
+}
+
+
+Sensore::~Sensore(){
+    delete shapeXZ;
+    delete shapeYZ;
+}
+
+void Sensore::initCSV(std::vector<std::vector<float>> data){
+    //timestamp_ns, wx, wy, wz, ax, ay, az, gx, gy, gz
+    if (data.size() < 10) throw "Sensor data empty";
+    float stTime = int64_t( data[0][0] ) ;
+
+    for (std::vector<float> row : data){
+        timeData.push_back(int64_t( row[0] ) - stTime);
+
+        std::vector<float> tmpR = {row[2],row[3],row[1]}; 
+        std::vector<float> tmpA = {row[5],row[6],row[4]};
+        std::vector<float> tmpG = {-row[8],-row[9],-row[7]};
+
+        
+        rotData.push_back(tmpR);
+        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, float multiplier){
+   
+    //calcolo la posizione e velocità
+    if (*dataPos >= gData.size()) *dataPos = gData.size()-1;
+
+    calcRotWithG(*dataPos);
+
+
+    body.setAcc(rb::Vector3{accData[*dataPos]});
+    body.step(cl, multiplier);
+   
+}
+
+sf::Shape* Sensore::draw(ReferencePlane plane){
+    
+    rb::Vector3 tmpRot = body.getRot();
+    rb::Vector3 tmpPos = body.getPos();
+
+    switch (plane)
+    {
+    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;
+    
+    case ReferencePlane::YZ:
+        {
+        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;
+    
+    default:
+        break;
+    }
+
+    return nullptr;
+}
+
+
+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];
+
+    //x = mod * cosX -> mod = x/cosx -> cosx = x/mod
+
+    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);
+    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

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/pieces/methods/torso.cpp

@@ -0,0 +1,52 @@
+#include "../headers/torso.hpp"
+
+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, torso_Dim.y/2);
+    color = torso_Col;
+    globalPos = {0,0,0};
+
+    initialize_shapes(torso_Dim);
+}
+
+Torso::~Torso(){
+    delete shapeXZ;
+    delete shapeYZ;
+}
+
+void Torso::update(sf::Clock cl,float multiplier){
+    
+}
+
+sf::Shape* Torso::draw(ReferencePlane plane){
+   
+    rb::Vector3 tmpRot = body.getRot();
+    rb::Vector3 tmpPos = body.getPos();
+
+
+    switch (plane)
+    {
+    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;
+    
+    case ReferencePlane::YZ:
+        {
+        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;
+    
+    default:
+        break;
+    }
+
+    return nullptr;
+}

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/rigidbody/headers/rb.hpp

@@ -0,0 +1,91 @@
+#include<math.h>
+#include<vector>
+#include <SFML/Graphics.hpp>
+
+#ifndef RB_H
+#define RB_H
+
+    namespace rb{
+        typedef std::vector<float> Vector3;
+        //typedef std::vector<_Float16> Vector3;
+
+        class rigidbody
+        {
+            private:
+                Vector3 vel = {0,0,0};
+                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};
+                Vector3 centerOfMass = {0,0,0};
+
+                int64_t prevT = 0;
+
+                //funzioni
+                void calcVel(const float Dtime);
+                void calcRot(const float Dtime);
+                void calcAcc(const Vector3 Dacc);
+                void calcTanAcc(const Vector3 Dacc);
+                void calcPos(const float Dtime);
+
+
+
+            public:
+                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 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);
+        };
+
+    }
+
+
+    
+    inline rb::Vector3 operator+(const rb::Vector3& v1, const rb::Vector3& v2) {
+            if (v1.size() != 3 || v2.size() != 3) {
+                throw std::invalid_argument("I vettori devono avere esattamente 3 elementi.");
+            }
+            return rb::Vector3{
+                v1[0] + v2[0],
+                v1[1] + v2[1],
+                v1[2] + v2[2]
+            };
+        }
+
+     inline rb::Vector3 operator-(const rb::Vector3& v1, const rb::Vector3& v2) {
+            if (v1.size() != 3 || v2.size() != 3) {
+                throw std::invalid_argument("I vettori devono avere esattamente 3 elementi.");
+            }
+            return rb::Vector3{
+                v1[0] - v2[0],
+                v1[1] - v2[1],
+                v1[2] - v2[2]
+            };
+        }
+    
+        /*
+        inline bool operator!=(const rb::Vector3& v1, const rb::Vector3& v2) {
+            if (v1.size() != 3 || v2.size() != 3) {
+                throw std::invalid_argument("I vettori devono avere esattamente 3 elementi.");
+            }
+            return true;
+        }*/
+#endif

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/rigidbody/methods/rb_class.cpp

@@ -0,0 +1,139 @@
+#include "../headers/rb.hpp"
+
+
+using namespace rb ;
+
+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";
+
+    this->coords = coords;
+    this->centerOfMass = centerOfMass;
+    this->mass = mass;
+    this->R = radius;
+}
+
+rigidbody::~rigidbody()
+{
+}
+
+Vector3 rigidbody::getPos(){
+    return Vector3 {coords};
+}
+Vector3 rigidbody::getRot(){
+    return Vector3 {rot};
+}
+
+void rigidbody::setPos(Vector3 Npos){
+    if (Npos.size() != 3) throw "Pos must be 3 in lenght!";
+
+    int i = 0;
+    for (float axis : Npos){
+        coords[i] = axis;
+        i++;
+    }
+
+}
+
+void rigidbody::setAcc(const Vector3 Nacc){
+    if (Nacc.size() != 3) throw "Acc vector must be 3 in lenght!";
+    
+    int i = 0;
+    for (float axis : Nacc){
+        acc[i] = axis;
+        i++;
+    }
+
+}
+
+void rigidbody::setRot(const Vector3 Nrot){
+    if (Nrot.size() != 3) throw "Vel vector must be 3 in lenght!";
+    
+    int i = 0;
+    for (float axis : Nrot){
+        rot[i] = axis;
+        i++;
+    }
+
+}
+
+void rigidbody::calcVel(const float Dtime){
+    Vector3 tmpVel;
+
+    for (float a : acc){
+        //if (a>0.8 || a<-0.8)
+        tmpVel.push_back( a*Dtime );
+    }
+
+    int i = 0;
+    for (float nv : tmpVel){
+        vel[i++] += nv;
+    }
+}
+
+void rigidbody::calcPos(const float Dtime){
+    Vector3 tmpPos;
+    
+    for (float v : vel){
+        tmpPos.push_back( v*Dtime );
+    }
+
+    int i = 0;
+    for (float np : tmpPos){
+        coords[i++] += np *100;//(np* cos(float(rot[i]))) *100;
+    }
+}
+
+void rigidbody::step(const sf::Clock time, float multiplier){
+    int64_t Dtime = time.getElapsedTime().asMicroseconds();
+    if (prevT == 0) prevT >= Dtime;
+
+
+    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);
+}

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/sfml_util.cpp

@@ -0,0 +1,406 @@
+#include <SFML/Graphics.hpp>
+#include <math.h>
+#include "pieces/headers/piece_interface.hpp"
+#include "joints/headers/joint_interface.hpp"
+#include "collections/headers/collection_interface.hpp"
+#include <imgui.h>
+#include <imgui-SFML.h>
+
+template <typename T1, typename T2>
+double dist(sf::Vector2<T1> p1, sf::Vector2<T2> p2)
+{
+    return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
+}
+
+
+////////////////////////////////////////////////////////////
+/// GUI state
+
+struct State
+{
+    sf::RenderWindow window;
+    int menubar_height = 50;
+    std::vector<PieceInterface*> pieces;
+    std::vector<JointInterface*> joints;
+    std::vector<CollectionInterface*> collections;
+
+    std::vector<collection> createdColl;
+
+    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;
+    sf::Vector2i mouse_pos;
+
+    /// per settare l'ntervallo di visualizzazione
+    unsigned int* pos ;
+    unsigned int* intervalMajLimit ;
+    unsigned int* intervalMinLimit ;
+    unsigned int maxEntries = 1000;
+    bool play = true;
+
+    State(unsigned w, unsigned h, std::string title, unsigned int* maj, unsigned int* min, unsigned int* pos) 
+    {
+        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; 
+        updateCollections();
+    }
+    void update();
+    void setIntervall(int n){
+        maxEntries = n;
+    }
+
+    void updateCollections(){
+        createdColl.clear();
+        for (auto c: collections){
+            createdColl.push_back(c->create(selectedPlane));
+        }
+    }
+    ~State(){
+        delete tMul;
+    }
+};
+
+///
+////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////
+/// Fisics functions
+
+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(PieceClock, *tMul);
+            }
+        }
+        for (auto j : i.joints){
+            j->movechild();
+        }
+    }
+
+    if (play){
+        for(PieceInterface* p : pieces){
+            p->update(PieceClock, *tMul);
+        }
+    }
+    for(JointInterface* j : joints){
+        j->movechild();
+    }
+    
+}
+
+///
+//////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////
+/// Callback functions
+void handle_close(State &gs)
+{
+    gs.window.close();
+}
+
+void handle_text(const sf::Event::TextEntered &textEnter, State &gs)
+{
+   
+}
+
+void handle_keyPressed(const sf::Event::KeyPressed &keyPressed, State &gs)
+{
+    if (keyPressed.scancode == sf::Keyboard::Scancode::Space){
+        if (gs.selectedPlane == ReferencePlane::XZ)
+            gs.selectedPlane = ReferencePlane::YZ;
+        else 
+            gs.selectedPlane = ReferencePlane::XZ;
+    }
+    gs.updateCollections();
+}
+
+
+void handle_mouseMove(const sf::Event::MouseMoved &mouseMoved, State &gs)
+{
+    sf::Vector2i offset = mouseMoved.position - gs.mouse_pos;
+    gs.mouse_pos = mouseMoved.position;
+    float px = 1;
+    float py = 0;
+    switch (gs.selectedPlane)
+    {
+    case ReferencePlane::XZ:
+        px = 1;
+        py = 0;
+        break;
+    case ReferencePlane::YZ:
+        px = 0;
+        py = 1;
+        break;
+    
+    default:
+        break;
+    }
+
+    if (gs.drag){
+        for(PieceInterface* p : gs.pieces){
+            p->globalPos = {p->globalPos[0] + (offset.x * px), p->globalPos[1]+ (offset.x * py),p->globalPos[2] + offset.y};
+        }
+        for(collection c : gs.createdColl){
+            for(auto p : c.pieces){
+                p->globalPos = {p->globalPos[0] + (offset.x * px), p->globalPos[1]+ (offset.x * py),p->globalPos[2] + offset.y};
+            }
+        }
+    }
+
+    #ifdef DEBUG_MODE
+
+    if (gs.selected != nullptr && gs.drag_Piece){
+        rb::Vector3 tmp = gs.selected->body.getPos();
+        gs.selected->body.setPos({tmp[0]+ (offset.x * px),tmp[1]+ (offset.x * py),tmp[2]+offset.y});
+    }
+    if (gs.selected != nullptr && gs.rot_Piece){
+        rb::Vector3 tmp = gs.selected->body.getRot();
+
+        float nrot = float(offset.x)/100;
+        gs.selected->body.setRot({tmp[0]+(nrot*py),tmp[1]+(nrot*px),tmp[2]});
+        
+        //printf("Rotation : %f,%f,%f \n",gs.pieces[gs.selected]->body.getRot()[0],gs.pieces[gs.selected]->body.getRot()[1],gs.pieces[gs.selected]->body.getRot()[2]);
+    }
+    #endif
+    
+}
+
+void handle_mousePressed(const sf::Event::MouseButtonPressed &mouseBP, State &gs)
+{
+    gs.mouse_pos = mouseBP.position;
+    if ( mouseBP.button == sf::Mouse::Button::Middle) gs.drag = true;
+
+    #ifdef DEBUG_MODE
+
+    if ( mouseBP.button == sf::Mouse::Button::Left){
+        gs.drag_Piece = true;
+
+        for (PieceInterface* p : gs.pieces){
+            sf::Vector2f pos;
+            if (gs.selectedPlane == ReferencePlane::XZ)
+             pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]};
+            else if (gs.selectedPlane == ReferencePlane::YZ)
+             pos = {p->globalPos[1]+ p->body.getPos()[1], p->globalPos[2]+ p->body.getPos()[2]};
+
+            if (dist(pos,mouseBP.position) < 20){
+                gs.selected = p;
+            }
+        }
+        for (collection c : gs.createdColl){
+            for (auto p : c.pieces){
+                sf::Vector2f pos;
+                if (gs.selectedPlane == ReferencePlane::XZ)
+                pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]};
+                else if (gs.selectedPlane == ReferencePlane::YZ)
+                pos = {p->globalPos[1]+ p->body.getPos()[1], p->globalPos[2]+ p->body.getPos()[2]};
+
+                if (dist(pos,mouseBP.position) < 20){
+                    gs.selected = p;
+                }
+                
+            }
+        }
+
+    }
+    if ( mouseBP.button == sf::Mouse::Button::Right){
+        gs.rot_Piece = true;
+        for (PieceInterface* p : gs.pieces){
+            sf::Vector2f pos;
+            if (gs.selectedPlane == ReferencePlane::XZ)
+            pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]};
+            else if (gs.selectedPlane == ReferencePlane::YZ)
+            pos = {p->globalPos[1]+ p->body.getPos()[1], p->globalPos[2]+ p->body.getPos()[2]};
+
+            if (dist(pos,mouseBP.position) < 20){
+                gs.selected = p;
+            }
+            
+        }
+        for (collection c : gs.createdColl){
+            for (auto p : c.pieces){
+                sf::Vector2f pos;
+                if (gs.selectedPlane == ReferencePlane::XZ)
+                pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]};
+                else if (gs.selectedPlane == ReferencePlane::YZ)
+                pos = {p->globalPos[1]+ p->body.getPos()[1], p->globalPos[2]+ p->body.getPos()[2]};
+
+                if (dist(pos,mouseBP.position) < 20){
+                    gs.selected = p;
+                }
+                
+            }
+        }
+    }
+    
+    #endif
+
+}
+
+void handle_mouseRelease(const sf::Event::MouseButtonReleased &, State &gs)
+{
+    gs.drag = false;
+    gs.drag_Piece = false;
+    gs.rot_Piece = false;
+    gs.selected = nullptr;
+}
+
+
+
+void handle_resize(const sf::Event::Resized &resized, State &gs)
+{
+    sf::FloatRect visibleArea({0.f, 0.f}, sf::Vector2f(resized.size));
+    gs.window.setView(sf::View(visibleArea));
+}
+
+
+///
+////////////////////////////////////////////////////////////
+
+
+////////////////////////////////////////////////////////////
+/// Graphics
+void doGUI(State &gs)
+{
+    // TODO: here code to display the menus
+    //Bottoni
+    sf::Time elapsed = gs.clock.restart();
+
+    unsigned int zero = 0;
+
+    ImGui::SFML::Update(gs.window, elapsed);
+    ImGui::ShowDemoWindow();
+
+    //Finestra gestione posizione nei dati
+    ImGuiWindowFlags sdp_flags = ImGuiWindowFlags_NoMove|
+                                 ImGuiWindowFlags_NoResize|
+                                 ImGuiWindowFlags_NoScrollbar|
+                                 ImGuiWindowFlags_NoCollapse|
+                                 ImGuiWindowFlags_NoTitleBar;
+   
+    ImGui::Begin("Set data position", 0,sdp_flags);
+    ImGui::SetNextItemWidth(ImGui::GetContentRegionAvail().x * 0.3);
+    ImGui::SliderScalar("Start", ImGuiDataType_U32 ,gs.intervalMinLimit,&zero,gs.intervalMajLimit);
+    ImGui::SameLine();
+    ImGui::SetNextItemWidth(ImGui::GetContentRegionAvail().x * 0.5);
+    if (ImGui::SliderScalar("Pos", ImGuiDataType_U32 ,gs.pos,gs.intervalMinLimit,gs.intervalMajLimit)){
+        gs.play = false;
+    }
+    ImGui::SameLine();
+    ImGui::SetNextItemWidth(ImGui::GetContentRegionAvail().x * 0.8);
+    ImGui::SliderScalar("End", ImGuiDataType_U32 ,gs.intervalMajLimit,gs.intervalMinLimit,&gs.maxEntries);
+    bool red = false;
+    if (!gs.play){
+        ImGui::PushStyleColor(ImGuiCol_Button, ImVec4(1,0,0,1));
+        red = true;
+    }
+    ImGui::SameLine();
+    if (ImGui::ArrowButton("Play", ImGuiDir_Right)){
+        gs.play = !gs.play;
+    }
+    if (red) ImGui::PopStyleColor();
+    ImGui::End();
+
+
+    //Finestra gestione piano visualizzazione
+    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.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);
+}
+
+void doGraphics(State &gs)
+{
+   
+    gs.window.clear();
+
+    //disegno i pezzi singoli
+    for(PieceInterface* p: gs.pieces){
+        gs.window.draw(*p->draw(gs.selectedPlane));
+    }
+    
+    //disegno le collezioni
+    for(auto c : gs.createdColl){
+        for(auto p : c.pieces){
+            gs.window.draw(*p->draw(gs.selectedPlane));
+        }
+    }
+
+    while (const std::optional event = gs.window.pollEvent()) {
+        ImGui::SFML::ProcessEvent(gs.window, *event);
+        if (event->is<sf::Event::Closed>())
+            handle_close (gs);
+        if (const auto* resized = event->getIf<sf::Event::Resized>())
+            handle_resize (*resized, gs);
+        if (const auto* pressed = event->getIf<sf::Event::KeyPressed>())
+            handle_keyPressed (*pressed, gs);
+        if (const auto* moved = event->getIf<sf::Event::MouseMoved>())
+            handle_mouseMove (*moved, gs);   
+        if (const auto* mpressed = event->getIf<sf::Event::MouseButtonPressed>())
+            handle_mousePressed (*mpressed, gs); 
+        if (const auto* mreleased = event->getIf<sf::Event::MouseButtonReleased>())
+            handle_mouseRelease (*mreleased, gs);  
+    }
+    doGUI(gs);
+
+  
+    
+    // TODO: add here code to display shapes in your canvas
+
+    gs.window.display();
+}
+///
+////////////////////////////////////////////////////////////

RELEASES/FCG_VisualizzatoreCamminata-0.9/src/testMain.cpp

@@ -0,0 +1,122 @@
+#include "include.hpp"
+
+int main() {
+    CSVProcessor processor;
+    try {
+        processor.readCSVFile("data.csv");
+
+        // Access headers
+        const auto& headers = processor.getHeaders();
+        for (const auto& header : headers) {
+            std::cout << header << "\t";
+        }
+        std::cout << std::endl;
+
+        // Access data
+        int n = 0;
+        const auto& data = processor.getData();
+        for (const auto& row : data) {
+            if (n++ >40) break;
+            for (float value : row) {
+                std::cout << value << "\t";
+            }
+            std::cout << std::endl;
+        }
+    } catch (const std::exception& e) {
+        std::cerr << "Error: " << e.what() << std::endl;
+    }
+
+    // inizializzo variabili per gestire l'intervallo di visualizzazione
+
+    unsigned int min = 0;
+    unsigned int pos = 0;
+    unsigned int maj = 100;
+
+    //Costruisco la GUI 
+    State gs(800, 700, "Visualizzatore passo",&maj,&min,&pos);
+    gs.window.setFramerateLimit(70);
+    printf("Costruisco gli oggetti\n");
+
+    try{
+        
+        processor.readCSVFile (DATA_PATH + "coscia_filt.csv");
+        const auto& coscia = processor.getData();
+        gs.setIntervall(coscia.size());
+
+        /*        
+        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));
+
+        gs.pieces[1]->body.setRot({0,0,0});
+
+
+        processor.readCSVFile(DATA_PATH + "caviglia_filt.csv");
+        const auto& caviglia = processor.getData();
+        gs.pieces.push_back(new Sensore (rb::Vector3{300,300,500},_Float16( 0.2 ),&pos,caviglia));
+
+        gs.pieces.push_back(new Torso(rb::Vector3{300,400,150},2));
+
+        // modifico la rotazione relativa della gamba
+        gs.pieces[1]->body.setRot({_Float16 (1.3),_Float16 (1.7),0});
+        gs.pieces[3]->body.setRot({_Float16 (1.8),_Float16 (1.7),0});
+
+
+
+        // aggiungo i joint
+        gs.joints.push_back(new PivotJoint(gs.pieces[4], {gs.pieces[1]}, rb::Vector3{0,-100,50}));
+        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;
+        d.cavigliaData = "caviglia_dx.csv";
+        d.cosciaData = "coscia_dx.csv";
+        gamba_data s;
+        s.dataPos = &pos;
+        s.cavigliaData = "caviglia_sx.csv";
+        s.cosciaData = "coscia_sx.csv";
+        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){
+        printf("%s\n",e);
+    }
+    printf("Avvio l'interfaccia grafica\n");
+
+
+    unsigned int curTime = 0;
+    unsigned int freq = 50;
+    const unsigned int T = 1000/freq; //i sensori hanno una freq di campionamento di 50hz
+
+    //Avvio il loop della GUI
+    gs.clock.start();
+    gs.updateCollections();
+
+    sf::Clock mainClock;
+    while (gs.window.isOpen()) 
+    {
+        curTime += mainClock.restart().asMilliseconds() *(*gs.tMul) ;
+        if (curTime > T){
+            if (gs.play && pos+curTime/T < maj) pos += curTime / T; 
+            curTime = 0;
+        }
+
+        // Show update
+        gs.update();
+        doGraphics(gs);
+    }
+    
+    return 0;
+}
+