fix operator overload

interface/Tools.h

@@ -26,7 +26,15 @@ struct Vector3D {
     return *this;
   }
 
-  Vector3D operator+=(Vector3D v) { return (*this + v); }
+  Vector3D operator+=(Vector3D v) { 
+    *this = *this + v;
+    return *this;
+  }
+
+  float distance(const Vector3D& v) const {
+    return sqrt(std::pow(this->x - v.x, 2) + std::pow(this->y - v.y, 2) +
+                std::pow(this->z - v.z, 2));
+  }
 };
 #endif
 
@@ -57,6 +65,14 @@ struct Vector4D {
   }
   float E() { return y; }
 
+  Vector3D position() const {
+    return Vector3D(this->x1, this->x2, this->x3);
+  }
+  float distance(Vector4D& v) const {
+    return sqrt(std::pow(this->x1 - v.x1, 2) + std::pow(this->x2 - v.x2, 2) +
+                std::pow(this->x3 - v.x3, 2) + std::pow(this->y - v.y, 2));
+  }
+
   Vector4D operator+=(Vector4D v) {
     this->x1 += v.x1;
     this->x2 += v.x2;