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
src/joints/methods/pivot_joint_class.cpp
+42 -19
View File
@@ -9,34 +9,57 @@ void PivotJoint::rotate(unsigned int id){
//// 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 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 sinB = glm::sin(beta);
float beta1 = float (cRot[1] - oldCRot[id][1]);
float cosB1 = glm::cos(beta1);
float sinB1 = glm::sin(beta1);
glm::mat4 Rpx = glm::mat4{
1 , 0, 0, 0,
0, cosA, sinA, 0,
0, -sinA, cosA, 0,
0, 0, 0, 1
};
glm::mat3 R1 = glm::mat3(
/*cos*/ cosA, /*sin*/ sinA, 0,
/*-sin*/ -sinA , /*cos*/ cosA, 0,
0, 0, 1
);
glm::mat4 Rpy = glm::mat4{
cosA1 , 0, sinA1, 0,
0, 1, 0, 0,
-sinA1, 0, cosA1, 0,
0, 0, 0, 1
};
glm::mat4 Rcx = glm::mat4{
1 , 0, 0, 0,
0, cosB, sinB, 0,
0, -sinB, cosB, 0,
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]};
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]});
//printf("Offset = %f %f %f \n" , offN[0], offN[1], offN[2]);
oldRot = fRot; //aggiorno la rotazione per il ciclo successivo
oldCRot[id] = cRot;
@@ -55,7 +78,7 @@ PivotJoint::PivotJoint(PieceInterface* father,std::vector<PieceInterface*> child
rb::Vector3_s fRot = father->body.getRot();
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;
@@ -69,7 +92,7 @@ PivotJoint::PivotJoint(PieceInterface* father,std::vector<PieceInterface*> child
//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();
rb::Vector3 cCoords = c->body.getPos();
tmpCoords = cCoords - pivotCenter;
/*
src/joints/methods/rigid_joint_class.cpp
+33 -15
View File
@@ -12,30 +12,48 @@ void RigidJoint::rotate(unsigned int id){
childs[id]->body.setRot(fRot + rotOffset[id]);
// sposto il alla distanza offset rispetto all'origine R*pos
//passo a coordinate 3D per calcolare rotazione sul piano YZ oltre che al piano XZ
// 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 sinA = glm::sin(alpha);
float cosB = glm::cos(beta);
float sinB = glm::sin(beta);
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::mat4 Rx = glm::mat4{
1 , 0, 0, 0,
0, cosA, sinA, 0,
0, -sinA, cosA, 0,
0, 0, 0, 1
};
glm::vec3 XZ_cPos = {offset[id][0],offset[id][2],1};
glm::vec3 resRot = R * XZ_cPos;
glm::mat4 Ry = glm::mat4{
cosB , 0, sinB, 0,
0, 1, 0, 0,
-sinB, 0, cosB, 0,
0, 0, 0, 1
};
offset[id][0] = resRot[0] ;
offset[id][2] = resRot[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]});
}
void RigidJoint::traslate(unsigned int id){
src/pieces/methods/caviglia_class.cpp
+4 -2
View File
@@ -29,16 +29,18 @@ sf::Shape* Caviglia::draw(ReferencePlane plane){
case ReferencePlane::XZ:
{
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->setScale({1,cos(float(tmpRot[0]))});
return shape;}
break;
case ReferencePlane::YZ:
{
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->setScale({1,cos(float(tmpRot[1]))});
return shape;}
break;
src/pieces/methods/coscia_class.cpp
+12 -2
View File
@@ -28,16 +28,26 @@ sf::Shape* Coscia::draw(ReferencePlane plane){
case ReferencePlane::XZ:
{
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]});
//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;}
break;
case ReferencePlane::YZ:
{
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]});
//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;}
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> tmpG = {-row[8],-row[9],-row[7]};
/////// DA CAMBIARE QUI ///////////
rotData.push_back(tmpR);
accData.push_back(tmpA);
gData.push_back(tmpG);
@@ -66,7 +69,7 @@ sf::Shape* Sensore::draw(ReferencePlane plane){
case ReferencePlane::XZ:
{
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]});
return shape;}
break;
@@ -74,7 +77,7 @@ sf::Shape* Sensore::draw(ReferencePlane plane){
case ReferencePlane::YZ:
{
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]});
return shape;}
break;
@@ -98,11 +101,11 @@ void Sensore::calcRotWithG(unsigned int index){ // calcolo rotazione con valori
float tmpSinY = -grav[1] / modG;
float tmpSinZ = -grav[2] / modG;
float tmpAX = acos(tmpSinY);
float tmpAY = acos(tmpSinZ);
float tmpAZ = acos(tmpSinX);
float tmpAX = acos(tmpSinX);
float tmpAY = acos(tmpSinY);
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:
{
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]});
return shape;}
break;
@@ -37,7 +37,7 @@ sf::Shape* Torso::draw(ReferencePlane plane){
case ReferencePlane::YZ:
{
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]});
return shape;}
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;
gs.mouse_pos = mouseMoved.position;
float px = 1;
float py = 0;
switch (gs.selectedPlane)
{
case ReferencePlane::XZ:
px = 1;
py = 0;
break;
case ReferencePlane::YZ:
px = 0;
py = 1;
break;
default:
break;
}
if (gs.drag){
for(PieceInterface* p : gs.pieces){
p->globalPos = {p->globalPos[0] + offset.x, 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
p->globalPos = {p->globalPos[0] + (offset.x * px), p->globalPos[1]+ (offset.x * py),p->globalPos[2] + offset.y};
}
}
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});
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){
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});
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;
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]};
sf::Vector2f pos;
if (gs.selectedPlane == ReferencePlane::XZ)
pos = {p->globalPos[0]+ p->body.getPos()[0], p->globalPos[2]+ p->body.getPos()[2]};
else if (gs.selectedPlane == ReferencePlane::YZ)
pos = {p->globalPos[1]+ p->body.getPos()[1], p->globalPos[2]+ p->body.getPos()[2]};
if (dist(pos,mouseBP.position) < 20){
gs.selected = i;
src/testMain.cpp
+8 -8
View File
@@ -38,30 +38,30 @@ int main() {
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 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});
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 Sensore (rb::Vector3{300,300,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)});
gs.pieces[1]->body.setRot({_Float16 (1.3),_Float16 (1.5),0});
gs.pieces[3]->body.setRot({_Float16 (1.8),_Float16 (1.5),0});
// aggiungo i joint
gs.joints.push_back(new PivotJoint(gs.pieces[4], {gs.pieces[1]}, rb::Vector3{0,-100,50}));
gs.joints.push_back(new RigidJoint(gs.pieces[1], {gs.pieces[0]}));
gs.joints.push_back(new PivotJoint(gs.pieces[1], {gs.pieces[3]}, rb::Vector3{0,0,100}));
gs.joints.push_back(new RigidJoint(gs.pieces[3], {gs.pieces[2]}));
gs.joints.push_back(new PivotJoint(gs.pieces[4], {gs.pieces[1]}, rb::Vector3{0,0,50}));
printf("Ho costruito tutto!\n");