container_box2d_inl.h

Go to the documentation of this file.

#ifndef CPP_COLLISION_INCLUDE_COLLISION_GRID_BOX2D_H_
#define CPP_COLLISION_INCLUDE_COLLISION_GRID_BOX2D_H_

#include <stdint.h>
#include "collision/collision_object.h"
#include "collision/shape_group.h"
#include "collision/solvers/primitive_collision_queries.h"

#include <box2d/b2_dynamic_tree.h>
#include <boost/align/aligned_allocator.hpp>

#include "collision/narrowphase/detail/aabb.h"
#include "collision/solvers/accelerators/declarations.h"
#include "collision/solvers/accelerators/detail/common.h"
#include "collision/solvers/accelerators/detail/common_impl.h"
#include "collision/solvers/sat2d/aabb_sat2d.h"
#include "collision/solvers/sat2d/obb_sat2d.h"

template <typename T>
using aligned_vector =
    std::vector<T, boost::alignment::aligned_allocator<T, 16>>;

namespace collision {
namespace detail {
namespace accelerators {
class ContainerCollisionRequestDataBox2D {
 public:
  ContainerCollisionRequestDataBox2D(aligned_vector<int>& candidates)
      : candidates_(&candidates){

        };
  bool QueryCallback(int obj_id) {
    candidates_->push_back(obj_id);
    return true;
  }

  aligned_vector<int>* candidates_;
};

typedef Eigen::Matrix<uint64_t, Eigen::Dynamic, 1> VectorXi64;

class ContainerBox2D {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  ContainerBox2D(aligned_vector<collision::CollisionObject*> objects_in,
                 const ContainerSettings& sett) {
    constructHelper();

    for (auto obj : objects_in) {
      addObject(obj);
    }
    double min_x = std::numeric_limits<double>::max();
    double max_x = std::numeric_limits<double>::min();
    double min_y = std::numeric_limits<double>::max();
    double max_y = std::numeric_limits<double>::min();
    for (auto obj : objects_in) {
      AABB aabb = getAABB(obj);

      min_x = std::min(min_x, aabb.x_min);
      min_y = std::min(min_y, aabb.y_min);
      max_x = std::max(max_x, aabb.x_max);
      max_y = std::max(max_y, aabb.y_max);
    }

    broadphase_manager_ = new b2DynamicTree();

    finalizeCreation();
  }

  ContainerBox2D(const ShapeGroup* sg_ptr, const ContainerSettings& sett) {
    constructHelper();
    for (auto obj : sg_ptr->unpack()) {
      addObject(obj.get());
    }

    auto sg_aabb = sg_ptr->getAABB();
    double min_x = sg_aabb->min()(0);
    double min_y = sg_aabb->min()(1);
    double max_x = sg_aabb->max()(0);
    double max_y = sg_aabb->max()(1);

    broadphase_manager_ = new b2DynamicTree();

    finalizeCreation();
  }

  virtual ~ContainerBox2D() {
    if (broadphase_manager_) delete broadphase_manager_;
  }

  int addObject(const CollisionObject* obj) {
    if (obj->getCollisionObjectClass() == collision::OBJ_CLASS_SHAPE) {
      AABB aabb = getAABB(obj);

      registerObjectId(obj);
      aabbs_.push_back(aabb);
      ObjectProxy proxy = CreateProxy(obj, aabbs_.back());
      objects_.push_back(proxy);
      return 0;
    }
    return -1;
  }

  inline int checkCollision(const CollisionObject* obj,
                            const ContainerCollisionRequest& req) {
    bool res = false;
    TIMER_START(TIMER_grid_hashing);
    AABB aabb = getAABB(obj);

    ObjectProxy proxy = CreateProxy(obj, aabb);
    TIMER_STOP(TIMER_grid_hashing);
    aligned_vector<int> collision_candidates;

    {
      STACK_TIMER tim(TIMER_grid_candidates);

      numchecks_ += objects_.size();
      getCandidates(proxy, collision_candidates);
    }
    if (req.enable_verification) {
      aligned_vector<int> collision_candidates2;
      {
        for (int i = 0; i < objects_optimized_id_.size(); i++) {
          // ground truth collisions between AABBs including border collisions
          if (AABB_SAT2D(getAABB(object_by_id_[objects_optimized_id_[i]]))
                  .collides_eps(AABB_SAT2D(aabb), -1e-11)) {
            collision_candidates2.push_back(objects_optimized_id_[i]);
          }
        }
      }

      std::sort(collision_candidates2.begin(), collision_candidates2.end());
      std::set<int> collision_candidates_set;
      for (auto el : collision_candidates) {
        collision_candidates_set.insert(el);
      }
      std::sort(collision_candidates.begin(), collision_candidates.end());
      int found_count = std::unique(collision_candidates.begin(),
                                    collision_candidates.end()) -
                        collision_candidates.begin();
      int real_count = std::unique(collision_candidates2.begin(),
                                   collision_candidates2.end()) -
                       collision_candidates2.begin();

      volatile int missed = real_count - found_count;

      bool bMissed = false;

      for (auto el : collision_candidates2) {
        if (collision_candidates_set.find(el) ==
            collision_candidates_set.end()) {
          bMissed = true;
          break;
        }
      }

      if (missed > 0 || bMissed) {
        std::cout << "broadphase verification: miss\n";
        throw std::exception();
        return -1;
      }
    }
    {
      STACK_TIMER tim(TIMER_grid_narrowphase);
      collision_checked_id_.fill(0);
      int numcands_local = 0;
      if (obj->getCollisionObjectType() == OBJ_TYPE_OBB_BOX && all_obbs_) {
        const RectangleOBB* obb1 = static_cast<const RectangleOBB*>(obj);
        std::vector<OBB_SAT2D, Eigen::aligned_allocator<OBB_SAT2D>>
            obb1_fast_container;
        obb1_fast_container.emplace_back(obb1->center(), obb1->r_x() * 2,
                                         obb1->r_y() * 2, obb1->local_x_axis(),
                                         obb1->local_y_axis());

        for (auto el : collision_candidates) {
          if ((el < 0) || (el >= fast_obbs_.size())) {
            std::cout << "internal error";
            throw std::exception();
          }
          if (!collision_checked_id_(el)) {
            collision_checked_id_(el) = 1;
            numcands_local += 1;
            if (fast_obbs_[el].overlaps(*(obb1_fast_container.begin())))

            {
              numcands_ += numcands_local;
              return 1;
            }
          }
        }
      } else {
        for (auto el : collision_candidates) {
          if (!collision_checked_id_(el)) {
            numcands_local += 1;
            collision_checked_id_(el) = 1;

            if (object_by_id_[el]->collide(*obj)) {
              numcands_ += numcands_local;
              return 1;
            }
          }
        }
      }
      numcands_ += numcands_local;
    }
    return 0;
  }

  inline int numchecks() const { return numchecks_; }

  inline int numcands() const { return numcands_; }

 private:
  b2DynamicTree* broadphase_manager_;

  std::vector<AABB> aabbs_;
  std::vector<b2AABB> box2d_objects_;

  std::vector<ObjectProxy> objects_;

  VectorXi64 masks_optimized_;
  Eigen::VectorXi res_mask_optimized_;
  aligned_vector<int> objects_optimized_id_;
  aligned_vector<CollisionObject*> object_by_id_;
  std::vector<OBB_SAT2D, Eigen::aligned_allocator<OBB_SAT2D>> fast_obbs_;
  Eigen::VectorXi collision_checked_id_;

  std::map<const CollisionObject*, int> object_ids_;

  bool all_obbs_;

  int numchecks_;
  int numcands_;

  void constructHelper(void) {
    numchecks_ = 0;
    numcands_ = 0;
    broadphase_manager_ = 0;
  }

  inline int finalizeCreation(void) {
    int total_objects = 0;
    total_objects += objects_.size();

    objects_optimized_id_.clear();
    object_by_id_.clear();
    object_by_id_.resize(object_ids_.size());
    collision_checked_id_ = Eigen::VectorXi::Zero(object_ids_.size());

    masks_optimized_ = VectorXi64::Zero(total_objects);
    res_mask_optimized_ = Eigen::VectorXi::Zero(total_objects);

    all_obbs_ = true;
    for (auto el : objects_) {
      if (all_obbs_ &&
          el.obj_ptr->getCollisionObjectType() != OBJ_TYPE_OBB_BOX) {
        all_obbs_ = false;
      }

      objects_optimized_id_.push_back(getObjectId(el.obj_ptr));
      object_by_id_[objects_optimized_id_.back()] = el.obj_ptr;
      masks_optimized_[objects_optimized_id_.size() - 1] = el.mask;
      auto curAABB = &aabbs_[objects_optimized_id_.back()];
      collision::RectangleAABBConstPtr rect(new collision::RectangleAABB(
          (curAABB->x_max - curAABB->x_min) / 2,
          (curAABB->y_max - curAABB->y_min) / 2,
          Eigen::Vector2d((curAABB->x_max + curAABB->x_min) / 2,
                          (curAABB->y_max + curAABB->y_min) / 2)));
      box2d_objects_.emplace_back();
      float eps = 1e-5;
      box2d_objects_.back().lowerBound =
          b2Vec2(curAABB->x_min - eps, curAABB->y_min - eps);
      box2d_objects_.back().upperBound =
          b2Vec2(curAABB->x_max + eps, curAABB->y_max + eps);
    }
    for (auto id : objects_optimized_id_) {
      broadphase_manager_->CreateProxy(box2d_objects_[id],
                                       &objects_optimized_id_[id]);
    }

    return 0;
  }

  inline int getCandidates(ObjectProxy& proxy,
                           aligned_vector<int>& candidates) {
    AABB curAABB = getAABB(proxy.obj_ptr);
    b2AABB b2Obj;
    float eps = 1e-5;
    b2Obj.lowerBound = b2Vec2(curAABB.x_min - eps, curAABB.y_min - eps);
    b2Obj.upperBound = b2Vec2(curAABB.x_max + eps, curAABB.y_max + eps);
    aligned_vector<int> candidates2;
    ContainerCollisionRequestDataBox2D reqData(candidates2);
    broadphase_manager_->Query(&reqData, b2Obj);
    for (auto el : candidates2) {
      candidates.push_back(*((int*)(broadphase_manager_->GetUserData(el))));
    }
    return 0;
  }

  inline int getObjectId(const collision::CollisionObject* obj) {
    auto search = object_ids_.find(obj);
    if (search != object_ids_.end()) {
      return search->second;
    } else
      throw std::exception();
  }

  inline uint64_t get_mask(const AABB& obj) { return 0; }

  inline ObjectProxy CreateProxy(const CollisionObject* obj,
                                 const collision::AABB& aabb) {
    uint64_t mask = get_mask(aabb);
    return ObjectProxy{mask, const_cast<CollisionObject*>(obj)};
    // return ObjectProxy{mask,const_cast<CollisionObject*>(obj), aabb};
  }

  int registerObjectId(const CollisionObject* obj) {
    object_ids_.insert(std::make_pair(obj, (int)object_ids_.size()));
    if (obj->getCollisionObjectType() == OBJ_TYPE_OBB_BOX) {
      const collision::RectangleOBB* obb_in =
          static_cast<const collision::RectangleOBB*>(obj);

      fast_obbs_.emplace_back(obb_in->center(), obb_in->r_x() * 2,
                              obb_in->r_y() * 2, obb_in->local_x_axis(),
                              obb_in->local_y_axis());
    }
    return 0;
  }

  inline AABB getAABB(const CollisionObject* obj) {
    if (obj->getCollisionObjectType() == OBJ_TYPE_OBB_BOX) {
      AABB a = (static_cast<const RectangleOBB*>(obj))->getAABB_fast();
      return a;
    } else {
      auto aabb_fcl = obj->getAABB();
      return AABB(*aabb_fcl);
    }
  }
};
}  // namespace accelerators
}  // namespace detail
}  // namespace collision

#endif /* CPP_COLLISION_INCLUDE_COLLISION_GRID_FCL_H_ */