Update physics math functions and add collider setters

src/physics/World.cpp

@@ -6,6 +6,9 @@ World::World(scalar timestep, int substeps)
 {
  this->timestep = timestep;
  this->substeps = substeps;
+ // this->collision_manager_a = new hpp::fcl::DynamicAABBTreeArrayCollisionManager();
+
+ // this->collision_manager_a->setup();
 };
 
 void World::step()
@@ -14,13 +17,12 @@ void World::step()
  scalar inv_h = 1 / h;
 
  this->broad_phase_collision_detection();
- for (ContactConstraint &constraint : this->contact_contraints)
+ for (ContactConstraint &constraint : this->contact_constraints)
  {
  constraint.reset_lagrange_multipliers();
  }
  for (int i = 0; i < this->substeps; i++)
  {
-
  this->update_bodies_position_and_orientation(h);
  this->solve_positions(inv_h, h);
  this->update_bodies_velocities(inv_h);
@@ -46,7 +48,7 @@ void World::update_bodies_velocities(scalar inv_h)
 
 void World::solve_positions(scalar inv_h, scalar h)
 {
- for (ContactConstraint &constraint : this->contact_contraints)
+ for (ContactConstraint &constraint : this->contact_constraints)
  {
  constraint.apply_constraint(inv_h);
  }
@@ -66,7 +68,7 @@ void World::solve_positions(scalar inv_h, scalar h)
 }
 void World::solve_velocities(scalar h)
 {
- for (ContactConstraint &constraint : this->contact_contraints)
+ for (ContactConstraint &constraint : this->contact_constraints)
  {
  constraint.apply_constraint_velocity_level(h);
  }
@@ -123,17 +125,17 @@ void World::set_body_plane_collider(int id, vec3 normal, scalar offset)
 
 void World::set_body_box_collider(int id, vec3 half_extents)
 {
- this->bodies[id].set_box_collider(half_extents);
+ this->bodies[id].set_box_collider(half_extents, true);
 }
 
 void World::set_body_cylinder_collider(int id, scalar radius, scalar height)
 {
- this->bodies[id].set_cylinder_collider(radius, height);
+ this->bodies[id].set_cylinder_collider(radius, height, true);
 }
 
 void World::set_body_sphere_collider(int id, scalar radius)
 {
- this->bodies[id].set_sphere_collider(radius);
+ this->bodies[id].set_sphere_collider(radius, true);
 }
 
 void World::set_body_capsule_collider(int id, scalar radius, scalar height)
@@ -222,8 +224,8 @@ int World::create_contact_constraint(int body_1_id,
  ContactConstraint constraint = ContactConstraint(&this->bodies[body_1_id],
  &this->bodies[body_2_id]);
 
- this->contact_contraints.push_back(constraint);
- return (int)(this->contact_contraints.size() - 1);
+ this->contact_constraints.push_back(constraint);
+ return (int)(this->contact_constraints.size() - 1);
 }
 
 int World::add_rotational_constraint(RotationalConstraint constraint)
@@ -263,7 +265,7 @@ AABB World::get_aabb(int id)
  // .min = aabb.min - expansion_factor,
  // .max = aabb.max + expansion_factor,
  // };
- 
+
  return aabb;
 }
 
@@ -276,21 +278,69 @@ void World::collisions_detection_preparations(void)
  {
  for (int j = i + 1; j < n_bodies; j++)
  {
- int  id = this->create_contact_constraint(i, j);
- body_pair_to_contact_constraint_map.insert({{i,j}, id});
+ int id = this->create_contact_constraint(i, j);
+ this->body_pair_to_contact_constraint_map.insert({{i, j}, id});
  }
  }
+
+ // for (int i = 0; i < n_bodies; i++)
+ // {
+ // hpp::fcl::CollisionObject *obj = new hpp::fcl::CollisionObject(bodies[i].collider_info,
+ // ti::get_eigen_transform(bodies[i].position,
+ // bodies[i].orientation));
+
+ // int *id_ptr = new int(i);
+ // obj->setUserData(id_ptr);
+
+ // this->collision_objects.push_back(obj);
+ // }
+
+ // this->collision_manager_a->clear();
+ // this->collision_manager_a->registerObjects(this->collision_objects);
 }
 
 // Collisions
 void World::broad_phase_collision_detection(void)
 {
 
- for (ContactConstraint &constraint : this->contact_contraints)
+ // size_t num_bodies = this->get_number_of_bodies();
+
+ // hpp::fcl::CollisionCallBackCollect *collision_callback = new hpp::fcl::CollisionCallBackCollect(num_bodies * (num_bodies - 1) / 2);
+
+ // for (int i = 0; i < num_bodies; i++)
+ // {
+ // this->collision_objects[i]->setTransform(ti::get_eigen_transform(bodies[i].position, bodies[i].orientation));
+ // this->collision_objects[i]->computeAABB();
+ // }
+
+ // this->collision_manager_a->update(this->collision_objects);
+
+ // this->collision_manager_a->collide(collision_callback);
+
+ // for (auto &constraint : this->contact_constraints)
+ // {
+ // constraint.broad_phase_detection = false;
+ // }
+
+ // for (auto &col_pair : collision_callback->getCollisionPairs())
+ // {
+ // int id_1 = *reinterpret_cast<int *>(col_pair.first->getUserData());
+ // int id_2 = *reinterpret_cast<int *>(col_pair.second->getUserData());
+ // if (id_1 > id_2)
+ // {
+ // std::swap(id_1, id_2);
+ // };
+ // size_t constraint_id = this->body_pair_to_contact_constraint_map[{id_1, id_2}];
+ // this->contact_constraints[constraint_id].broad_phase_detection = true;
+
+ // }
+
+ // delete collision_callback;
+
+ for (auto &constraint : this->contact_constraints)
  {
- constraint.check_broad_phase(timestep);
+ constraint.check_broad_phase(this->timestep);
  }
-
 }
 
 // Adding plane:

src/physics/World.hpp

@@ -11,6 +11,10 @@
 #include <tuple>
 #include <map>
 #include "aabb_tree.hpp"
+#include "hpp/fcl/broadphase/broadphase_collision_manager.h"
+#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h>
+#include <hpp/fcl/broadphase/default_broadphase_callbacks.h>
+
 // Create a blank class called World
 namespace robosim
 {
@@ -33,8 +37,7 @@ namespace robosim
  std::vector<std::tuple<int, int, bool>> broad_phase_detections;
  AABBTree aabb_tree = AABBTree();
 
- // Planes
- // Static body used to solve the plane contact constraints
+ // Plane (if used has a reserved attribute in the world class)
  int plane_body_idx = -1; // Default (-1) (you wont get anything!)
 
 
@@ -44,7 +47,7 @@ namespace robosim
  void solve_velocities(scalar inv_h);
 
  public:
- std::vector<ContactConstraint> contact_contraints;
+ std::vector<ContactConstraint> contact_constraints;
  // Constructor
  World(scalar timestep = 0.01, int substeps = 20);
  // Destructor

src/physics/bodies/Body.cpp

@@ -158,26 +158,54 @@ void Body::apply_positional_velocity_constraint_impulse(vec3 impulse, vec3 r){
 
 }
 
+void Body::set_intertia_tensor(const mat3 &intertia_tensor){
+
+ this->inertia_tensor = inertia_tensor;
+ this->inverse_inertia_tensor = ti::inverse(this->inertia_tensor);
+ this->update_inertia_tensor_world();
+}
 
-void Body::set_box_collider(vec3 half_extents){
+void Body::set_box_collider(vec3 half_extents, bool recompute_inertia){
  this->collider_info = std::make_shared<hpp::fcl::Box>(
  half_extents.x * 2.0, 
  half_extents.y * 2.0, 
  half_extents.z * 2.0);
+
+ if (recompute_inertia && this->type == BodyType::DYNAMIC){
+ scalar vol = this->collider_info->computeVolume();
+ inertia_tensor = ti::mar3_from_eigen(this->collider_info->computeMomentofInertiaRelatedToCOM())/vol;
+ set_intertia_tensor(inertia_tensor * this->mass);
+ }
+
 }
 
-void Body::set_sphere_collider(scalar radius){
+void Body::set_sphere_collider(scalar radius, bool recompute_inertia){
  this->collider_info = std::make_shared<hpp::fcl::Sphere>(radius);
+ if (recompute_inertia && this->type == BodyType::DYNAMIC){
+ scalar vol = this->collider_info->computeVolume();
+ inertia_tensor = ti::mar3_from_eigen(this->collider_info->computeMomentofInertiaRelatedToCOM())/vol;
+ set_intertia_tensor(inertia_tensor * this->mass);
+ }
 }
 
-void Body::set_capsule_collider(scalar radius, scalar height){
+void Body::set_capsule_collider(scalar radius, scalar height, bool recompute_inertia){
  this->collider_info = std::make_shared<hpp::fcl::Capsule>( radius, height);
+ if (recompute_inertia && this->type == BodyType::DYNAMIC){
+ scalar vol = this->collider_info->computeVolume();
+ inertia_tensor = ti::mar3_from_eigen(this->collider_info->computeMomentofInertiaRelatedToCOM())/vol;
+ set_intertia_tensor(inertia_tensor * this->mass);
+ }
 }
 
-void Body::set_cylinder_collider(scalar radius, scalar height){
+void Body::set_cylinder_collider(scalar radius, scalar height, bool recompute_inertia){
  this->collider_info = std::make_shared<hpp::fcl::Cylinder>( radius, height);
+ if (recompute_inertia && this->type == BodyType::DYNAMIC){
+ scalar vol = this->collider_info->computeVolume();
+ inertia_tensor = ti::mar3_from_eigen(this->collider_info->computeMomentofInertiaRelatedToCOM())/vol;
+ set_intertia_tensor(inertia_tensor * this->mass);
+ }
 }
 
 void Body::set_plane_collider(vec3 normal, scalar offset){
- this->collider_info = std::make_shared<hpp::fcl::Plane>(normal.x, normal.y, normal.z, offset);
+ this->collider_info = std::make_shared<hpp::fcl::Halfspace>(normal.x, normal.y, normal.z, offset);
 }

src/physics/bodies/Body.hpp

@@ -73,7 +73,7 @@ class Body
  scalar get_rotational_generalized_inverse_mass(vec3 n);
 
  void update_inertia_tensor_world();
- void update_inverse_inertia_tensor_world();
+ //void update_inverse_inertia_tensor_world();
 
  void update_position_and_orientation(scalar time_step);
 
@@ -116,13 +116,15 @@ class Body
 
  void apply_positional_velocity_constraint_impulse(vec3 impulse, vec3 r);
 
- void set_box_collider(vec3 half_extents);
+ void set_box_collider(vec3 half_extents, bool recompute_inertia = true);
 
- void set_sphere_collider(scalar radius);
+ void set_sphere_collider(scalar radius, bool recompute_inertia = true);
 
- void set_capsule_collider(scalar radius, scalar height);
+ void set_capsule_collider(scalar radius, scalar height, bool recompute_inertia = true);
 
- void set_cylinder_collider(scalar radius, scalar height);
+ void set_cylinder_collider(scalar radius, scalar height, bool recompute_inertia = true);
 
  void set_plane_collider(vec3 normal, scalar offset);
+
+ void set_intertia_tensor(const mat3 &intertia_tensor);
 };

src/physics/math/math.cpp

@@ -31,10 +31,27 @@ namespace ti
  return (vec3){v[0], v[1], v[2]};
  }
 
+ mat3 mar3_from_eigen(const hpp::fcl::Matrix3f &mat){
+ return (mat3){
+ mat(0, 0), mat(0, 1), mat(0, 2),
+ mat(0, 1), mat(1, 1), mat(1, 2),
+ mat(0, 2), mat(1, 2), mat(2, 2)
+ };
+ }
+
  quat from_eigen(hpp::fcl::Quaternion3f q)
  {
  return (quat){q.x(), q.y(), q.z(), q.w()};
  }
+
+
+ hpp::fcl::Vec3f to_eigen(vec3 v)
+ {
+
+ return (hpp::fcl::Vec3f){v[0], v[1], v[2]};
+ }
+
+
 };
 
 std::string ToString(const vec2 &v, int precision)

src/physics/math/math.hpp

@@ -92,5 +92,9 @@ namespace ti
 
  vec3 from_eigen(hpp::fcl::Vec3f v);
 
+ mat3 mar3_from_eigen(const hpp::fcl::Matrix3f &mat);
+
+ hpp::fcl::Vec3f to_eigen(vec3 v);
+
  quat from_eigen(hpp::fcl::Quaternion3f q);
 };