Rimosso file .lock

.gitignore

@@ -10,6 +10,10 @@ 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

README.md

@@ -1,42 +1,31 @@
 # FCG_VisualizzatoreCamminata
 
+## Per compilare:
 
-## Nella versione versione v0.1 è presente la base del progetto.
+    cd ./RELEASES
+    mkdir build //se non esiste già
+    cd build
+    cmake ..
+    cmake --build . -j$(nproc)
 
-- La gerarchia dei file e delle classi
+## Per lanciare:
 
-    - 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)
+    cd bin
+    ./build/bin/mainV1 
+    
+    //Per eseguire le altre release cambiare mainV1 con mainV(1 - 11)
 
 
-# Per compliare:
+## Come utilizzare il software:
+Sono stati inseriti dei dati di default per testare, non è necessario caricare altro per lanciare.
 
-    cmake --build
+Per modificare i dati da visualizzare bisogna modificare il nome dei file all'interno del main **(non è supportato l'aggiornamento dei dati a runtime)**
 
-# Per lanciare:
+Per muoversi nella scena:
+ - Cliccare e tenere premuto tasto centrale del mouse.
 
-    ./build/bin/mainV4
+Per le interazioni con la GUI:
+ - Tutte le interazioni avvengono tramite cursore del mouse
+   - per gli slider: click con tasto sinistro del mouse e trascinare 
+   - per i checkbox basta click sulla spunta/rettangolino
+   - per il tasto pause/resume (in basso a destra), click con tasto sinistro. Se il tasto è colorato di rosso vuol dire che la riproduzione è in pausa. 

RELEASES/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.28)
+project(AllReleases LANGUAGES CXX)
+
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
+
+# 1. Identifichiamo tutte le cartelle che rappresentano una release.
+# Usiamo il pattern che identifica le tue cartelle (es. FCG_VisualizzatoreCamminata-*)
+file(GLOB RELEASE_DIRS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} "FCG_VisualizzatoreCamminata-*")
+
+message(STATUS "Trovate ${RELEASE_DIRS} release da compilare.")
+
+# 2. Cicliamo su ogni cartella trovata
+foreach(RELEASE_DIR ${RELEASE_DIRS})
+    # Verifichiamo che esista un file CMakeLists.txt all'interno della cartella
+    if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${RELEASE_DIR}/CMakeLists.txt")
+        message(STATUS "Aggiunta release: ${RELEASE_DIR}")
+        
+        # Aggiungiamo la sottocartella al progetto principale.
+        # CMake gestirà la compilazione di ogni singola release come parte di questo unico progetto.
+        add_subdirectory(${RELEASE_DIR})
+    else()
+        message(WARNING "Saltata cartella ${RELEASE_DIR}: CMakeLists.txt non trovato.")
+    endif()
+endforeach()

RELEASES/FCG_VisualizzatoreCamminata-0.1/.gitignore

@@ -0,0 +1,19 @@
+CMakeLists.txt.user
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Testing
+Makefile
+cmake_install.cmake
+install_manifest.txt
+compile_commands.json
+CTestTestfile.cmake
+_deps
+CMakeUserPresets.json
+
+# 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.1/CMakeLists.txt

@@ -0,0 +1,23 @@
+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)
+
+set(METHODS_PATH "./src/*/methods/*.cpp")
+
+#V1
+set(VERSION "V1")
+
+file(GLOB_RECURSE METHODS_SRC "${METHODS_PATH}")
+add_executable(main${VERSION} ./src/testMain.cpp ${METHODS_SRC} )
+target_compile_features(main${VERSION} PRIVATE cxx_std_17)
+target_link_libraries(main${VERSION} PRIVATE SFML::Graphics)

RELEASES/FCG_VisualizzatoreCamminata-0.1/README.md

@@ -0,0 +1,26 @@
+# 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.
+
+Per compliare:
+
+    cmake --build
+
+Per lanciare:
+
+    ./build/bin/mainV1

RELEASES/FCG_VisualizzatoreCamminata-0.1/src/fis.hpp

@@ -0,0 +1,44 @@
+#include<math.h>
+#include"rb_class.cpp"
+#include<time.h>
+
+/*
+Questo namespace deve contenere le funzioni di gestione della fisica del motore.
+Ogni moto in questo engine sarà ti tipo uniformemente accelerato per dare una semplificazione della realtà.
+Le funzioni necessarie sono:
+
+- Calcolo velocità, richiede: 
+    (Oggetto rigidbody)
+    (time di partenza, time di arrivo o delta t)
+    (Accelerazione media)
+    (velocità iniziale)
+
+- Calcolo rotazione, richiede:
+    (Oggetto rigidbody)
+    (time di partenza, time di arrivo o delta t)
+    (Accelerazione tangenziale)
+    (RAD/s iniziale)
+
+- Calcolo accelerazione, deve modificare i valori di accelerazione su oggetti di tipo rigidbody, richiede:
+    (Oggetto rigidbody)
+
+- Calcolo posizione, deve calcolare la posizione di un rigidbody in in un intervallo di tempo
+
+- Calcolo energia potenziale
+
+- Calcolo inerzia 
+
+- Calcolo energia meccanica 
+
+*/
+using namespace rb;
+
+namespace fis{
+    
+    void calcVel(rigidbody body, const time_t Dtime);
+    void calcRot(rigidbody body, const time_t Dtime);
+    void calcAcc(rigidbody body, const std::vector<float> Dacc);
+    void calcTanAcc(rigidbody body, const std::vector<float> Dacc);
+    void calcPos(rigidbody body, const time_t Dtime);
+
+}

RELEASES/FCG_VisualizzatoreCamminata-0.1/src/include.hpp

@@ -0,0 +1,7 @@
+#include <iostream>
+#include "csv/headers/csv.hpp"
+#include "sfml_util.cpp"
+#include "pieces/headers/coscia.hpp"
+#include "pieces/headers/sensore.hpp"
+#include "joints/headers/rigid_joint.hpp"
+#include "joints/headers/pivot_joint.hpp"

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

@@ -0,0 +1,33 @@
+#include "../../pieces/headers/piece_interface.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_s> rotOffset;
+        PieceInterface* father;
+        std::vector<PieceInterface*> childs;
+
+        virtual ~JointInterface(){};
+        virtual void movechild() = 0;
+
+        
+};
+
+#endif

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

@@ -0,0 +1,22 @@
+#include "joint_interface.hpp"
+
+class PivotJoint : public JointInterface {
+    protected:
+        void rotate(unsigned int id) override;
+        void traslate(unsigned int id) override;
+
+        rb::Vector3_s oldRot;
+        rb::Vector3 pivot;
+        std::vector<rb::Vector3_s> oldCRot;
+
+        //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();
+};

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

@@ -0,0 +1,111 @@
+#include "../headers/pivot_joint.hpp"
+
+#define ZERO_INT 0.00001
+
+void PivotJoint::rotate(unsigned int id){
+    rb::Vector3_s fRot = father->body.getRot();
+    rb::Vector3_s cRot = childs[id]->body.getRot();
+    
+    ////  sposto l'origine passivamente su tutti gli assi ////
+
+    float r1 = sqrt(pow(pivot[0],2)+pow(pivot[2],2)); //calcolo modulo dell'offset (per ora solo sul piano xz)
+    float r2 = sqrt(pow(offset[id][0],2)+pow(offset[id][2],2)); 
+    if (r1>ZERO_INT){
+        
+        float sign = pivot[2] >= 0 ? 1 : -1;
+
+
+        sf::Angle alpha = sf::radians(fRot[2] - oldRot[2]); // angolo aggiunto
+        sf::Angle alpha1 = sf::radians(acos(sign * pivot[0]/r1)); // angolo rispetto alla posizione del pivot 
+        sf::Angle alpha2 = alpha + alpha1;
+
+        sf::Vector2f tmpCoordsX = sf::Vector2f(r1,alpha2);
+        pivot = {sign * tmpCoordsX.x,pivot[1],sign * tmpCoordsX.y};
+
+        //calcolo la posizione in base alla rotazione del child 
+        sign = offset[id][2] >= 0 ? 1 : -1;
+
+        sf::Angle beta = sf::radians(cRot[2] - oldCRot[id][2]);
+        sf::Angle beta1 = sf::radians(acos(sign * offset[id][0]/r2));
+        sf::Angle beta2 = beta + beta1;
+        sf::Vector2f tmpCoordsC = sf::Vector2f(r2,beta2);
+        offset[id] = {sign * tmpCoordsC.x,offset[id][1],sign * tmpCoordsC.y};
+
+
+        //ora devo muovere il child rispetto al nuovo offset 
+        rb::Vector3 pivotPos = father->body.getPos()+father->globalPos+pivot;
+        rb::Vector3 cPos = childs[id]->body.getPos() + childs[id]->globalPos;
+
+        rb::Vector3 tmpChild =  pivotPos + offset[id];
+
+        childs[id]->body.setPos(tmpChild - childs[id]->globalPos); 
+    }
+
+
+    oldRot = fRot; //aggiorno la rotazione per il ciclo successivo
+    oldCRot[id] = cRot;
+
+    // r cosA = x -> x/r = cosA
+
+    /*
+    Devo spostare l'offset per poter ricalcolare la posizione relativa dei child rispetto al father dopo aver eseguito la rotazione.
+    La rotazione va eseguita nella posizione del mondo, ovvero sulle coordinate di body
+
+    Le coordinate camera non vanno toccate, determinano solo lo spostamento rispetto alla telecamera
+    */
+}   
+
+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_s fRot = father->body.getRot();
+
+    pivot = pivotPoint;
+    rb::Vector3 pivotCenter = father->globalPos + father->body.getPos() + pivot;
+
+    /*
+    float sign = pivot[2] >= 0 ? 1 : -1;
+    float r = sqrt(pow(pivot[0],2)+pow(pivot[2],2));
+    rotOffset.push_back(  rb::Vector3_s{0,0,_Float16( acos(sign * pivot[0]/r) )} );
+    */
+
+
+    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->globalPos + c->body.getPos();
+        tmpCoords = cCoords - pivotCenter;
+
+        /*
+        float r = sqrt(pow(tmpCoords[0],2)+pow(tmpCoords[2],2));
+        oldCRot.push_back( rb::Vector3_s{0,0,_Float16( acos(tmpCoords[0]/r) )} );
+        */
+        oldCRot.push_back(c->body.getRot());
+
+        offset.push_back(tmpCoords);
+        
+    }
+
+
+}
+
+PivotJoint::~PivotJoint(){
+
+}
+
+
+void PivotJoint::movechild(){
+   
+    for ( unsigned int i = 0; i < childs.size(); i++){
+        traslate(i);
+        rotate(i);
+    }
+}

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

@@ -0,0 +1,99 @@
+#include "../headers/rigid_joint.hpp"
+
+#define ZERO_INT 0.00001
+
+void RigidJoint::rotate(unsigned int id){
+    rb::Vector3_s fRot = father->body.getRot();
+    rb::Vector3_s fRotOld = childs[id]->body.getRot() - rotOffset[id];
+
+    rb::Vector3 cPos = childs[id]->body.getPos() + childs[id]->globalPos;
+
+    childs[id]->body.setRot(fRot + rotOffset[id]);
+
+    ////  sposto l'origine passivamente su tutti gli assi ////
+    //se si muove il child devo muovere anche il padre -> devo trovare la differenza di posizione prima di ricalcolare l'offset
+    /*
+    if (cPos != oldCPos[id]);*/
+
+    float r = sqrt(pow(offset[id][0],2)+pow(offset[id][2],2)); //calcolo modulo dell'offset (per ora solo sul piano xz)
+
+    if (r>ZERO_INT){
+        
+        float sign = offset[id][2] >= 0 ? 1 : -1;
+        
+
+        sf::Angle alpha = sf::radians(fRot[2] - fRotOld[2]); // angolo aggiunto
+        sf::Angle alpha1 = sf::radians(acos(sign * offset[id][0]/r)); // angolo rispetto alla posizione del child 
+        sf::Angle beta = alpha + alpha1;
+
+        sf::Vector2f tmpCoordsX = sf::Vector2f(r,beta);
+        offset[id] = {sign * tmpCoordsX.x,offset[id][1],sign * tmpCoordsX.y};
+
+        //ora devo muovere il child rispetto al nuovo offset 
+
+        
+
+        rb::Vector3 fPos = father->body.getPos() + father->globalPos;
+        rb::Vector3 tmpChild =  fPos + offset[id];
+
+        childs[id]->body.setPos(tmpChild - childs[id]->globalPos); 
+    }
+    else{
+        childs[id]->body.setPos(father->body.getPos()+father->globalPos-childs[id]->globalPos);
+    }
+
+
+    // r cosA = x -> x/r = cosA
+
+    /*
+    Devo spostare l'offset per poter ricalcolare la posizione relativa dei child rispetto al father dopo aver eseguito la rotazione.
+    La rotazione va eseguita nella posizione del mondo, ovvero sulle coordinate di body
+
+    Le coordinate camera non vanno toccate, determinano solo lo spostamento rispetto alla telecamera
+    */
+}   
+
+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_s fRot = father->body.getRot();
+
+
+
+
+    //mi calcolo l'offset per ogni child rispetto al padre
+    for(PieceInterface* c : childs){
+        rb::Vector3 tmpCoords;
+        rb::Vector3_s 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.1/src/pieces/headers/coscia.hpp

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

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

@@ -0,0 +1,40 @@
+#include <SFML/Graphics.hpp>
+#include <math.h>
+#include "../../rigidbody/headers/rb.hpp"
+//#include "../../joints/headers/joint_interface.hpp"
+
+#ifndef PIECE_INTERFACE_H
+#define PIECE_INTERFACE_H
+
+
+// costanti
+
+const sf::Vector2f caviglia_Dim = {50, 200};
+const sf::Color caviglia_Col = sf::Color::Red;
+
+enum class ReferencePlane {
+    XY,
+    YZ,
+    XZ
+};
+
+//classi
+class PieceInterface{
+    protected:
+        
+        //std::vector<JointInterface*> joints;
+
+    public:
+        sf::Shape* shape;
+        sf::Vector2f size;
+        rb::Vector3 globalPos;
+        rb::rigidbody body;
+        sf::Color color;
+
+        virtual void update(sf::Clock cl) = 0;
+        virtual sf::Shape* draw(ReferencePlane plane) = 0;
+        virtual ~PieceInterface(){}
+};
+
+
+#endif

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

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

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

@@ -0,0 +1,40 @@
+#include "../headers/coscia.hpp"
+
+Coscia::Coscia(rb::Vector3 coords, _Float16 mass){
+    size = coscia_Dim;
+    rb::Vector3 com = {size.x/2,0, size.y/2};
+    body = rb::rigidbody(coords, com, mass);
+    color = coscia_Col;
+    shape = new sf::RectangleShape(size);
+    shape->setOrigin({size.x/2,size.y/2});
+    globalPos = {0,0,0};
+}
+
+Coscia::~Coscia(){
+    delete shape;
+}
+
+void Coscia::update(sf::Clock cl){
+    //body.step(cl);
+}
+
+sf::Shape* Coscia::draw(ReferencePlane plane){
+    shape->setFillColor(color);
+    rb::Vector3 tmpPos = body.getPos();
+    rb::Vector3_s tmpRot = body.getRot();
+
+    switch (plane)
+    {
+    case ReferencePlane::XZ:
+        shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[2])));
+        break;
+    
+
+    default:
+        break;
+    }
+
+   
+    return shape;
+}

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

@@ -0,0 +1,102 @@
+#include "../headers/sensore.hpp"
+
+
+Sensore::Sensore(rb::Vector3 coords, _Float16 mass){
+    size = sensore_Dim;
+    rb::Vector3 com = {size.x/2,0, size.y/2};
+    body = rb::rigidbody({0,0,0}, com, mass);
+    color = sensore_Col;
+    shape = new sf::RectangleShape(size);
+    globalPos = coords;
+}
+
+Sensore::Sensore(rb::Vector3 coords, _Float16 mass, unsigned int st, unsigned int dataIntvl, std::vector<std::vector<float>> data) : Sensore(coords, mass){
+        dataPos = st;
+        this->dataIntvl = dataIntvl;
+        initCSV(data);
+}
+
+
+Sensore::~Sensore(){
+    delete shape;
+}
+
+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";
+    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);
+    }
+}
+
+
+void Sensore::update(sf::Clock cl){
+    // Aggiorno la posizione nei dati
+    int64_t currTime = cl.getElapsedTime().asMicroseconds() *100000;
+    if (timeData[dataPos] < currTime && dataIntvl - dataPos > 0) { //aggiorno solo se ho cambiato posizione
+        dataPos++;
+
+        //calcolo la posizione e velocità
+        calcRotWithG(dataPos);
+        body.setAcc(rb::Vector3{accData[dataPos]});
+        body.step(cl);
+    }
+
+}
+
+sf::Shape* Sensore::draw(ReferencePlane plane){
+    shape->setFillColor(color);
+
+    shape->setOrigin({sensore_Dim.x/2, sensore_Dim.y/2});
+    
+    rb::Vector3_s tmpRot = body.getRot();
+    
+
+    rb::Vector3 tmpPos = body.getPos();
+
+    switch (plane)
+    {
+    case ReferencePlane::XZ:
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[2])));
+        shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
+        break;
+    
+    default:
+        break;
+    }
+    
+
+    
+
+    return shape;
+}
+
+
+void Sensore::calcRotWithG(unsigned int index){ // calcolo rotazione con valori della gravità
+
+    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 tmpAX = acos(tmpSinY);
+    float tmpAY = acos(tmpSinZ);
+    float tmpAZ = acos(tmpSinX);
+
+    body.setRot(rb::Vector3_s{_Float16( tmpAX),_Float16( tmpAY),_Float16( tmpAZ) });
+
+}

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

@@ -0,0 +1,108 @@
+#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_s;
+
+        class rigidbody
+        {
+            private:
+                Vector3 vel = {0,0,0};
+                Vector3 acc = {0,0,0};
+                Vector3_s rot = {0,0,0};
+                Vector3 tanAcc = {0,0,0};
+
+                _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 time_t 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);
+                ~rigidbody();
+
+                
+                Vector3 getPos();
+                Vector3_s getRot();
+                void setPos(const Vector3 Npos);
+                void setRot(const Vector3_s Nrot);
+                void setVel(const Vector3 Nacc);
+                void setAcc(const Vector3 Nvel);
+                void step(const sf::Clock time);
+
+                //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 rb::Vector3_s operator+(const rb::Vector3_s& v1, const rb::Vector3_s& v2) {
+            if (v1.size() != 3 || v2.size() != 3) {
+                throw std::invalid_argument("I vettori devono avere esattamente 3 elementi.");
+            }
+            return rb::Vector3_s{
+                v1[0] + v2[0],
+                v1[1] + v2[1],
+                v1[2] + v2[2]
+            };
+        }
+
+     inline rb::Vector3_s operator-(const rb::Vector3_s& v1, const rb::Vector3_s& v2) {
+            if (v1.size() != 3 || v2.size() != 3) {
+                throw std::invalid_argument("I vettori devono avere esattamente 3 elementi.");
+            }
+            return rb::Vector3_s{
+                v1[0] - v2[0],
+                v1[1] - v2[1],
+                v1[2] - v2[2]
+            };
+        }
+        /*
+        inline bool operator!=(const rb::Vector3_s& v1, const rb::Vector3_s& 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.1/src/rigidbody/methods/rb_class.cpp

@@ -0,0 +1,100 @@
+#include "../headers/rb.hpp"
+
+
+using namespace rb ;
+
+rigidbody::rigidbody(Vector3 coords, Vector3 centerOfMass, _Float16 mass)
+{
+    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;
+
+}
+
+rigidbody::~rigidbody()
+{
+}
+
+Vector3 rigidbody::getPos(){
+    return Vector3 {coords};
+}
+Vector3_s rigidbody::getRot(){
+    return Vector3_s {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 "Vel vector must be 3 in lenght!";
+    
+    int i = 0;
+    for (float axis : Nacc){
+        acc[i] = axis;
+        i++;
+    }
+
+}
+
+void rigidbody::setRot(const Vector3_s 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){
+    int64_t Dtime = time.getElapsedTime().asMicroseconds();
+    if (prevT == 0) prevT = Dtime;
+
+
+    float dt = (float(Dtime) / 1000000.0) - (float(prevT) / 1000000.0);
+    prevT = Dtime;
+
+    calcPos(dt);
+    calcVel(dt);
+}
+    
+

RELEASES/FCG_VisualizzatoreCamminata-0.1/src/sfml_util.cpp

@@ -0,0 +1,184 @@
+#include <SFML/Graphics.hpp>
+#include <math.h>
+#include "pieces/headers/piece_interface.hpp"
+#include "joints/headers/joint_interface.hpp"
+
+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;
+    sf::Vector2f cameraOffset = {0.,0.};
+
+    sf::Clock clock;
+
+    int selected = -1;
+
+    bool rot_Piece = false;
+    bool drag_Piece = false;
+    bool drag = false;
+    sf::Vector2i mouse_pos;
+
+    State(unsigned w, unsigned h, std::string title) 
+    {
+        window = sf::RenderWindow(sf::VideoMode({w, h}), title);
+        clock.restart();
+    }
+    void update();
+};
+
+///
+////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////
+/// Fisics functions
+
+void State::update(){
+    
+
+    for(PieceInterface* p : pieces){
+        p->update(clock);
+    }
+    for(JointInterface* j : joints){
+        j->movechild();
+    }
+}
+
+///
+//////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////
+/// Callback functions
+void handle(const sf::Event::Closed &, State &gs)
+{
+    gs.window.close();
+}
+
+void handle(const sf::Event::TextEntered &textEnter, State &gs)
+{
+   
+}
+
+void handle(const sf::Event::KeyPressed &keyPressed, State &gs)
+{
+}
+
+void handle(const sf::Event::MouseMoved &mouseMoved, State &gs)
+{
+    sf::Vector2i offset = mouseMoved.position - gs.mouse_pos;
+    gs.mouse_pos = mouseMoved.position;
+    if (gs.drag){
+        for(PieceInterface* p : gs.pieces){
+            p->globalPos = {p->globalPos[0] + offset.x, p->globalPos[1],p->globalPos[2] + offset.y};
+
+            /// Devo spostare sul piano di visualizzazione
+            /// Quindi dovrò settare una variabile che mi definisce qual è il piano preso in considerazione, questo sarà nello state
+        }
+    }
+    if (gs.selected != -1 && gs.drag_Piece){
+        rb::Vector3 tmp = gs.pieces[gs.selected]->body.getPos();
+        gs.pieces[gs.selected]->body.setPos({tmp[0]+offset.x,tmp[1],tmp[2]+offset.y});
+    }
+    if (gs.selected != -1 && gs.rot_Piece){
+        rb::Vector3_s tmp = gs.pieces[gs.selected]->body.getRot();
+
+        _Float16 nrot = _Float16(offset.x)/10;
+        gs.pieces[gs.selected]->body.setRot({tmp[0],tmp[1],tmp[2]+nrot});
+    }
+
+    
+}
+
+void handle(const sf::Event::MouseButtonPressed &mouseBP, State &gs)
+{
+    gs.mouse_pos = mouseBP.position;
+    if ( mouseBP.button == sf::Mouse::Button::Middle) gs.drag = true;
+    if ( mouseBP.button == sf::Mouse::Button::Left){
+        gs.drag_Piece = true;
+        int i = 0;
+        for (PieceInterface* p : gs.pieces){
+            
+            sf::Vector2f pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]};
+
+            if (dist(pos,mouseBP.position) < 20){
+                gs.selected = i;
+                break;
+            }
+            i++;
+        }
+    }
+    if ( mouseBP.button == sf::Mouse::Button::Right){
+        gs.rot_Piece = true;
+        int i = 0;
+        for (PieceInterface* p : gs.pieces){
+            
+            sf::Vector2f pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]};
+
+            if (dist(pos,mouseBP.position) < 20){
+                gs.selected = i;
+                break;
+            }
+            i++;
+        }
+    }
+
+}
+
+void handle(const sf::Event::MouseButtonReleased &, State &gs)
+{
+    gs.drag = false;
+    gs.drag_Piece = false;
+    gs.rot_Piece = false;
+    gs.selected = -1;
+}
+
+void handle(const sf::Event::Resized &resized, State &gs)
+{
+    sf::FloatRect visibleArea({0.f, 0.f}, sf::Vector2f(resized.size));
+    gs.window.setView(sf::View(visibleArea));
+}
+
+template <typename T>
+void handle(const T &, State &gs)
+{
+    // All unhandled events will end up here
+}
+///
+////////////////////////////////////////////////////////////
+
+
+////////////////////////////////////////////////////////////
+/// Graphics
+void doGUI(State &gs)
+{
+    // TODO: here code to display the menus
+    //Bottoni
+    
+}
+
+void doGraphics(State &gs)
+{
+    gs.window.clear();
+    doGUI(gs);
+
+    for(PieceInterface* p: gs.pieces){
+        gs.window.draw(*p->draw(ReferencePlane::XZ));
+    }
+    
+    // TODO: add here code to display shapes in your canvas
+
+    gs.window.display();
+}
+///
+////////////////////////////////////////////////////////////

RELEASES/FCG_VisualizzatoreCamminata-0.1/src/testMain.cpp

@@ -0,0 +1,83 @@
+#include "include.hpp"
+
+#define DATA_PATH std::string("./../../data/")
+
+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;
+    }
+
+    //Costruisco la GUI 
+    State gs(800, 600, "Visualizzatore passo");
+    gs.window.setFramerateLimit(60);
+    printf("Costruisco gli oggetti\n");
+
+    try{
+        processor.readCSVFile (DATA_PATH + "coscia_filt.csv");
+        const auto& coscia = processor.getData();
+
+
+        gs.pieces.push_back(new Coscia (rb::Vector3{300,10,300},2));
+        gs.pieces.push_back(new Sensore (rb::Vector3{300,300,300},_Float16( 0.2 ),900,3000,coscia));
+        gs.pieces.push_back(new Coscia (rb::Vector3{300,10,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,700,500},_Float16( 0.2 ),900,3000,caviglia));
+        
+        // modifico la rotazione relativa della gamba
+        gs.pieces[1]->body.setRot({0,0,_Float16 (1.6)});
+        gs.pieces[3]->body.setRot({0,0,_Float16 (1.7)});
+
+
+        // aggiungo i joint
+
+        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]}));
+
+        printf("Ho costruito tutto!\n");
+    }
+    catch(char* e){
+        printf("%s\n",e);
+    }
+    printf("Avvio l'interfaccia grafica\n");
+    //Avvio il loop della GUI
+    gs.clock.start();
+    while (gs.window.isOpen()) 
+    {
+        // event loop and handler through callbacks
+        gs.window.handleEvents([&](const auto &event)
+                               { handle(event, gs); });
+        // Show update
+        gs.update();
+        doGraphics(gs);
+    }
+    
+    return 0;
+}
+

RELEASES/FCG_VisualizzatoreCamminata-0.2/.gitignore

@@ -0,0 +1,19 @@
+CMakeLists.txt.user
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Testing
+Makefile
+cmake_install.cmake
+install_manifest.txt
+compile_commands.json
+CTestTestfile.cmake
+_deps
+CMakeUserPresets.json
+
+# 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.2/CMakeLists.txt

@@ -0,0 +1,31 @@
+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)
+
+
+set(METHODS_PATH "./src/*/methods/*.cpp")
+
+
+set(VERSION "V2")
+
+file(GLOB_RECURSE METHODS_SRC "${METHODS_PATH}")
+add_executable(main${VERSION} ./src/testMain.cpp ${METHODS_SRC} )
+target_compile_features(main${VERSION} PRIVATE cxx_std_17)
+target_link_libraries(main${VERSION} PRIVATE SFML::Graphics glm)

RELEASES/FCG_VisualizzatoreCamminata-0.2/README.md

@@ -0,0 +1,31 @@
+# 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) 
+
+Per compliare:
+
+    cmake --build
+
+Per lanciare:
+
+    ./build/bin/mainV2

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

@@ -0,0 +1,20 @@
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <string>
+
+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;
+
+};

RELEASES/FCG_VisualizzatoreCamminata-0.2/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.2/src/include.hpp

@@ -0,0 +1,8 @@
+#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 "joints/headers/rigid_joint.hpp"
+#include "joints/headers/pivot_joint.hpp"

RELEASES/FCG_VisualizzatoreCamminata-0.2/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_s> rotOffset;
+        PieceInterface* father;
+        std::vector<PieceInterface*> childs;
+
+        virtual ~JointInterface(){};
+        virtual void movechild() = 0;
+
+        
+};
+
+#endif

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

@@ -0,0 +1,22 @@
+#include "joint_interface.hpp"
+
+class PivotJoint : public JointInterface {
+    protected:
+        void rotate(unsigned int id) override;
+        void traslate(unsigned int id) override;
+
+        rb::Vector3_s oldRot;
+        rb::Vector3 pivot;
+        std::vector<rb::Vector3_s> oldCRot;
+
+        //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();
+};

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

@@ -0,0 +1,19 @@
+#include "joint_interface.hpp"
+
+
+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();
+};

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

@@ -0,0 +1,99 @@
+#include "../headers/pivot_joint.hpp"
+
+#define ZERO_INT 0.00001
+
+void PivotJoint::rotate(unsigned int id){
+    rb::Vector3_s fRot = father->body.getRot();
+    rb::Vector3 fPos = father->body.getPos();
+    rb::Vector3_s cRot = childs[id]->body.getRot();
+    
+    ////  sposto l'origine passivamente su tutti gli assi ////
+
+    float alpha = float (fRot[2] - oldRot[2]);
+    float cosA = glm::cos(alpha);
+    float sinA = glm::sin(alpha);
+
+    float beta = float (cRot[2] - oldCRot[id][2]);
+    float cosB = glm::cos(beta);
+    float sinB = glm::sin(beta);
+
+    glm::mat3 R1 = glm::mat3(
+        /*cos*/ cosA, /*sin*/ sinA, 0,
+        /*-sin*/ -sinA , /*cos*/  cosA, 0,
+        0,      0,           1
+    );
+
+    glm::mat3 R2 = glm::mat3(
+        /*cos*/ cosB, /*sin*/ sinB, 0,
+        /*-sin*/ -sinB , /*cos*/  cosB, 0,
+        0,      0,           1
+    );
+
+    glm::vec3 pivotNXZ = R1 * glm::vec3(pivot[0],pivot[2],1);
+    pivot = rb::Vector3{pivotNXZ[0],pivot[1],pivotNXZ[1]};
+    glm::vec3 offsetNXZ = R2 * glm::vec3(offset[id][0],offset[id][2],1);
+    offset[id] = rb::Vector3{offsetNXZ[0],offset[id][1],offsetNXZ[1]};
+
+    
+    childs[id]->body.setPos(rb::Vector3{fPos[0]+offset[id][0]+pivot[0],fPos[1]+offset[id][1]+pivot[1],fPos[2]+offset[id][2]+pivot[2]});
+
+
+    oldRot = fRot; //aggiorno la rotazione per il ciclo successivo
+    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_s fRot = father->body.getRot();
+
+    pivot = pivotPoint;
+    rb::Vector3 pivotCenter = father->globalPos + father->body.getPos() + pivot;
+
+    /*
+    float sign = pivot[2] >= 0 ? 1 : -1;
+    float r = sqrt(pow(pivot[0],2)+pow(pivot[2],2));
+    rotOffset.push_back(  rb::Vector3_s{0,0,_Float16( acos(sign * pivot[0]/r) )} );
+    */
+
+
+    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->globalPos + c->body.getPos();
+        tmpCoords = cCoords - pivotCenter;
+
+        /*
+        float r = sqrt(pow(tmpCoords[0],2)+pow(tmpCoords[2],2));
+        oldCRot.push_back( rb::Vector3_s{0,0,_Float16( acos(tmpCoords[0]/r) )} );
+        */
+        oldCRot.push_back(c->body.getRot());
+
+        offset.push_back(tmpCoords);
+        
+    }
+
+
+}
+
+PivotJoint::~PivotJoint(){
+
+}
+
+
+void PivotJoint::movechild(){
+   
+    for ( unsigned int i = 0; i < childs.size(); i++){
+        traslate(i);
+        rotate(i);
+    }
+}

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

@@ -0,0 +1,84 @@
+#include "../headers/rigid_joint.hpp"
+
+#define ZERO_INT 0.00001
+
+//using namespace glm;
+
+void RigidJoint::rotate(unsigned int id){
+    rb::Vector3_s fRot = father->body.getRot();
+    rb::Vector3_s 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]);
+
+
+    // sposto il alla distanza offset rispetto all'origine R*pos
+    // calcolo alpha angolo 
+
+    float alpha = float (fRot[2] - fRotOld[2]);
+    float cosA = glm::cos(alpha);
+    float sinA = glm::sin(alpha);
+
+    glm::mat3 R = glm::mat3(
+        /*cos*/ cosA, /*sin*/ sinA, 0,
+        /*-sin*/ -sinA , /*cos*/  cosA, 0,
+        0,      0,           1
+    );
+
+    //sposto il child all'origine rispetto al padre
+
+    glm::vec3 XZ_cPos = {offset[id][0],offset[id][2],1};
+    glm::vec3 resRot = R * XZ_cPos;
+
+    offset[id][0] = resRot[0] ;
+    offset[id][2] = resRot[1] ; 
+
+    childs[id]->body.setPos({fPos[0]-offset[id][0],offset[id][1],fPos[2] -offset[id][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_s fRot = father->body.getRot();
+
+
+
+
+    //mi calcolo l'offset per ogni child rispetto al padre
+    for(PieceInterface* c : childs){
+        rb::Vector3 tmpCoords;
+        rb::Vector3_s 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.2/src/pieces/headers/caviglia.hpp

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

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

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

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

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

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

@@ -0,0 +1,35 @@
+#include <SFML/Graphics.hpp>
+#include <math.h>
+#include "../../rigidbody/headers/rb.hpp"
+
+
+#ifndef PIECE_INTERFACE_H
+#define PIECE_INTERFACE_H
+
+
+enum class ReferencePlane {
+    XY,
+    YZ,
+    XZ
+};
+
+//classi
+class PieceInterface{
+    protected:
+        
+        //std::vector<JointInterface*> joints;
+
+    public:
+        sf::Shape* shape;
+        sf::Vector2f size;
+        rb::Vector3 globalPos;
+        rb::rigidbody body;
+        sf::Color color;
+
+        virtual void update(sf::Clock cl) = 0;
+        virtual sf::Shape* draw(ReferencePlane plane) = 0;
+        virtual ~PieceInterface(){}
+};
+
+
+#endif

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

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

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

@@ -0,0 +1,40 @@
+#include "../headers/caviglia.hpp"
+
+Caviglia::Caviglia(rb::Vector3 coords, _Float16 mass){
+    size = caviglia_Dim;
+    rb::Vector3 com = {size.x/2,0, size.y/2};
+    body = rb::rigidbody(coords, com, mass);
+    color = caviglia_Col;
+    shape = new sf::RectangleShape(size);
+    shape->setOrigin({size.x/2,size.y/2});
+    globalPos = {0,0,0};
+}
+
+Caviglia::~Caviglia(){
+    delete shape;
+}
+
+void Caviglia::update(sf::Clock cl){
+    //body.step(cl);
+}
+
+sf::Shape* Caviglia::draw(ReferencePlane plane){
+    shape->setFillColor(color);
+    rb::Vector3 tmpPos = body.getPos();
+    rb::Vector3_s tmpRot = body.getRot();
+
+    switch (plane)
+    {
+    case ReferencePlane::XZ:
+        shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[2])));
+        break;
+    
+
+    default:
+        break;
+    }
+
+   
+    return shape;
+}

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

@@ -0,0 +1,40 @@
+#include "../headers/coscia.hpp"
+
+Coscia::Coscia(rb::Vector3 coords, _Float16 mass){
+    size = coscia_Dim;
+    rb::Vector3 com = {size.x/2,0, size.y/2};
+    body = rb::rigidbody(coords, com, mass);
+    color = coscia_Col;
+    shape = new sf::RectangleShape(size);
+    shape->setOrigin({size.x/2,size.y/2});
+    globalPos = {0,0,0};
+}
+
+Coscia::~Coscia(){
+    delete shape;
+}
+
+void Coscia::update(sf::Clock cl){
+    //body.step(cl);
+}
+
+sf::Shape* Coscia::draw(ReferencePlane plane){
+    shape->setFillColor(color);
+    rb::Vector3 tmpPos = body.getPos();
+    rb::Vector3_s tmpRot = body.getRot();
+
+    switch (plane)
+    {
+    case ReferencePlane::XZ:
+        shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[2])));
+        break;
+    
+
+    default:
+        break;
+    }
+
+   
+    return shape;
+}

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

@@ -0,0 +1,102 @@
+#include "../headers/sensore.hpp"
+
+
+Sensore::Sensore(rb::Vector3 coords, _Float16 mass){
+    size = sensore_Dim;
+    rb::Vector3 com = {size.x/2,0, size.y/2};
+    body = rb::rigidbody(coords, com, mass);
+    color = sensore_Col;
+    shape = new sf::RectangleShape(size);
+    globalPos = {0,0,0};
+}
+
+Sensore::Sensore(rb::Vector3 coords, _Float16 mass, unsigned int st, unsigned int dataIntvl, std::vector<std::vector<float>> data) : Sensore(coords, mass){
+        dataPos = st;
+        this->dataIntvl = dataIntvl;
+        initCSV(data);
+}
+
+
+Sensore::~Sensore(){
+    delete shape;
+}
+
+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";
+    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);
+    }
+}
+
+
+void Sensore::update(sf::Clock cl){
+    // Aggiorno la posizione nei dati
+    int64_t currTime = cl.getElapsedTime().asMicroseconds() *100000;
+    if (timeData[dataPos] < currTime && dataIntvl - dataPos > 0) { //aggiorno solo se ho cambiato posizione
+        dataPos++;
+
+        //calcolo la posizione e velocità
+        calcRotWithG(dataPos);
+        body.setAcc(rb::Vector3{accData[dataPos]});
+        body.step(cl);
+    }
+
+}
+
+sf::Shape* Sensore::draw(ReferencePlane plane){
+    shape->setFillColor(color);
+
+    shape->setOrigin({sensore_Dim.x/2, sensore_Dim.y/2});
+    
+    rb::Vector3_s tmpRot = body.getRot();
+    
+
+    rb::Vector3 tmpPos = body.getPos();
+
+    switch (plane)
+    {
+    case ReferencePlane::XZ:
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[2])));
+        shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
+        break;
+    
+    default:
+        break;
+    }
+    
+
+    
+
+    return shape;
+}
+
+
+void Sensore::calcRotWithG(unsigned int index){ // calcolo rotazione con valori della gravità
+
+    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 tmpAX = acos(tmpSinY);
+    float tmpAY = acos(tmpSinZ);
+    float tmpAZ = acos(tmpSinX);
+
+    body.setRot(rb::Vector3_s{_Float16( tmpAX),_Float16( tmpAY),_Float16( tmpAZ) });
+
+}

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

@@ -0,0 +1,108 @@
+#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_s;
+
+        class rigidbody
+        {
+            private:
+                Vector3 vel = {0,0,0};
+                Vector3 acc = {0,0,0};
+                Vector3_s rot = {0,0,0};
+                Vector3 tanAcc = {0,0,0};
+
+                _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 time_t 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);
+                ~rigidbody();
+
+                
+                Vector3 getPos();
+                Vector3_s getRot();
+                void setPos(const Vector3 Npos);
+                void setRot(const Vector3_s Nrot);
+                void setVel(const Vector3 Nacc);
+                void setAcc(const Vector3 Nvel);
+                void step(const sf::Clock time);
+
+                //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 rb::Vector3_s operator+(const rb::Vector3_s& v1, const rb::Vector3_s& v2) {
+            if (v1.size() != 3 || v2.size() != 3) {
+                throw std::invalid_argument("I vettori devono avere esattamente 3 elementi.");
+            }
+            return rb::Vector3_s{
+                v1[0] + v2[0],
+                v1[1] + v2[1],
+                v1[2] + v2[2]
+            };
+        }
+
+     inline rb::Vector3_s operator-(const rb::Vector3_s& v1, const rb::Vector3_s& v2) {
+            if (v1.size() != 3 || v2.size() != 3) {
+                throw std::invalid_argument("I vettori devono avere esattamente 3 elementi.");
+            }
+            return rb::Vector3_s{
+                v1[0] - v2[0],
+                v1[1] - v2[1],
+                v1[2] - v2[2]
+            };
+        }
+        /*
+        inline bool operator!=(const rb::Vector3_s& v1, const rb::Vector3_s& 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.2/src/rigidbody/methods/rb_class.cpp

@@ -0,0 +1,100 @@
+#include "../headers/rb.hpp"
+
+
+using namespace rb ;
+
+rigidbody::rigidbody(Vector3 coords, Vector3 centerOfMass, _Float16 mass)
+{
+    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;
+
+}
+
+rigidbody::~rigidbody()
+{
+}
+
+Vector3 rigidbody::getPos(){
+    return Vector3 {coords};
+}
+Vector3_s rigidbody::getRot(){
+    return Vector3_s {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 "Vel vector must be 3 in lenght!";
+    
+    int i = 0;
+    for (float axis : Nacc){
+        acc[i] = axis;
+        i++;
+    }
+
+}
+
+void rigidbody::setRot(const Vector3_s Nrot){
+    if (Nrot.size() != 3) throw "Vel vector must be 3 in lenght!";
+    
+    int i = 0;
+    for (_Float16 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){
+    int64_t Dtime = time.getElapsedTime().asMicroseconds();
+    if (prevT == 0) prevT = Dtime;
+
+
+    float dt = (float(Dtime) / 1000000.0) - (float(prevT) / 1000000.0);
+    prevT = Dtime;
+
+    calcPos(dt);
+    calcVel(dt);
+}
+    
+

RELEASES/FCG_VisualizzatoreCamminata-0.2/src/sfml_util.cpp

@@ -0,0 +1,184 @@
+#include <SFML/Graphics.hpp>
+#include <math.h>
+#include "pieces/headers/piece_interface.hpp"
+#include "joints/headers/joint_interface.hpp"
+
+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;
+    sf::Vector2f cameraOffset = {0.,0.};
+
+    sf::Clock clock;
+
+    int selected = -1;
+
+    bool rot_Piece = false;
+    bool drag_Piece = false;
+    bool drag = false;
+    sf::Vector2i mouse_pos;
+
+    State(unsigned w, unsigned h, std::string title) 
+    {
+        window = sf::RenderWindow(sf::VideoMode({w, h}), title);
+        clock.restart();
+    }
+    void update();
+};
+
+///
+////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////
+/// Fisics functions
+
+void State::update(){
+    
+
+    for(PieceInterface* p : pieces){
+        p->update(clock);
+    }
+    for(JointInterface* j : joints){
+        j->movechild();
+    }
+}
+
+///
+//////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////
+/// Callback functions
+void handle(const sf::Event::Closed &, State &gs)
+{
+    gs.window.close();
+}
+
+void handle(const sf::Event::TextEntered &textEnter, State &gs)
+{
+   
+}
+
+void handle(const sf::Event::KeyPressed &keyPressed, State &gs)
+{
+}
+
+void handle(const sf::Event::MouseMoved &mouseMoved, State &gs)
+{
+    sf::Vector2i offset = mouseMoved.position - gs.mouse_pos;
+    gs.mouse_pos = mouseMoved.position;
+    if (gs.drag){
+        for(PieceInterface* p : gs.pieces){
+            p->globalPos = {p->globalPos[0] + offset.x, p->globalPos[1],p->globalPos[2] + offset.y};
+
+            /// Devo spostare sul piano di visualizzazione
+            /// Quindi dovrò settare una variabile che mi definisce qual è il piano preso in considerazione, questo sarà nello state
+        }
+    }
+    if (gs.selected != -1 && gs.drag_Piece){
+        rb::Vector3 tmp = gs.pieces[gs.selected]->body.getPos();
+        gs.pieces[gs.selected]->body.setPos({tmp[0]+offset.x,tmp[1],tmp[2]+offset.y});
+    }
+    if (gs.selected != -1 && gs.rot_Piece){
+        rb::Vector3_s tmp = gs.pieces[gs.selected]->body.getRot();
+
+        _Float16 nrot = _Float16(offset.x)/10;
+        gs.pieces[gs.selected]->body.setRot({tmp[0],tmp[1],tmp[2]+nrot});
+    }
+
+    
+}
+
+void handle(const sf::Event::MouseButtonPressed &mouseBP, State &gs)
+{
+    gs.mouse_pos = mouseBP.position;
+    if ( mouseBP.button == sf::Mouse::Button::Middle) gs.drag = true;
+    if ( mouseBP.button == sf::Mouse::Button::Left){
+        gs.drag_Piece = true;
+        int i = 0;
+        for (PieceInterface* p : gs.pieces){
+            
+            sf::Vector2f pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]};
+
+            if (dist(pos,mouseBP.position) < 20){
+                gs.selected = i;
+                break;
+            }
+            i++;
+        }
+    }
+    if ( mouseBP.button == sf::Mouse::Button::Right){
+        gs.rot_Piece = true;
+        int i = 0;
+        for (PieceInterface* p : gs.pieces){
+            
+            sf::Vector2f pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]};
+
+            if (dist(pos,mouseBP.position) < 20){
+                gs.selected = i;
+                break;
+            }
+            i++;
+        }
+    }
+
+}
+
+void handle(const sf::Event::MouseButtonReleased &, State &gs)
+{
+    gs.drag = false;
+    gs.drag_Piece = false;
+    gs.rot_Piece = false;
+    gs.selected = -1;
+}
+
+void handle(const sf::Event::Resized &resized, State &gs)
+{
+    sf::FloatRect visibleArea({0.f, 0.f}, sf::Vector2f(resized.size));
+    gs.window.setView(sf::View(visibleArea));
+}
+
+template <typename T>
+void handle(const T &, State &gs)
+{
+    // All unhandled events will end up here
+}
+///
+////////////////////////////////////////////////////////////
+
+
+////////////////////////////////////////////////////////////
+/// Graphics
+void doGUI(State &gs)
+{
+    // TODO: here code to display the menus
+    //Bottoni
+    
+}
+
+void doGraphics(State &gs)
+{
+    gs.window.clear();
+    doGUI(gs);
+
+    for(PieceInterface* p: gs.pieces){
+        gs.window.draw(*p->draw(ReferencePlane::XZ));
+    }
+    
+    // TODO: add here code to display shapes in your canvas
+
+    gs.window.display();
+}
+///
+////////////////////////////////////////////////////////////

RELEASES/FCG_VisualizzatoreCamminata-0.2/src/testMain.cpp

@@ -0,0 +1,83 @@
+#include "include.hpp"
+
+#define DATA_PATH std::string("./../../data/")
+
+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;
+    }
+
+    //Costruisco la GUI 
+    State gs(800, 600, "Visualizzatore passo");
+    gs.window.setFramerateLimit(60);
+    printf("Costruisco gli oggetti\n");
+
+    try{
+        processor.readCSVFile (DATA_PATH + "coscia_filt.csv");
+        const auto& coscia = processor.getData();
+
+
+        gs.pieces.push_back(new Coscia (rb::Vector3{300,10,300},2));
+        gs.pieces.push_back(new Sensore (rb::Vector3{300,300,300},_Float16( 0.2 ),900,3000,coscia));
+        gs.pieces.push_back(new Coscia (rb::Vector3{300,10,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,700,500},_Float16( 0.2 ),900,3000,caviglia));
+        
+        // modifico la rotazione relativa della gamba
+        gs.pieces[1]->body.setRot({0,0,_Float16 (1.6)});
+        gs.pieces[3]->body.setRot({0,0,_Float16 (1.7)});
+
+
+        // aggiungo i joint
+
+        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]}));
+
+        printf("Ho costruito tutto!\n");
+    }
+    catch(char* e){
+        printf("%s\n",e);
+    }
+    printf("Avvio l'interfaccia grafica\n");
+    //Avvio il loop della GUI
+    gs.clock.start();
+    while (gs.window.isOpen()) 
+    {
+        // event loop and handler through callbacks
+        gs.window.handleEvents([&](const auto &event)
+                               { handle(event, gs); });
+        // Show update
+        gs.update();
+        doGraphics(gs);
+    }
+    
+    return 0;
+}
+

RELEASES/FCG_VisualizzatoreCamminata-0.3/.gitignore

@@ -0,0 +1,19 @@
+CMakeLists.txt.user
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Testing
+Makefile
+cmake_install.cmake
+install_manifest.txt
+compile_commands.json
+CTestTestfile.cmake
+_deps
+CMakeUserPresets.json
+
+# 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.3/CMakeLists.txt

@@ -0,0 +1,31 @@
+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)
+
+
+set(METHODS_PATH "./src/*/methods/*.cpp")
+
+
+set(VERSION "V3")
+
+file(GLOB_RECURSE METHODS_SRC "${METHODS_PATH}")
+add_executable(main${VERSION} ./src/testMain.cpp ${METHODS_SRC} )
+target_compile_features(main${VERSION} PRIVATE cxx_std_17)
+target_link_libraries(main${VERSION} PRIVATE SFML::Graphics glm)

RELEASES/FCG_VisualizzatoreCamminata-0.3/README.md

@@ -0,0 +1,37 @@
+# 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
+
+
+# Per compliare:
+
+    cmake --build
+
+# Per lanciare:
+
+    ./build/bin/mainV3

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

@@ -0,0 +1,20 @@
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <string>
+
+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;
+
+};

RELEASES/FCG_VisualizzatoreCamminata-0.3/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.3/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_s> rotOffset;
+        PieceInterface* father;
+        std::vector<PieceInterface*> childs;
+
+        virtual ~JointInterface(){};
+        virtual void movechild() = 0;
+
+        
+};
+
+#endif

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

@@ -0,0 +1,22 @@
+#include "joint_interface.hpp"
+
+class PivotJoint : public JointInterface {
+    protected:
+        void rotate(unsigned int id) override;
+        void traslate(unsigned int id) override;
+
+        rb::Vector3_s oldRot;
+        rb::Vector3 pivot;
+        std::vector<rb::Vector3_s> oldCRot;
+
+        //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();
+};

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

@@ -0,0 +1,19 @@
+#include "joint_interface.hpp"
+
+
+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();
+};

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

@@ -0,0 +1,99 @@
+#include "../headers/pivot_joint.hpp"
+
+#define ZERO_INT 0.00001
+
+void PivotJoint::rotate(unsigned int id){
+    rb::Vector3_s fRot = father->body.getRot();
+    rb::Vector3 fPos = father->body.getPos();
+    rb::Vector3_s cRot = childs[id]->body.getRot();
+    
+    ////  sposto l'origine passivamente su tutti gli assi ////
+
+    float alpha = float (fRot[2] - oldRot[2]);
+    float cosA = glm::cos(alpha);
+    float sinA = glm::sin(alpha);
+
+    float beta = float (cRot[2] - oldCRot[id][2]);
+    float cosB = glm::cos(beta);
+    float sinB = glm::sin(beta);
+
+    glm::mat3 R1 = glm::mat3(
+        /*cos*/ cosA, /*sin*/ sinA, 0,
+        /*-sin*/ -sinA , /*cos*/  cosA, 0,
+        0,      0,           1
+    );
+
+    glm::mat3 R2 = glm::mat3(
+        /*cos*/ cosB, /*sin*/ sinB, 0,
+        /*-sin*/ -sinB , /*cos*/  cosB, 0,
+        0,      0,           1
+    );
+
+    glm::vec3 pivotNXZ = R1 * glm::vec3(pivot[0],pivot[2],1);
+    pivot = rb::Vector3{pivotNXZ[0],pivot[1],pivotNXZ[1]};
+    glm::vec3 offsetNXZ = R2 * glm::vec3(offset[id][0],offset[id][2],1);
+    offset[id] = rb::Vector3{offsetNXZ[0],offset[id][1],offsetNXZ[1]};
+
+    
+    childs[id]->body.setPos(rb::Vector3{fPos[0]+offset[id][0]+pivot[0],fPos[1]+offset[id][1]+pivot[1],fPos[2]+offset[id][2]+pivot[2]});
+
+
+    oldRot = fRot; //aggiorno la rotazione per il ciclo successivo
+    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_s fRot = father->body.getRot();
+
+    pivot = pivotPoint;
+    rb::Vector3 pivotCenter = father->globalPos + father->body.getPos() + pivot;
+
+    /*
+    float sign = pivot[2] >= 0 ? 1 : -1;
+    float r = sqrt(pow(pivot[0],2)+pow(pivot[2],2));
+    rotOffset.push_back(  rb::Vector3_s{0,0,_Float16( acos(sign * pivot[0]/r) )} );
+    */
+
+
+    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->globalPos + c->body.getPos();
+        tmpCoords = cCoords - pivotCenter;
+
+        /*
+        float r = sqrt(pow(tmpCoords[0],2)+pow(tmpCoords[2],2));
+        oldCRot.push_back( rb::Vector3_s{0,0,_Float16( acos(tmpCoords[0]/r) )} );
+        */
+        oldCRot.push_back(c->body.getRot());
+
+        offset.push_back(tmpCoords);
+        
+    }
+
+
+}
+
+PivotJoint::~PivotJoint(){
+
+}
+
+
+void PivotJoint::movechild(){
+   
+    for ( unsigned int i = 0; i < childs.size(); i++){
+        traslate(i);
+        rotate(i);
+    }
+}

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

@@ -0,0 +1,84 @@
+#include "../headers/rigid_joint.hpp"
+
+#define ZERO_INT 0.00001
+
+//using namespace glm;
+
+void RigidJoint::rotate(unsigned int id){
+    rb::Vector3_s fRot = father->body.getRot();
+    rb::Vector3_s 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]);
+
+
+    // sposto il alla distanza offset rispetto all'origine R*pos
+    // calcolo alpha angolo 
+
+    float alpha = float (fRot[2] - fRotOld[2]);
+    float cosA = glm::cos(alpha);
+    float sinA = glm::sin(alpha);
+
+    glm::mat3 R = glm::mat3(
+        /*cos*/ cosA, /*sin*/ sinA, 0,
+        /*-sin*/ -sinA , /*cos*/  cosA, 0,
+        0,      0,           1
+    );
+
+    //sposto il child all'origine rispetto al padre
+
+    glm::vec3 XZ_cPos = {offset[id][0],offset[id][2],1};
+    glm::vec3 resRot = R * XZ_cPos;
+
+    offset[id][0] = resRot[0] ;
+    offset[id][2] = resRot[1] ; 
+
+    childs[id]->body.setPos({fPos[0]-offset[id][0],offset[id][1],fPos[2] -offset[id][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_s fRot = father->body.getRot();
+
+
+
+
+    //mi calcolo l'offset per ogni child rispetto al padre
+    for(PieceInterface* c : childs){
+        rb::Vector3 tmpCoords;
+        rb::Vector3_s 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.3/src/pieces/headers/joint_piece.hpp

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

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

@@ -0,0 +1,50 @@
+#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);
+    color = caviglia_Col;
+    globalPos = {0,0,0};
+
+    initialize_shapes(caviglia_Dim);
+}
+
+
+Caviglia::~Caviglia(){
+    delete shapeXZ;
+    delete shapeYZ;
+}
+
+void Caviglia::update(sf::Clock cl){
+    //body.step(cl);
+}
+
+sf::Shape* Caviglia::draw(ReferencePlane plane){
+    
+    rb::Vector3 tmpPos = body.getPos();
+    rb::Vector3_s tmpRot = body.getRot();
+
+    switch (plane)
+    {
+    case ReferencePlane::XZ:
+        {
+        sf::Shape* shape = shapeXZ;
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[2])));
+        shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
+        return shape;}
+        break;
+    
+    case ReferencePlane::YZ:
+        {
+        sf::Shape* shape = shapeYZ;
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
+        shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
+        return shape;}
+        break;
+    
+    default:
+        break;
+    }
+   
+    return nullptr;
+}

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

@@ -0,0 +1,50 @@
+#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);
+    color = coscia_Col;
+    globalPos = {0,0,0};
+    initialize_shapes(coscia_Dim);
+}
+
+Coscia::~Coscia(){
+    delete shapeXZ;
+    delete shapeYZ;
+}
+
+void Coscia::update(sf::Clock cl){
+    //body.step(cl);
+}
+
+sf::Shape* Coscia::draw(ReferencePlane plane){
+    
+    rb::Vector3 tmpPos = body.getPos();
+    rb::Vector3_s tmpRot = body.getRot();
+
+
+    switch (plane)
+    {
+    case ReferencePlane::XZ:
+        {
+        sf::Shape* shape = shapeXZ;
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[2])));
+        shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
+        return shape;}
+        break;
+    
+    case ReferencePlane::YZ:
+        {
+        sf::Shape* shape = shapeYZ;
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
+        shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
+        return shape;}
+        break;
+    
+    default:
+        break;
+    }
+
+   
+    return nullptr;
+}

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

@@ -0,0 +1,113 @@
+#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);
+    color = sensore_Col;
+    globalPos = {0,0,0};
+    initialize_shapes(sensore_Dim);
+}
+
+Sensore::Sensore(rb::Vector3 coords, _Float16 mass, unsigned int st, unsigned int dataIntvl, std::vector<std::vector<float>> data) : Sensore(coords, mass){
+        dataPos = st;
+        this->dataIntvl = dataIntvl;
+        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() < 1) 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);
+    }
+}
+
+
+void Sensore::update(sf::Clock cl){
+    // Aggiorno la posizione nei dati
+    int64_t currTime = cl.getElapsedTime().asMicroseconds() *100000;
+    if (timeData[dataPos] < currTime && dataIntvl - dataPos > 0) { //aggiorno solo se ho cambiato posizione
+        dataPos++;
+
+        //calcolo la posizione e velocità
+        calcRotWithG(dataPos);
+
+
+        body.setAcc(rb::Vector3{accData[dataPos]});
+        body.step(cl);
+    }
+
+}
+
+sf::Shape* Sensore::draw(ReferencePlane plane){
+    
+    rb::Vector3_s tmpRot = body.getRot();
+    rb::Vector3 tmpPos = body.getPos();
+
+    switch (plane)
+    {
+    case ReferencePlane::XZ:
+        {
+        sf::Shape* shape = shapeXZ;
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[2])));
+        shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
+        return shape;}
+        break;
+    
+    case ReferencePlane::YZ:
+        {
+        sf::Shape* shape = shapeYZ;
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
+        shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
+        return shape;}
+        break;
+    
+    default:
+        break;
+    }
+
+    return nullptr;
+}
+
+
+void Sensore::calcRotWithG(unsigned int index){ // calcolo rotazione con valori della gravità
+
+    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 tmpAX = acos(tmpSinY);
+    float tmpAY = acos(tmpSinZ);
+    float tmpAZ = acos(tmpSinX);
+
+    body.setRot(rb::Vector3_s{_Float16( tmpAX),_Float16( tmpAY),_Float16( tmpAZ) });
+
+}
+/*
+void Sensore::calcRotWithConstraint(){
+
+};*/
+
+/////////////// cinematica inversa

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

@@ -0,0 +1,50 @@
+#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);
+    color = torso_Col;
+    globalPos = {0,0,0};
+
+    initialize_shapes(torso_Dim);
+}
+
+Torso::~Torso(){
+    delete shapeXZ;
+    delete shapeYZ;
+}
+
+void Torso::update(sf::Clock cl){
+    
+}
+
+sf::Shape* Torso::draw(ReferencePlane plane){
+   
+    rb::Vector3_s tmpRot = body.getRot();
+    rb::Vector3 tmpPos = body.getPos();
+
+
+    switch (plane)
+    {
+    case ReferencePlane::XZ:
+        {
+        sf::Shape* shape = shapeXZ;
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[2])));
+        shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
+        return shape;}
+        break;
+    
+    case ReferencePlane::YZ:
+        {
+        sf::Shape* shape = shapeYZ;
+        shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
+        shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
+        return shape;}
+        break;
+    
+    default:
+        break;
+    }
+
+    return nullptr;
+}

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

@@ -0,0 +1,108 @@
+#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_s;
+
+        class rigidbody
+        {
+            private:
+                Vector3 vel = {0,0,0};
+                Vector3 acc = {0,0,0};
+                Vector3_s rot = {0,0,0};
+                Vector3 tanAcc = {0,0,0};
+
+                _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 time_t 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);
+                ~rigidbody();
+
+                
+                Vector3 getPos();
+                Vector3_s getRot();
+                void setPos(const Vector3 Npos);
+                void setRot(const Vector3_s Nrot);
+                void setVel(const Vector3 Nacc);
+                void setAcc(const Vector3 Nvel);
+                void step(const sf::Clock time);
+
+                //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 rb::Vector3_s operator+(const rb::Vector3_s& v1, const rb::Vector3_s& v2) {
+            if (v1.size() != 3 || v2.size() != 3) {
+                throw std::invalid_argument("I vettori devono avere esattamente 3 elementi.");
+            }
+            return rb::Vector3_s{
+                v1[0] + v2[0],
+                v1[1] + v2[1],
+                v1[2] + v2[2]
+            };
+        }
+
+     inline rb::Vector3_s operator-(const rb::Vector3_s& v1, const rb::Vector3_s& v2) {
+            if (v1.size() != 3 || v2.size() != 3) {
+                throw std::invalid_argument("I vettori devono avere esattamente 3 elementi.");
+            }
+            return rb::Vector3_s{
+                v1[0] - v2[0],
+                v1[1] - v2[1],
+                v1[2] - v2[2]
+            };
+        }
+        /*
+        inline bool operator!=(const rb::Vector3_s& v1, const rb::Vector3_s& 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.3/src/rigidbody/methods/rb_class.cpp

@@ -0,0 +1,100 @@
+#include "../headers/rb.hpp"
+
+
+using namespace rb ;
+
+rigidbody::rigidbody(Vector3 coords, Vector3 centerOfMass, _Float16 mass)
+{
+    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;
+
+}
+
+rigidbody::~rigidbody()
+{
+}
+
+Vector3 rigidbody::getPos(){
+    return Vector3 {coords};
+}
+Vector3_s rigidbody::getRot(){
+    return Vector3_s {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 "Vel vector must be 3 in lenght!";
+    
+    int i = 0;
+    for (float axis : Nacc){
+        acc[i] = axis;
+        i++;
+    }
+
+}
+
+void rigidbody::setRot(const Vector3_s Nrot){
+    if (Nrot.size() != 3) throw "Vel vector must be 3 in lenght!";
+    
+    int i = 0;
+    for (_Float16 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){
+    int64_t Dtime = time.getElapsedTime().asMicroseconds();
+    if (prevT == 0) prevT = Dtime;
+
+
+    float dt = (float(Dtime) / 1000000.0) - (float(prevT) / 1000000.0);
+    prevT = Dtime;
+
+    calcPos(dt);
+    calcVel(dt);
+}
+    
+

RELEASES/FCG_VisualizzatoreCamminata-0.3/src/sfml_util.cpp

@@ -0,0 +1,189 @@
+#include <SFML/Graphics.hpp>
+#include <math.h>
+#include "pieces/headers/piece_interface.hpp"
+#include "joints/headers/joint_interface.hpp"
+
+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;
+    sf::Vector2f cameraOffset = {0.,0.};
+
+    sf::Clock clock;
+    ReferencePlane selectedPlane = ReferencePlane::XZ;
+
+    int selected = -1;
+
+    bool rot_Piece = false;
+    bool drag_Piece = false;
+    bool drag = false;
+    sf::Vector2i mouse_pos;
+
+    State(unsigned w, unsigned h, std::string title) 
+    {
+        window = sf::RenderWindow(sf::VideoMode({w, h}), title);
+        clock.restart();
+    }
+    void update();
+};
+
+///
+////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////
+/// Fisics functions
+
+void State::update(){
+    
+
+    for(PieceInterface* p : pieces){
+        p->update(clock);
+    }
+    for(JointInterface* j : joints){
+        j->movechild();
+    }
+}
+
+///
+//////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////
+/// Callback functions
+void handle(const sf::Event::Closed &, State &gs)
+{
+    gs.window.close();
+}
+
+void handle(const sf::Event::TextEntered &textEnter, State &gs)
+{
+   
+}
+
+void handle(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;
+    }
+}
+
+void handle(const sf::Event::MouseMoved &mouseMoved, State &gs)
+{
+    sf::Vector2i offset = mouseMoved.position - gs.mouse_pos;
+    gs.mouse_pos = mouseMoved.position;
+    if (gs.drag){
+        for(PieceInterface* p : gs.pieces){
+            p->globalPos = {p->globalPos[0] + offset.x, p->globalPos[1],p->globalPos[2] + offset.y};
+
+            /// Devo spostare sul piano di visualizzazione
+            /// Quindi dovrò settare una variabile che mi definisce qual è il piano preso in considerazione, questo sarà nello state
+        }
+    }
+    if (gs.selected != -1 && gs.drag_Piece){
+        rb::Vector3 tmp = gs.pieces[gs.selected]->body.getPos();
+        gs.pieces[gs.selected]->body.setPos({tmp[0]+offset.x,tmp[1],tmp[2]+offset.y});
+    }
+    if (gs.selected != -1 && gs.rot_Piece){
+        rb::Vector3_s tmp = gs.pieces[gs.selected]->body.getRot();
+
+        _Float16 nrot = _Float16(offset.x)/10;
+        gs.pieces[gs.selected]->body.setRot({tmp[0],tmp[1],tmp[2]+nrot});
+    }
+
+    
+}
+
+void handle(const sf::Event::MouseButtonPressed &mouseBP, State &gs)
+{
+    gs.mouse_pos = mouseBP.position;
+    if ( mouseBP.button == sf::Mouse::Button::Middle) gs.drag = true;
+    if ( mouseBP.button == sf::Mouse::Button::Left){
+        gs.drag_Piece = true;
+        int i = 0;
+        for (PieceInterface* p : gs.pieces){
+            
+            sf::Vector2f pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]};
+
+            if (dist(pos,mouseBP.position) < 20){
+                gs.selected = i;
+            }
+            i++;
+        }
+    }
+    if ( mouseBP.button == sf::Mouse::Button::Right){
+        gs.rot_Piece = true;
+        int i = 0;
+        for (PieceInterface* p : gs.pieces){
+            
+            sf::Vector2f pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]};
+
+            if (dist(pos,mouseBP.position) < 20){
+                gs.selected = i;
+            }
+            i++;
+        }
+    }
+
+}
+
+void handle(const sf::Event::MouseButtonReleased &, State &gs)
+{
+    gs.drag = false;
+    gs.drag_Piece = false;
+    gs.rot_Piece = false;
+    gs.selected = -1;
+}
+
+void handle(const sf::Event::Resized &resized, State &gs)
+{
+    sf::FloatRect visibleArea({0.f, 0.f}, sf::Vector2f(resized.size));
+    gs.window.setView(sf::View(visibleArea));
+}
+
+template <typename T>
+void handle(const T &, State &gs)
+{
+    // All unhandled events will end up here
+}
+///
+////////////////////////////////////////////////////////////
+
+
+////////////////////////////////////////////////////////////
+/// Graphics
+void doGUI(State &gs)
+{
+    // TODO: here code to display the menus
+    //Bottoni
+    
+}
+
+void doGraphics(State &gs)
+{
+    gs.window.clear();
+    doGUI(gs);
+
+    for(PieceInterface* p: gs.pieces){
+        gs.window.draw(*p->draw(gs.selectedPlane));
+    }
+    
+    // TODO: add here code to display shapes in your canvas
+
+    gs.window.display();
+}
+///
+////////////////////////////////////////////////////////////

RELEASES/FCG_VisualizzatoreCamminata-0.3/src/testMain.cpp

@@ -0,0 +1,87 @@
+#include "include.hpp"
+
+#define DATA_PATH std::string("./../../data/")
+
+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;
+    }
+
+    //Costruisco la GUI 
+    State gs(800, 600, "Visualizzatore passo");
+    gs.window.setFramerateLimit(60);
+    printf("Costruisco gli oggetti\n");
+
+    try{
+        processor.readCSVFile (DATA_PATH + "coscia_filt.csv");
+        const auto& coscia = processor.getData();
+
+
+        gs.pieces.push_back(new Coscia (rb::Vector3{300,10,300},2));
+        gs.pieces.push_back(new Sensore (rb::Vector3{300,300,300},_Float16( 0.2 ),900,3000,coscia));
+        gs.pieces.push_back(new Caviglia (rb::Vector3{300,10,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,700,500},_Float16( 0.2 ),900,3000,caviglia));
+        
+        gs.pieces.push_back(new Torso(rb::Vector3{300,400,150},2));
+
+        // modifico la rotazione relativa della gamba
+        gs.pieces[1]->body.setRot({0,0,_Float16 (1.6)});
+        gs.pieces[3]->body.setRot({0,0,_Float16 (1.7)});
+
+
+        // aggiungo i joint
+
+        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.joints.push_back(new PivotJoint(gs.pieces[4], {gs.pieces[1]}, rb::Vector3{0,0,50}));
+        
+
+        printf("Ho costruito tutto!\n");
+    }
+    catch(char* e){
+        printf("%s\n",e);
+    }
+    printf("Avvio l'interfaccia grafica\n");
+    //Avvio il loop della GUI
+    gs.clock.start();
+    while (gs.window.isOpen()) 
+    {
+        // event loop and handler through callbacks
+        gs.window.handleEvents([&](const auto &event)
+                               { handle(event, gs); });
+        // Show update
+        gs.update();
+        doGraphics(gs);
+    }
+    
+    return 0;
+}
+

RELEASES/FCG_VisualizzatoreCamminata-0.4/.gitignore

@@ -0,0 +1,19 @@
+CMakeLists.txt.user
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Testing
+Makefile
+cmake_install.cmake
+install_manifest.txt
+compile_commands.json
+CTestTestfile.cmake
+_deps
+CMakeUserPresets.json
+
+# 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-*