Added tests, removed unused test functions and 1 bug fix

blast/manipulator/manipulator.hpp

@@ -181,7 +181,9 @@ inline host_fn void dynamics(const Manipulator& manip, ManipulatorTempData& temp
                              modified_last_link_mass, modified_last_link_cog, modified_last_link_inertia);
     }
 
-    Vec3 modified_last_link_cdd = Qt[manip.n_joints - 1] * cdd[manip.n_joints - 1 - 1] + cross(wd[manip.n_joints - 1], modified_last_link_cog) + cross(w[manip.n_joints - 1], cross(w[manip.n_joints - 1], modified_last_link_cog));
+    // modify linear acceleration of cog to match its new position on the rigid body
+    Vec3 delta_cog              = modified_last_link_cog - manip.cog_offsets[manip.n_joints - 1];
+    Vec3 modified_last_link_cdd = cdd[manip.n_joints - 1] + cross(wd[manip.n_joints - 1], delta_cog) + cross(w[manip.n_joints - 1], cross(w[manip.n_joints - 1], delta_cog));
 
     f[joints - 1] = modified_last_link_mass * modified_last_link_cdd;
     n[joints - 1] = modified_last_link_inertia * wd[joints - 1] + cross(w[joints - 1], modified_last_link_inertia * w[joints - 1]) + cross(modified_last_link_cog, f[joints - 1]);

blast/world/CoDO.hpp

@@ -4,6 +4,22 @@
 namespace blast {
 
 
+// // Returns distance between an box and a point
+// inline real distance(const Box& box, const Vec3& point) {
+//   Mat3 Rtrans = transpose(box.rotation);
+
+//   Vec3 point_box = Rtrans * (point - box.center);
+
+//   Vec3  proj = {clamp(point_box.x, -box.extents.x, box.extents.x), clamp(point_box.y, -box.extents.y, box.extents.y), clamp(point_box.z, -box.extents.z, box.extents.z)};
+//   Array dist_in(3);
+//   dist_in[0]            = std::abs(point_box.x) - box.extents.x;
+//   dist_in[1]            = std::abs(point_box.y) - box.extents.y;
+//   dist_in[2]            = std::abs(point_box.z) - box.extents.z;
+//   real result_if_inside = max(dist_in);
+
+//   return result_if_inside > 0 ? norm(proj - point_box) : result_if_inside;
+// }
+
 // Returns distance between an box and a point
 inline real distance(const Box& box, const Vec3& point) {
   Mat3 Rtrans = transpose(box.rotation);
@@ -16,9 +32,7 @@ inline real distance(const Box& box, const Vec3& point) {
   dist_in[1]            = std::abs(point_box.y) - box.extents.y;
   dist_in[2]            = std::abs(point_box.z) - box.extents.z;
   real result_if_inside = max(dist_in);
-  if (max(dist_in) == 0.29048075959276026) {
-    int stop = 0;
-  }
+
   return result_if_inside > 0 ? norm(proj - point_box) : result_if_inside;
 }