Aggiunte e modifiche per lower_body

src/collections/headers/collection_interface.hpp

@@ -17,4 +17,15 @@ class CollectionInterface{
 };
 
 
-#endif
+#endif
+
+inline collection operator+(collection c1, collection c2){
+    collection res;
+    for (PieceInterface* i : c1.pieces){
+        res.pieces.push_back(i);
+    }
+    for (JointInterface* i : c1.joints){
+        res.joints.push_back(i);
+    }
+    return res;
+}

src/collections/headers/gamba.hpp

@@ -12,6 +12,7 @@ class Gamba : public CollectionInterface {
     public:
         Gamba(rb::Vector3 pos, unsigned int* dataPos, std::string cosciaData, std::string cavigliaData);
         collection create(ReferencePlane plane) override;
+        PieceInterface* getJointPiece();
 };
 
 #endif

src/collections/headers/lower_body.hpp

@@ -4,23 +4,30 @@
 #define LOWER_BODY_H
 
 struct gamba_data{
-    rb::Vector3 pos; 
     unsigned int* dataPos; 
     std::string cosciaData; 
     std::string cavigliaData;
 };
 
 
-class lower_body
-{
+class Lower_Body : CollectionInterface{
 protected:
-    Gamba sx;
-    Gamba dx;
-    Torso t;
+    Gamba* sx;
+    Gamba* dx;
+    Torso* t;
+
+    PivotJoint* jsx;
+    PivotJoint* jdx;
+
+    bool visible = true;
+    float alpha = 1;
 
 public:
-    lower_body(std::vector<gamba_data> data);
-    ~lower_body();
+    Lower_Body(std::vector<gamba_data> data);
+    ~Lower_Body();
+
+    void setVisibility(bool c);
+    collection create(ReferencePlane plane) override;
 };
 
 

src/collections/methods/gamba.cpp

@@ -43,4 +43,8 @@ collection Gamba::create(ReferencePlane plane){
     }
 
     return coll;
+}
+
+PieceInterface* Gamba::getJointPiece(){
+    return sensori[0];
 }

src/collections/methods/lower_body.cpp

@@ -0,0 +1,23 @@
+#include "../headers/lower_body.hpp"
+
+Lower_Body::Lower_Body(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));
+
+    PieceInterface* psx = sx->getJointPiece();
+    PieceInterface* pdx = dx->getJointPiece();
+
+    jsx = new PivotJoint(t, {psx}, rb::Vector3{0,-100,50});
+    jdx = new PivotJoint(t, {pdx}, rb::Vector3{0,100,50});
+}
+
+
+collection Lower_Body::create(ReferencePlane plane){
+    collection coll;
+    coll = coll + sx->create(plane);
+    coll = coll + dx->create(plane);
+
+
+}