Endertom/FCG_VisualizzatoreCamminata
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 11 Wiki Activity

Caricamento v4

Browse Source
This commit is contained in:
Endert0m
2026-05-31 11:58:56 +02:00
parent 5169bc4096
commit 5da309b43f
13 changed files with 147 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -23,7 +23,7 @@ FetchContent_MakeAvailable(glm)
set(METHODS_PATH "./src/*/methods/*.cpp") set(METHODS_PATH "./src/*/methods/*.cpp")
set(VERSION "V3") set(VERSION "V4")
file(GLOB_RECURSE METHODS_SRC "${METHODS_PATH}") file(GLOB_RECURSE METHODS_SRC "${METHODS_PATH}")
add_executable(main${VERSION} ./src/testMain.cpp ${METHODS_SRC} ) add_executable(main${VERSION} ./src/testMain.cpp ${METHODS_SRC} )
build/.cmake/api/v1/reply/codemodel-v2-48a6e2c377bb606c3455.json → build/.cmake/api/v1/reply/codemodel-v2-413561486a356cd66df3.json
+3 -3
View File
@@ -573,9 +573,9 @@
}, },
{ {
"directoryIndex" : 0, "directoryIndex" : 0,
"id" : "mainV3::@6890427a1f51a3e7e1df", "id" : "mainV4::@6890427a1f51a3e7e1df",
"jsonFile" : "target-mainV3-Debug-ee31761362aa79611c23.json", "jsonFile" : "target-mainV4-Debug-fc5cbebaffd99dc9855d.json",
"name" : "mainV3", "name" : "mainV4",
"projectIndex" : 0 "projectIndex" : 0
}, },
{ {
build/.cmake/api/v1/reply/index-2026-05-31T08-58-45-0525.json → build/.cmake/api/v1/reply/index-2026-05-31T09-56-00-0692.json
+2 -2
View File
@@ -26,7 +26,7 @@
"objects" : "objects" :
[ [
{ {
"jsonFile" : "codemodel-v2-48a6e2c377bb606c3455.json", "jsonFile" : "codemodel-v2-413561486a356cd66df3.json",
"kind" : "codemodel", "kind" : "codemodel",
"version" : "version" :
{ {
@@ -99,7 +99,7 @@
} }
}, },
{ {
"jsonFile" : "codemodel-v2-48a6e2c377bb606c3455.json", "jsonFile" : "codemodel-v2-413561486a356cd66df3.json",
"kind" : "codemodel", "kind" : "codemodel",
"version" : "version" :
{ {
build/.cmake/api/v1/reply/target-mainV3-Debug-ee31761362aa79611c23.json → build/.cmake/api/v1/reply/target-mainV4-Debug-fc5cbebaffd99dc9855d.json
+4 -4
View File
@@ -2,7 +2,7 @@
"artifacts" : "artifacts" :
[ [
{ {
"path" : "bin/mainV3" "path" : "bin/mainV4"
} }
], ],
"backtrace" : 1, "backtrace" : 1,
@@ -193,7 +193,7 @@
"id" : "glm::@ce54d98d7b326a3e12a8" "id" : "glm::@ce54d98d7b326a3e12a8"
} }
], ],
"id" : "mainV3::@6890427a1f51a3e7e1df", "id" : "mainV4::@6890427a1f51a3e7e1df",
"link" : "link" :
{ {
"commandFragments" : "commandFragments" :
@@ -296,8 +296,8 @@
"id" : "glm::@ce54d98d7b326a3e12a8" "id" : "glm::@ce54d98d7b326a3e12a8"
} }
], ],
"name" : "mainV3", "name" : "mainV4",
"nameOnDisk" : "mainV3", "nameOnDisk" : "mainV4",
"paths" : "paths" :
{ {
"build" : ".", "build" : ".",
build/bin/mainV4
Executable
BIN
View File
Binary file not shown.
src/joints/methods/pivot_joint_class.cpp
+41 -18
View File
@@ -9,34 +9,57 @@ void PivotJoint::rotate(unsigned int id){
//// sposto l'origine passivamente su tutti gli assi //// //// sposto l'origine passivamente su tutti gli assi ////
float alpha = float (fRot[2] - oldRot[2]); float alpha = float (fRot[0] - oldRot[0]);
float cosA = glm::cos(alpha); float cosA = glm::cos(alpha);
float sinA = glm::sin(alpha); float sinA = glm::sin(alpha);
float alpha1 = float (fRot[1] - oldRot[1]);
float cosA1 = glm::cos(alpha1);
float sinA1 = glm::sin(alpha1);
float beta = float (cRot[2] - oldCRot[id][2]); float beta = float (cRot[0] - oldCRot[id][0]);
float cosB = glm::cos(beta); float cosB = glm::cos(beta);
float sinB = glm::sin(beta); float sinB = glm::sin(beta);
float beta1 = float (cRot[1] - oldCRot[id][1]);
float cosB1 = glm::cos(beta1);
float sinB1 = glm::sin(beta1);
glm::mat3 R1 = glm::mat3( glm::mat4 Rpx = glm::mat4{
/*cos*/ cosA, /*sin*/ sinA, 0, 1 , 0, 0, 0,
/*-sin*/ -sinA , /*cos*/ cosA, 0, 0, cosA, sinA, 0,
0, 0, 1 0, -sinA, cosA, 0,
); 0, 0, 0, 1
};
glm::mat3 R2 = glm::mat3( glm::mat4 Rpy = glm::mat4{
/*cos*/ cosB, /*sin*/ sinB, 0, cosA1 , 0, sinA1, 0,
/*-sin*/ -sinB , /*cos*/ cosB, 0, 0, 1, 0, 0,
0, 0, 1 -sinA1, 0, cosA1, 0,
); 0, 0, 0, 1
};
glm::vec3 pivotNXZ = R1 * glm::vec3(pivot[0],pivot[2],1); glm::mat4 Rcx = glm::mat4{
pivot = rb::Vector3{pivotNXZ[0],pivot[1],pivotNXZ[1]}; 1 , 0, 0, 0,
glm::vec3 offsetNXZ = R2 * glm::vec3(offset[id][0],offset[id][2],1); 0, cosB, sinB, 0,
offset[id] = rb::Vector3{offsetNXZ[0],offset[id][1],offsetNXZ[1]}; 0, -sinB, cosB, 0,
0, 0, 0, 1
};
glm::mat4 Rcy = glm::mat4{
cosB1 , 0, sinB1, 0,
0, 1, 0, 0,
-sinB1, 0, cosB1, 0,
0, 0, 0, 1
};
glm::vec4 pivotN = Rpy * Rpx * glm::vec4(pivot[0], pivot[1], pivot[2],1);
pivot = rb::Vector3{pivotN[0],pivotN[1],pivotN[2]};
glm::vec4 offN = Rcy * Rcx * glm::vec4(offset[id][0],offset[id][1],offset[id][2],1);
offset[id] = rb::Vector3{offN[0],offN[1],offN[2]};
childs[id]->body.setPos(rb::Vector3{fPos[0]+offset[id][0]+pivot[0],fPos[1]+offset[id][1]+pivot[1],fPos[2]+offset[id][2]+pivot[2]}); 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]});
//printf("Offset = %f %f %f \n" , offN[0], offN[1], offN[2]);
oldRot = fRot; //aggiorno la rotazione per il ciclo successivo oldRot = fRot; //aggiorno la rotazione per il ciclo successivo
oldCRot[id] = cRot; oldCRot[id] = cRot;
@@ -55,7 +78,7 @@ PivotJoint::PivotJoint(PieceInterface* father,std::vector<PieceInterface*> child
rb::Vector3_s fRot = father->body.getRot(); rb::Vector3_s fRot = father->body.getRot();
pivot = pivotPoint; pivot = pivotPoint;
rb::Vector3 pivotCenter = father->globalPos + father->body.getPos() + pivot; rb::Vector3 pivotCenter = father->body.getPos() + pivot;
/* /*
float sign = pivot[2] >= 0 ? 1 : -1; float sign = pivot[2] >= 0 ? 1 : -1;
@@ -69,7 +92,7 @@ PivotJoint::PivotJoint(PieceInterface* father,std::vector<PieceInterface*> child
//mi calcolo l'offset per ogni child rispetto al pivot //mi calcolo l'offset per ogni child rispetto al pivot
for(PieceInterface* c : childs){ for(PieceInterface* c : childs){
rb::Vector3 tmpCoords; rb::Vector3 tmpCoords;
rb::Vector3 cCoords = c->globalPos + c->body.getPos(); rb::Vector3 cCoords = c->body.getPos();
tmpCoords = cCoords - pivotCenter; tmpCoords = cCoords - pivotCenter;
/* /*
src/joints/methods/rigid_joint_class.cpp
+32 -14
View File
@@ -12,29 +12,47 @@ void RigidJoint::rotate(unsigned int id){
childs[id]->body.setRot(fRot + rotOffset[id]); childs[id]->body.setRot(fRot + rotOffset[id]);
//passo a coordinate 3D per calcolare rotazione sul piano YZ oltre che al piano XZ
// sposto il alla distanza offset rispetto all'origine R*pos
// calcolo alpha angolo // calcolo alpha angolo
float alpha = float (fRot[2] - fRotOld[2]); float alpha = float (fRot[0] - fRotOld[0]);
float beta = float (fRot[1] - fRotOld[1]);
float cosA = glm::cos(alpha); float cosA = glm::cos(alpha);
float sinA = glm::sin(alpha); float sinA = glm::sin(alpha);
glm::mat3 R = glm::mat3( float cosB = glm::cos(beta);
/*cos*/ cosA, /*sin*/ sinA, 0, float sinB = glm::sin(beta);
/*-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::mat4 Rx = glm::mat4{
glm::vec3 resRot = R * XZ_cPos; 1 , 0, 0, 0,
0, cosA, sinA, 0,
0, -sinA, cosA, 0,
0, 0, 0, 1
};
offset[id][0] = resRot[0] ; glm::mat4 Ry = glm::mat4{
offset[id][2] = resRot[1] ; cosB , 0, sinB, 0,
0, 1, 0, 0,
-sinB, 0, cosB, 0,
0, 0, 0, 1
};
glm::mat4 T = glm::mat4{
1, 0, 0, 0,
0, 1, 0, 0,
0 ,0, 1, 0,
fPos[0], fPos[1], fPos[2], 1
};
glm::vec4 resRot = Rx * Ry * glm::vec4(offset[id][0],offset[id][1],offset[id][2],1);
offset[id] = rb::Vector3{resRot[0], resRot[1], resRot[2]};
glm::vec4 resTransf = T * resRot;
childs[id]->body.setPos({resTransf[0],resTransf[1],resTransf[2]});
childs[id]->body.setPos({fPos[0]-offset[id][0],offset[id][1],fPos[2] -offset[id][2]});
} }
src/pieces/methods/caviglia_class.cpp
+4 -2
View File
@@ -29,16 +29,18 @@ sf::Shape* Caviglia::draw(ReferencePlane plane){
case ReferencePlane::XZ: case ReferencePlane::XZ:
{ {
sf::Shape* shape = shapeXZ; sf::Shape* shape = shapeXZ;
shape->setRotation(sf::Angle(sf::radians(tmpRot[2]))); shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]}); shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
shape->setScale({1,cos(float(tmpRot[0]))});
return shape;} return shape;}
break; break;
case ReferencePlane::YZ: case ReferencePlane::YZ:
{ {
sf::Shape* shape = shapeYZ; sf::Shape* shape = shapeYZ;
shape->setRotation(sf::Angle(sf::radians(tmpRot[1]))); shape->setRotation(sf::Angle(sf::radians(tmpRot[0])));
shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]}); shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
shape->setScale({1,cos(float(tmpRot[1]))});
return shape;} return shape;}
break; break;
src/pieces/methods/coscia_class.cpp
+12 -2
View File
@@ -28,16 +28,26 @@ sf::Shape* Coscia::draw(ReferencePlane plane){
case ReferencePlane::XZ: case ReferencePlane::XZ:
{ {
sf::Shape* shape = shapeXZ; sf::Shape* shape = shapeXZ;
shape->setRotation(sf::Angle(sf::radians(tmpRot[2]))); shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]}); shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
//calcolo ridimensionamento dato da cos(x)-> questo per definire l'ancoraggio corretto del pivot
shape->setScale({1,cos(float(tmpRot[0]))});
//shape->setScale({1,(0.5* cos(float(tmpRot[0]*2)))+0.5});
return shape;} return shape;}
break; break;
case ReferencePlane::YZ: case ReferencePlane::YZ:
{ {
sf::Shape* shape = shapeYZ; sf::Shape* shape = shapeYZ;
shape->setRotation(sf::Angle(sf::radians(tmpRot[1]))); shape->setRotation(sf::Angle(sf::radians(tmpRot[0])));
shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]}); shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
//calcolo ridimensionamento dato da cos(x) -> questo per definire l'ancoraggio corretto del pivot
shape->setScale({1,cos(float(tmpRot[1]))});
//shape->setScale({1,(0.5* cos(float(tmpRot[1]*2)))+0.5});
return shape;} return shape;}
break; break;
src/pieces/methods/sensore_class.cpp
+9 -6
View File
@@ -33,6 +33,9 @@ void Sensore::initCSV(std::vector<std::vector<float>> data){
std::vector<float> tmpA = {row[5],row[6],row[4]}; std::vector<float> tmpA = {row[5],row[6],row[4]};
std::vector<float> tmpG = {-row[8],-row[9],-row[7]}; std::vector<float> tmpG = {-row[8],-row[9],-row[7]};
/////// DA CAMBIARE QUI ///////////
rotData.push_back(tmpR); rotData.push_back(tmpR);
accData.push_back(tmpA); accData.push_back(tmpA);
gData.push_back(tmpG); gData.push_back(tmpG);
@@ -66,7 +69,7 @@ sf::Shape* Sensore::draw(ReferencePlane plane){
case ReferencePlane::XZ: case ReferencePlane::XZ:
{ {
sf::Shape* shape = shapeXZ; sf::Shape* shape = shapeXZ;
shape->setRotation(sf::Angle(sf::radians(tmpRot[2]))); shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]}); shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
return shape;} return shape;}
break; break;
@@ -74,7 +77,7 @@ sf::Shape* Sensore::draw(ReferencePlane plane){
case ReferencePlane::YZ: case ReferencePlane::YZ:
{ {
sf::Shape* shape = shapeYZ; sf::Shape* shape = shapeYZ;
shape->setRotation(sf::Angle(sf::radians(tmpRot[1]))); shape->setRotation(sf::Angle(sf::radians(tmpRot[0])));
shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]}); shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
return shape;} return shape;}
break; break;
@@ -98,11 +101,11 @@ void Sensore::calcRotWithG(unsigned int index){ // calcolo rotazione con valori
float tmpSinY = -grav[1] / modG; float tmpSinY = -grav[1] / modG;
float tmpSinZ = -grav[2] / modG; float tmpSinZ = -grav[2] / modG;
float tmpAX = acos(tmpSinY); float tmpAX = acos(tmpSinX);
float tmpAY = acos(tmpSinZ); float tmpAY = acos(tmpSinY);
float tmpAZ = acos(tmpSinX); float tmpAZ = acos(tmpSinZ);
body.setRot(rb::Vector3_s{_Float16( tmpAX),_Float16( tmpAY),_Float16( tmpAZ) }); body.setRot(rb::Vector3_s{_Float16( tmpAY),_Float16( tmpAX),_Float16( tmpAZ) });
} }
/* /*
src/pieces/methods/torso.cpp
+2 -2
View File
@@ -29,7 +29,7 @@ sf::Shape* Torso::draw(ReferencePlane plane){
case ReferencePlane::XZ: case ReferencePlane::XZ:
{ {
sf::Shape* shape = shapeXZ; sf::Shape* shape = shapeXZ;
shape->setRotation(sf::Angle(sf::radians(tmpRot[2]))); shape->setRotation(sf::Angle(sf::radians(tmpRot[1])));
shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]}); shape->setPosition({tmpPos[0]+globalPos[0],tmpPos[2]+globalPos[2]});
return shape;} return shape;}
break; break;
@@ -37,7 +37,7 @@ sf::Shape* Torso::draw(ReferencePlane plane){
case ReferencePlane::YZ: case ReferencePlane::YZ:
{ {
sf::Shape* shape = shapeYZ; sf::Shape* shape = shapeYZ;
shape->setRotation(sf::Angle(sf::radians(tmpRot[1]))); shape->setRotation(sf::Angle(sf::radians(tmpRot[0])));
shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]}); shape->setPosition({tmpPos[1]+globalPos[1],tmpPos[2]+globalPos[2]});
return shape;} return shape;}
break; break;
src/sfml_util.cpp
+27 -8
View File
@@ -85,23 +85,39 @@ void handle(const sf::Event::MouseMoved &mouseMoved, State &gs)
{ {
sf::Vector2i offset = mouseMoved.position - gs.mouse_pos; sf::Vector2i offset = mouseMoved.position - gs.mouse_pos;
gs.mouse_pos = mouseMoved.position; gs.mouse_pos = mouseMoved.position;
float px = 1;
float py = 0;
switch (gs.selectedPlane)
{
case ReferencePlane::XZ:
px = 1;
py = 0;
break;
case ReferencePlane::YZ:
px = 0;
py = 1;
break;
default:
break;
}
if (gs.drag){ if (gs.drag){
for(PieceInterface* p : gs.pieces){ for(PieceInterface* p : gs.pieces){
p->globalPos = {p->globalPos[0] + offset.x, p->globalPos[1],p->globalPos[2] + offset.y}; p->globalPos = {p->globalPos[0] + (offset.x * px), p->globalPos[1]+ (offset.x * py),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){ if (gs.selected != -1 && gs.drag_Piece){
rb::Vector3 tmp = gs.pieces[gs.selected]->body.getPos(); 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}); gs.pieces[gs.selected]->body.setPos({tmp[0]+ (offset.x * px),tmp[1]+ (offset.x * py),tmp[2]+offset.y});
} }
if (gs.selected != -1 && gs.rot_Piece){ if (gs.selected != -1 && gs.rot_Piece){
rb::Vector3_s tmp = gs.pieces[gs.selected]->body.getRot(); rb::Vector3_s tmp = gs.pieces[gs.selected]->body.getRot();
_Float16 nrot = _Float16(offset.x)/10; _Float16 nrot = _Float16(offset.x)/10;
gs.pieces[gs.selected]->body.setRot({tmp[0],tmp[1],tmp[2]+nrot}); gs.pieces[gs.selected]->body.setRot({tmp[0]+(nrot*_Float16(py)),tmp[1]+(nrot*_Float16(px)),tmp[2]});
//printf("Rotation : %f,%f,%f \n",gs.pieces[gs.selected]->body.getRot()[0],gs.pieces[gs.selected]->body.getRot()[1],gs.pieces[gs.selected]->body.getRot()[2]);
} }
@@ -115,8 +131,11 @@ void handle(const sf::Event::MouseButtonPressed &mouseBP, State &gs)
gs.drag_Piece = true; gs.drag_Piece = true;
int i = 0; int i = 0;
for (PieceInterface* p : gs.pieces){ for (PieceInterface* p : gs.pieces){
sf::Vector2f pos;
sf::Vector2f pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]}; if (gs.selectedPlane == ReferencePlane::XZ)
pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]};
else if (gs.selectedPlane == ReferencePlane::YZ)
pos = {p->globalPos[1]+ p->body.getPos()[1], p->globalPos[2]+ p->body.getPos()[2]};
if (dist(pos,mouseBP.position) < 20){ if (dist(pos,mouseBP.position) < 20){
gs.selected = i; gs.selected = i;
src/testMain.cpp
+7 -7
View File
@@ -38,30 +38,30 @@ int main() {
const auto& coscia = processor.getData(); const auto& coscia = processor.getData();
gs.pieces.push_back(new Coscia (rb::Vector3{300,10,300},2)); gs.pieces.push_back(new Coscia (rb::Vector3{300,300,300},2));
gs.pieces.push_back(new Sensore (rb::Vector3{300,300,300},_Float16( 0.2 ),900,3000,coscia)); 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.push_back(new Caviglia (rb::Vector3{300,300,500},1));
gs.pieces[1]->body.setRot({0,0,0}); gs.pieces[1]->body.setRot({0,0,0});
processor.readCSVFile(DATA_PATH + "caviglia_filt.csv"); processor.readCSVFile(DATA_PATH + "caviglia_filt.csv");
const auto& caviglia = processor.getData(); 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 Sensore (rb::Vector3{300,300,500},_Float16( 0.2 ),900,3000,caviglia));
gs.pieces.push_back(new Torso(rb::Vector3{300,400,150},2)); gs.pieces.push_back(new Torso(rb::Vector3{300,400,150},2));
// modifico la rotazione relativa della gamba // modifico la rotazione relativa della gamba
gs.pieces[1]->body.setRot({0,0,_Float16 (1.6)}); gs.pieces[1]->body.setRot({_Float16 (1.3),_Float16 (1.5),0});
gs.pieces[3]->body.setRot({0,0,_Float16 (1.7)}); gs.pieces[3]->body.setRot({_Float16 (1.8),_Float16 (1.5),0});
// aggiungo i joint // aggiungo i joint
gs.joints.push_back(new PivotJoint(gs.pieces[4], {gs.pieces[1]}, rb::Vector3{0,-100,50}));
gs.joints.push_back(new RigidJoint(gs.pieces[1], {gs.pieces[0]})); gs.joints.push_back(new 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 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 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"); printf("Ho costruito tutto!\n");