[C++] waypoint : add size() and == methods

include/hpp/bezier-com-traj/utils.hh

@@ -33,6 +33,16 @@ struct waypoint_t {
   waypoint_t(MatrixXX A, VectorX b) : first(A), second(b) {}
 
   static waypoint_t Zero(size_t dim) { return initwp(dim, dim); }
+
+  size_t size() const{return second.size();}
+
+  bool isApprox(const waypoint_t& other, const value_type prec = Eigen::NumTraits<value_type>::dummy_precision()) const{
+    return first.isApprox(other.first,prec) && second.isApprox(other.second,prec);
+  }
+
+  bool operator==(const waypoint_t& other) const{ return isApprox(other); }
+
+  bool operator!=(const waypoint_t& other) const{ return !(*this == other); }
 };
 
 /**