Test lower_body in main

src/collections/methods/lower_body.cpp

@@ -1,10 +1,11 @@
 #include "../headers/lower_body.hpp"
 
-Lower_Body::Lower_Body(std::vector<gamba_data> data){
+Lower_Body::Lower_Body(rb::Vector3 pos,std::vector<gamba_data> data){
     if (data.size() != 2) throw "Lower_Body_Error: data vector size must be 2";
-    sx = new Gamba({0,0,150},data[0].dataPos,data[0].cosciaData,data[0].cavigliaData);
-    dx = new Gamba({0,0,150},data[1].dataPos,data[1].cosciaData,data[1].cavigliaData);
-    t = new Torso({0,100,0},_Float16(3.0));
+
+    sx = new Gamba({pos[0],pos[1],pos[2]+150},data[0].dataPos,data[0].cosciaData,data[0].cavigliaData);
+    dx = new Gamba({pos[0],pos[1],pos[2]+150},data[1].dataPos,data[1].cosciaData,data[1].cavigliaData);
+    t = new Torso({pos[0],pos[1]+100,pos[2]},_Float16(3.0));
 
     PieceInterface* psx = sx->getJointPiece();
     PieceInterface* pdx = dx->getJointPiece();
@@ -16,8 +17,34 @@ Lower_Body::Lower_Body(std::vector<gamba_data> data){
 
 collection Lower_Body::create(ReferencePlane plane){
     collection coll;
-    coll = coll + sx->create(plane);
-    coll = coll + dx->create(plane);
 
 
+    switch (plane)
+    {
+    case ReferencePlane::XZ: case ReferencePlane::XZN:
+        coll = coll + sx->create(plane);
+        coll = coll + dx->create(plane);
+        break;
+    case ReferencePlane::YZ:
+        coll = coll + dx->create(plane);
+        coll = coll + sx->create(plane);
+        break;
+
+    default:
+        break;
+    }
+    
+    return coll;
+}
+
+Lower_Body::~Lower_Body(){
+    delete sx;
+    delete dx;
+    delete t;
+    delete jdx;
+    delete jsx;
+}
+
+void Lower_Body::setVisibility(bool c){
+
 }