diff --git a/CMakeLists.txt b/CMakeLists.txt index 83ec509..0a33b35 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,7 +23,7 @@ FetchContent_MakeAvailable(glm) set(METHODS_PATH "./src/*/methods/*.cpp") -set(VERSION "V3") +set(VERSION "V4") file(GLOB_RECURSE METHODS_SRC "${METHODS_PATH}") add_executable(main${VERSION} ./src/testMain.cpp ${METHODS_SRC} ) diff --git a/build/.cmake/api/v1/reply/codemodel-v2-48a6e2c377bb606c3455.json b/build/.cmake/api/v1/reply/codemodel-v2-413561486a356cd66df3.json similarity index 99% rename from build/.cmake/api/v1/reply/codemodel-v2-48a6e2c377bb606c3455.json rename to build/.cmake/api/v1/reply/codemodel-v2-413561486a356cd66df3.json index 942b54e..84e6068 100644 --- a/build/.cmake/api/v1/reply/codemodel-v2-48a6e2c377bb606c3455.json +++ b/build/.cmake/api/v1/reply/codemodel-v2-413561486a356cd66df3.json @@ -573,9 +573,9 @@ }, { "directoryIndex" : 0, - "id" : "mainV3::@6890427a1f51a3e7e1df", - "jsonFile" : "target-mainV3-Debug-ee31761362aa79611c23.json", - "name" : "mainV3", + "id" : "mainV4::@6890427a1f51a3e7e1df", + "jsonFile" : "target-mainV4-Debug-fc5cbebaffd99dc9855d.json", + "name" : "mainV4", "projectIndex" : 0 }, { diff --git a/build/.cmake/api/v1/reply/index-2026-05-31T08-58-45-0525.json b/build/.cmake/api/v1/reply/index-2026-05-31T09-56-00-0692.json similarity index 94% rename from build/.cmake/api/v1/reply/index-2026-05-31T08-58-45-0525.json rename to build/.cmake/api/v1/reply/index-2026-05-31T09-56-00-0692.json index ea67de3..c32c7d3 100644 --- a/build/.cmake/api/v1/reply/index-2026-05-31T08-58-45-0525.json +++ b/build/.cmake/api/v1/reply/index-2026-05-31T09-56-00-0692.json @@ -26,7 +26,7 @@ "objects" : [ { - "jsonFile" : "codemodel-v2-48a6e2c377bb606c3455.json", + "jsonFile" : "codemodel-v2-413561486a356cd66df3.json", "kind" : "codemodel", "version" : { @@ -99,7 +99,7 @@ } }, { - "jsonFile" : "codemodel-v2-48a6e2c377bb606c3455.json", + "jsonFile" : "codemodel-v2-413561486a356cd66df3.json", "kind" : "codemodel", "version" : { diff --git a/build/.cmake/api/v1/reply/target-mainV3-Debug-ee31761362aa79611c23.json b/build/.cmake/api/v1/reply/target-mainV4-Debug-fc5cbebaffd99dc9855d.json similarity index 98% rename from build/.cmake/api/v1/reply/target-mainV3-Debug-ee31761362aa79611c23.json rename to build/.cmake/api/v1/reply/target-mainV4-Debug-fc5cbebaffd99dc9855d.json index f4c3d84..6f6c717 100644 --- a/build/.cmake/api/v1/reply/target-mainV3-Debug-ee31761362aa79611c23.json +++ b/build/.cmake/api/v1/reply/target-mainV4-Debug-fc5cbebaffd99dc9855d.json @@ -2,7 +2,7 @@ "artifacts" : [ { - "path" : "bin/mainV3" + "path" : "bin/mainV4" } ], "backtrace" : 1, @@ -193,7 +193,7 @@ "id" : "glm::@ce54d98d7b326a3e12a8" } ], - "id" : "mainV3::@6890427a1f51a3e7e1df", + "id" : "mainV4::@6890427a1f51a3e7e1df", "link" : { "commandFragments" : @@ -296,8 +296,8 @@ "id" : "glm::@ce54d98d7b326a3e12a8" } ], - "name" : "mainV3", - "nameOnDisk" : "mainV3", + "name" : "mainV4", + "nameOnDisk" : "mainV4", "paths" : { "build" : ".", diff --git a/build/bin/mainV4 b/build/bin/mainV4 new file mode 100755 index 0000000..8d02f1b Binary files /dev/null and b/build/bin/mainV4 differ diff --git a/src/joints/methods/pivot_joint_class.cpp b/src/joints/methods/pivot_joint_class.cpp index 9c73707..b8a8117 100644 --- a/src/joints/methods/pivot_joint_class.cpp +++ b/src/joints/methods/pivot_joint_class.cpp @@ -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 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 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; /* diff --git a/src/joints/methods/rigid_joint_class.cpp b/src/joints/methods/rigid_joint_class.cpp index 1e9beaf..fb09a74 100644 --- a/src/joints/methods/rigid_joint_class.cpp +++ b/src/joints/methods/rigid_joint_class.cpp @@ -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){ diff --git a/src/pieces/methods/caviglia_class.cpp b/src/pieces/methods/caviglia_class.cpp index d3be389..0818c5e 100644 --- a/src/pieces/methods/caviglia_class.cpp +++ b/src/pieces/methods/caviglia_class.cpp @@ -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; diff --git a/src/pieces/methods/coscia_class.cpp b/src/pieces/methods/coscia_class.cpp index ce882e1..14f5ffa 100644 --- a/src/pieces/methods/coscia_class.cpp +++ b/src/pieces/methods/coscia_class.cpp @@ -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; diff --git a/src/pieces/methods/sensore_class.cpp b/src/pieces/methods/sensore_class.cpp index 03611c6..aa16df0 100644 --- a/src/pieces/methods/sensore_class.cpp +++ b/src/pieces/methods/sensore_class.cpp @@ -33,6 +33,9 @@ void Sensore::initCSV(std::vector> data){ std::vector tmpA = {row[5],row[6],row[4]}; std::vector 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) }); } /* diff --git a/src/pieces/methods/torso.cpp b/src/pieces/methods/torso.cpp index c85f899..ec82add 100644 --- a/src/pieces/methods/torso.cpp +++ b/src/pieces/methods/torso.cpp @@ -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; diff --git a/src/sfml_util.cpp b/src/sfml_util.cpp index 9ce978e..e86f9c6 100644 --- a/src/sfml_util.cpp +++ b/src/sfml_util.cpp @@ -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; diff --git a/src/testMain.cpp b/src/testMain.cpp index eafa386..2241d50 100755 --- a/src/testMain.cpp +++ b/src/testMain.cpp @@ -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");