text: /**
   * PANDA 3D SOFTWARE
   * Copyright (c) Carnegie Mellon University.  All rights reserved.
   *
   * All use of this software is subject to the terms of the revised BSD
   * license.  You should have received a copy of this license along
   * with this source code in a file named "LICENSE."
   *
   * @file bulletWorld.cxx
   * @author enn0x
   * @date 2010-01-23
   */
  
  #include "bulletWorld.h"
  
  #include "config_bullet.h"
  
  #include "bulletFilterCallbackData.h"
  #include "bulletPersistentManifold.h"
  #include "bulletShape.h"
  #include "bulletSoftBodyWorldInfo.h"
  #include "bulletTickCallbackData.h"
  
  #include "collideMask.h"
  #include "lightMutexHolder.h"
  
  #define clamp(x, x_min, x_max) std::max(std::min(x, x_max), x_min)
  
  using std::endl;
  using std::istream;
  using std::ostream;
  using std::string;
  
  TypeHandle BulletWorld::_type_handle;
  
  PStatCollector BulletWorld::_pstat_physics("App:Bullet:DoPhysics");
  PStatCollector BulletWorld::_pstat_simulation("App:Bullet:DoPhysics:Simulation");
  PStatCollector BulletWorld::_pstat_p2b("App:Bullet:DoPhysics:SyncP2B");
  PStatCollector BulletWorld::_pstat_b2p("App:Bullet:DoPhysics:SyncB2P");
  
  PT(CallbackObject) bullet_contact_added_callback;
  
  /**
   *
   */
  BulletWorld::
  BulletWorld() {
  
    // Init groups filter matrix
    for (size_t i = 0; i < 32; ++i) {
      _filter_cb2._collide[i].clear();
      _filter_cb2._collide[i].set_bit(i);
    }
  
    // Broadphase
    btScalar dx(bullet_sap_extents);
    btVector3 extents(dx, dx, dx);
  
    switch (bullet_broadphase_algorithm) {
      case BA_sweep_and_prune:
        _broadphase = new btAxisSweep3(extents, extents, 1024);
        break;
      case BA_dynamic_aabb_tree:
        _broadphase = new btDbvtBroadphase();
        break;
      default:
        bullet_cat.error() << "no proper broadphase algorithm!" << endl;
    }
    nassertv(_broadphase);
  
    // Configuration
    _configuration = new btSoftBodyRigidBodyCollisionConfiguration();
    nassertv(_configuration);
  
    // Dispatcher
    _dispatcher = new btCollisionDispatcher(_configuration);
    nassertv(_dispatcher);
  
    // Solver
    _solver = new btSequentialImpulseConstraintSolver;
    nassertv(_solver);
  
    // World
    _world = new btSoftRigidDynamicsWorld(_dispatcher, _broadphase, _solver, _configuration);
    nassertv(_world);
    nassertv(_world->getPairCache());
  
    _world->setWorldUserInfo(this);
    _world->setGravity(btVector3(0.0f, 0.0f, 0.0f));
  
    // Ghost-pair callback
    _world->getPairCache()->setInternalGhostPairCallback(&_ghost_cb);
  
    // Filter callback
    _filter_algorithm = bullet_filter_algorithm;
    switch (_filter_algorithm) {
      case FA_mask:
        _filter_cb = &_filter_cb1;
        break;
      case FA_groups_mask:
        _filter_cb = &_filter_cb2;
        break;
      case FA_callback:
        _filter_cb = &_filter_cb3;
        break;
      default:
        bullet_cat.error() << "no proper filter algorithm!" << endl;
        _filter_cb = nullptr;
    }
  
    _world->getPairCache()->setOverlapFilterCallback(_filter_cb);
  
    // Tick callback
    _tick_callback_obj = nullptr;
  
    // SoftBodyWorldInfo
    _info.m_dispatcher = _dispatcher;
    _info.m_broadphase = _broadphase;
    _info.m_gravity.setValue(0.0f, 0.0f, 0.0f);
    _info.m_sparsesdf.Initialize();
  
    // Register GIMPACT algorithm
    btGImpactCollisionAlgorithm::registerAlgorithm(_dispatcher);
  
    // Some prefered settings
    _world->getDispatchInfo().m_enableSPU = true;      // default: true
    _world->getDispatchInfo().m_useContinuous = true;  // default: true
    _world->getSolverInfo().m_splitImpulse = false;    // default: false
    _world->getSolverInfo().m_numIterations = bullet_solver_iterations;
  }
  
  /**
   *
   */
  LightMutex &BulletWorld::
  get_global_lock() {
    
    static LightMutex lock;
    
    return lock;
  }
  
  /**
   *
   */
  BulletSoftBodyWorldInfo BulletWorld::
  get_world_info() {
  
    return BulletSoftBodyWorldInfo(_info);
  }
  
  /**
   *
   */
  void BulletWorld::
  set_debug_node(BulletDebugNode *node) {
    LightMutexHolder holder(get_global_lock());
  
    nassertv(node);
    if (node != _debug) {
      if (_debug != nullptr) {
          _debug->_debug_stale = false;
          _debug->_debug_world = nullptr;
      }
  
      _debug = node;
      _world->setDebugDrawer(&(_debug->_drawer));
    }
  }
  
  /**
   * Removes a debug node that has been assigned to this BulletWorld.
   */
  void BulletWorld::
  clear_debug_node() {
    LightMutexHolder holder(get_global_lock());
  
    if (_debug != nullptr) {
      _debug->_debug_stale = false;
      _debug->_debug_world = nullptr;
      _world->setDebugDrawer(nullptr);
      _debug = nullptr;
    }
  }
  
  /**
   *
   */
  void BulletWorld::
  set_gravity(const LVector3 &gravity) {
    LightMutexHolder holder(get_global_lock());
  
    _world->setGravity(LVecBase3_to_btVector3(gravity));
    _info.m_gravity.setValue(gravity.get_x(), gravity.get_y(), gravity.get_z());
  }
  
  /**
   *
   */
  void BulletWorld::
  set_gravity(PN_stdfloat gx, PN_stdfloat gy, PN_stdfloat gz) {
    LightMutexHolder holder(get_global_lock());
  
    _world->setGravity(btVector3((btScalar)gx, (btScalar)gy, (btScalar)gz));
    _info.m_gravity.setValue((btScalar)gx, (btScalar)gy, (btScalar)gz);
  }
  
  /**
   *
   */
  const LVector3 BulletWorld::
  get_gravity() const {
    LightMutexHolder holder(get_global_lock());
  
    return btVector3_to_LVector3(_world->getGravity());
  }
  
  /**
   *
   */
  int BulletWorld::
  do_physics(PN_stdfloat dt, int max_substeps, PN_stdfloat stepsize) {
    LightMutexHolder holder(get_global_lock());
  
    _pstat_physics.start();
  
    int num_substeps = clamp(int(dt / stepsize), 1, max_substeps);
  
    // Synchronize Panda to Bullet
    _pstat_p2b.start();
    do_sync_p2b(dt, num_substeps);
    _pstat_p2b.stop();
  
    // Simulation
    _pstat_simulation.start();
    int n = _world->stepSimulation((btScalar)dt, max_substeps, (btScalar)stepsize);
    _pstat_simulation.stop();
  
    // Synchronize Bullet to Panda
    _pstat_b2p.start();
    do_sync_b2p();
    _info.m_sparsesdf.GarbageCollect(bullet_gc_lifetime);
    _pstat_b2p.stop();
  
    // Render debug
    if (_debug) {
      _debug->do_sync_b2p(_world);
    }
  
    _pstat_physics.stop();
  
    return n;
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_sync_p2b(PN_stdfloat dt, int num_substeps) {
  
    for (BulletRigidBodyNode *body : _bodies) {
      body->do_sync_p2b();
    }
  
    for (BulletSoftBodyNode *softbody : _softbodies) {
      softbody->do_sync_p2b();
    }
  
    for (BulletGhostNode *ghost : _ghosts) {
      ghost->do_sync_p2b();
    }
  
    for (BulletBaseCharacterControllerNode *character : _characters) {
      character->do_sync_p2b(dt, num_substeps);
    }
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_sync_b2p() {
  
    for (BulletRigidBodyNode *body : _bodies) {
      body->do_sync_b2p();
    }
  
    for (BulletSoftBodyNode *softbody : _softbodies) {
      softbody->do_sync_b2p();
    }
  
    for (BulletGhostNode *ghost : _ghosts) {
      ghost->do_sync_b2p();
    }
  
    for (BulletBaseCharacterControllerNode *character : _characters) {
      character->do_sync_b2p();
    }
  
    for (BulletVehicle *vehicle : _vehicles) {
      vehicle->do_sync_b2p();
    }
  }
  
  /**
   *
   */
  void BulletWorld::
  attach(TypedObject *object) {
    LightMutexHolder holder(get_global_lock());
  
    if (object->is_of_type(BulletGhostNode::get_class_type())) {
      do_attach_ghost(DCAST(BulletGhostNode, object));
    }
    else if (object->is_of_type(BulletRigidBodyNode::get_class_type())) {
      do_attach_rigid_body(DCAST(BulletRigidBodyNode, object));
    }
    else if (object->is_of_type(BulletSoftBodyNode::get_class_type())) {
      do_attach_soft_body(DCAST(BulletSoftBodyNode, object));
    }
    else if (object->is_of_type(BulletBaseCharacterControllerNode::get_class_type())) {
      do_attach_character(DCAST(BulletBaseCharacterControllerNode, object));
    }
    else if (object->is_of_type(BulletVehicle::get_class_type())) {
      do_attach_vehicle(DCAST(BulletVehicle, object));
    }
    else if (object->is_of_type(BulletConstraint::get_class_type())) {
      do_attach_constraint(DCAST(BulletConstraint, object));
    }
    else {
      bullet_cat->error() << "not a bullet world object!" << endl;
    }
  }
  
  /**
   *
   */
  void BulletWorld::
  remove(TypedObject *object) {
    LightMutexHolder holder(get_global_lock());
  
    if (object->is_of_type(BulletGhostNode::get_class_type())) {
      do_remove_ghost(DCAST(BulletGhostNode, object));
    }
    else if (object->is_of_type(BulletRigidBodyNode::get_class_type())) {
      do_remove_rigid_body(DCAST(BulletRigidBodyNode, object));
    }
    else if (object->is_of_type(BulletSoftBodyNode::get_class_type())) {
      do_remove_soft_body(DCAST(BulletSoftBodyNode, object));
    }
    else if (object->is_of_type(BulletBaseCharacterControllerNode::get_class_type())) {
      do_remove_character(DCAST(BulletBaseCharacterControllerNode, object));
    }
    else if (object->is_of_type(BulletVehicle::get_class_type())) {
      do_remove_vehicle(DCAST(BulletVehicle, object));
    }
    else if (object->is_of_type(BulletConstraint::get_class_type())) {
      do_remove_constraint(DCAST(BulletConstraint, object));
    }
    else {
      bullet_cat->error() << "not a bullet world object!" << endl;
    }
  }
  
  /**
   * Deprecated!  Please use BulletWorld::attach
   */
  void BulletWorld::
  attach_rigid_body(BulletRigidBodyNode *node) {
    LightMutexHolder holder(get_global_lock());
  
    do_attach_rigid_body(node);
  }
  
  /**
   * Deprecated.! Please use BulletWorld::remove
   */
  void BulletWorld::
  remove_rigid_body(BulletRigidBodyNode *node) {
    LightMutexHolder holder(get_global_lock());
  
    do_remove_rigid_body(node);
  }
  
  /**
   * Deprecated!  Please use BulletWorld::attach
   */
  void BulletWorld::
  attach_soft_body(BulletSoftBodyNode *node) {
    LightMutexHolder holder(get_global_lock());
  
    do_attach_soft_body(node);
  }
  
  /**
   * Deprecated.! Please use BulletWorld::remove
   */
  void BulletWorld::
  remove_soft_body(BulletSoftBodyNode *node) {
    LightMutexHolder holder(get_global_lock());
  
    do_remove_soft_body(node);
  }
  
  /**
   * Deprecated!  Please use BulletWorld::attach
   */
  void BulletWorld::
  attach_ghost(BulletGhostNode *node) {
    LightMutexHolder holder(get_global_lock());
  
    do_attach_ghost(node);
  }
  
  /**
   * Deprecated.! Please use BulletWorld::remove
   */
  void BulletWorld::
  remove_ghost(BulletGhostNode *node) {
    LightMutexHolder holder(get_global_lock());
  
    do_remove_ghost(node);
  }
  
  /**
   * Deprecated!  Please use BulletWorld::attach
   */
  void BulletWorld::
  attach_character(BulletBaseCharacterControllerNode *node) {
    LightMutexHolder holder(get_global_lock());
  
    do_attach_character(node);
  }
  
  /**
   * Deprecated.! Please use BulletWorld::remove
   */
  void BulletWorld::
  remove_character(BulletBaseCharacterControllerNode *node) {
    LightMutexHolder holder(get_global_lock());
  
    do_remove_character(node);
  }
  
  /**
   * Deprecated!  Please use BulletWorld::attach
   */
  void BulletWorld::
  attach_vehicle(BulletVehicle *vehicle) {
    LightMutexHolder holder(get_global_lock());
  
    do_attach_vehicle(vehicle);
  }
  
  /**
   * Deprecated.! Please use BulletWorld::remove
   */
  void BulletWorld::
  remove_vehicle(BulletVehicle *vehicle) {
    LightMutexHolder holder(get_global_lock());
  
    do_remove_vehicle(vehicle);
  }
  
  /**
   * Attaches a single constraint to a world.  Collision checks between the
   * linked objects will be disabled if the second parameter is set to TRUE.
   */
  void BulletWorld::
  attach_constraint(BulletConstraint *constraint, bool linked_collision) {
    LightMutexHolder holder(get_global_lock());
  
    do_attach_constraint(constraint, linked_collision);
  }
  
  /**
   * Deprecated.! Please use BulletWorld::remove
   */
  void BulletWorld::
  remove_constraint(BulletConstraint *constraint) {
    LightMutexHolder holder(get_global_lock());
  
    do_remove_constraint(constraint);
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_attach_rigid_body(BulletRigidBodyNode *node) {
  
    nassertv(node);
  
    btRigidBody *ptr = btRigidBody::upcast(node->get_object());
  
    BulletRigidBodies::iterator found;
    PT(BulletRigidBodyNode) ptnode = node;
    found = find(_bodies.begin(), _bodies.end(), ptnode);
  
    if (found == _bodies.end()) {
      _bodies.push_back(node);
      _world->addRigidBody(ptr);
    }
    else {
      bullet_cat.warning() << "rigid body already attached" << endl;
    }
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_remove_rigid_body(BulletRigidBodyNode *node) {
  
    nassertv(node);
  
    btRigidBody *ptr = btRigidBody::upcast(node->get_object());
  
    BulletRigidBodies::iterator found;
    PT(BulletRigidBodyNode) ptnode = node;
    found = find(_bodies.begin(), _bodies.end(), ptnode);
  
    if (found == _bodies.end()) {
      bullet_cat.warning() << "rigid body not attached" << endl;
    }
    else {
      _bodies.erase(found);
      _world->removeRigidBody(ptr);
    }
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_attach_soft_body(BulletSoftBodyNode *node) {
  
    nassertv(node);
  
    btSoftBody *ptr = btSoftBody::upcast(node->get_object());
  
    // TODO: groupfilter settings (see ghost objects too)
    short group = btBroadphaseProxy::DefaultFilter;
    short mask = btBroadphaseProxy::AllFilter;
  
    BulletSoftBodies::iterator found;
    PT(BulletSoftBodyNode) ptnode = node;
    found = find(_softbodies.begin(), _softbodies.end(), ptnode);
  
    if (found == _softbodies.end()) {
      _softbodies.push_back(node);
      _world->addSoftBody(ptr, group, mask);
    }
    else {
      bullet_cat.warning() << "soft body already attached" << endl;
    }
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_remove_soft_body(BulletSoftBodyNode *node) {
  
    nassertv(node);
  
    btSoftBody *ptr = btSoftBody::upcast(node->get_object());
  
    BulletSoftBodies::iterator found;
    PT(BulletSoftBodyNode) ptnode = node;
    found = find(_softbodies.begin(), _softbodies.end(), ptnode);
  
    if (found == _softbodies.end()) {
      bullet_cat.warning() << "soft body not attached" << endl;
    }
    else {
      _softbodies.erase(found);
      _world->removeSoftBody(ptr);
    }
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_attach_ghost(BulletGhostNode *node) {
  
    nassertv(node);
  
    // TODO groupfilter settings...
  /*
  enum CollisionFilterGroups {
    DefaultFilter = 1,
    StaticFilter = 2,
    KinematicFilter = 4,
    DebrisFilter = 8,
    SensorTrigger = 16,
    CharacterFilter = 32,
    AllFilter = -1
  }
  */
  
    short group = btBroadphaseProxy::SensorTrigger;
    short mask = btBroadphaseProxy::AllFilter
              & ~btBroadphaseProxy::StaticFilter
              & ~btBroadphaseProxy::SensorTrigger;
  
    btGhostObject *ptr = btGhostObject::upcast(node->get_object());
  
    BulletGhosts::iterator found;
    PT(BulletGhostNode) ptnode = node;
    found = find(_ghosts.begin(), _ghosts.end(), ptnode);
  
    if (found == _ghosts.end()) {
      _ghosts.push_back(node);
      _world->addCollisionObject(ptr, group, mask);
    }
    else {
      bullet_cat.warning() << "ghost already attached" << endl;
    }
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_remove_ghost(BulletGhostNode *node) {
  
    nassertv(node);
  
    btGhostObject *ptr = btGhostObject::upcast(node->get_object());
  
    BulletGhosts::iterator found;
    PT(BulletGhostNode) ptnode = node;
    found = find(_ghosts.begin(), _ghosts.end(), ptnode);
  
    if (found == _ghosts.end()) {
      bullet_cat.warning() << "ghost not attached" << endl;
    }
    else {
      _ghosts.erase(found);
      _world->removeCollisionObject(ptr);
    }
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_attach_character(BulletBaseCharacterControllerNode *node) {
  
    nassertv(node);
  
    BulletCharacterControllers::iterator found;
    PT(BulletBaseCharacterControllerNode) ptnode = node;
    found = find(_characters.begin(), _characters.end(), ptnode);
  
    if (found == _characters.end()) {
      _characters.push_back(node);
  
      _world->addCollisionObject(node->get_ghost(),
        btBroadphaseProxy::CharacterFilter,
        btBroadphaseProxy::StaticFilter|btBroadphaseProxy::DefaultFilter);
  
      _world->addCharacter(node->get_character());
    }
    else {
      bullet_cat.warning() << "character already attached" << endl;
    }
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_remove_character(BulletBaseCharacterControllerNode *node) {
  
    nassertv(node);
  
    BulletCharacterControllers::iterator found;
    PT(BulletBaseCharacterControllerNode) ptnode = node;
    found = find(_characters.begin(), _characters.end(), ptnode);
  
    if (found == _characters.end()) {
      bullet_cat.warning() << "character not attached" << endl;
    }
    else {
      _characters.erase(found);
      _world->removeCollisionObject(node->get_ghost());
      _world->removeCharacter(node->get_character());
    }
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_attach_vehicle(BulletVehicle *vehicle) {
  
    nassertv(vehicle);
  
    BulletVehicles::iterator found;
    PT(BulletVehicle) ptvehicle = vehicle;
    found = find(_vehicles.begin(), _vehicles.end(), ptvehicle);
  
    if (found == _vehicles.end()) {
      _vehicles.push_back(vehicle);
      _world->addVehicle(vehicle->get_vehicle());
    }
    else {
      bullet_cat.warning() << "vehicle already attached" << endl;
    }
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_remove_vehicle(BulletVehicle *vehicle) {
  
    nassertv(vehicle);
  
    do_remove_rigid_body(vehicle->do_get_chassis());
  
    BulletVehicles::iterator found;
    PT(BulletVehicle) ptvehicle = vehicle;
    found = find(_vehicles.begin(), _vehicles.end(), ptvehicle);
  
    if (found == _vehicles.end()) {
      bullet_cat.warning() << "vehicle not attached" << endl;
    }
    else {
      _vehicles.erase(found);
      _world->removeVehicle(vehicle->get_vehicle());
    }
  }
  
  /**
   * Attaches a single constraint to a world.  Collision checks between the
   * linked objects will be disabled if the second parameter is set to TRUE.
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_attach_constraint(BulletConstraint *constraint, bool linked_collision) {
  
    nassertv(constraint);
  
    BulletConstraints::iterator found;
    PT(BulletConstraint) ptconstraint = constraint;
    found = find(_constraints.begin(), _constraints.end(), ptconstraint);
  
    if (found == _constraints.end()) {
      _constraints.push_back(constraint);
      _world->addConstraint(constraint->ptr(), linked_collision);
    }
    else {
      bullet_cat.warning() << "constraint already attached" << endl;
    }
  }
  
  /**
   * Assumes the lock(bullet global lock) is held by the caller
   */
  void BulletWorld::
  do_remove_constraint(BulletConstraint *constraint) {
  
    nassertv(constraint);
  
    BulletConstraints::iterator found;
    PT(BulletConstraint) ptconstraint = constraint;
    found = find(_constraints.begin(), _constraints.end(), ptconstraint);
  
    if (found == _constraints.end()) {
      bullet_cat.warning() << "constraint not attached" << endl;
    }
    else {
      _constraints.erase(found);
      _world->removeConstraint(constraint->ptr());
    }
  }
  
  /**
   *
   */
  int BulletWorld::
  get_num_rigid_bodies() const {
    LightMutexHolder holder(get_global_lock());
  
    return _bodies.size();
  }
  
  /**
   *
   */
  BulletRigidBodyNode *BulletWorld::
  get_rigid_body(int idx) const {
    LightMutexHolder holder(get_global_lock());
  
    nassertr(idx >= 0 && idx < (int)_bodies.size(), nullptr);
    return _bodies[idx];
  }
  
  /**
   *
   */
  int BulletWorld::
  get_num_soft_bodies() const {
    LightMutexHolder holder(get_global_lock());
  
    return _softbodies.size();
  }
  
  /**
   *
   */
  BulletSoftBodyNode *BulletWorld::
  get_soft_body(int idx) const {
    LightMutexHolder holder(get_global_lock());
  
    nassertr(idx >= 0 && idx < (int)_softbodies.size(), nullptr);
    return _softbodies[idx];
  }
  
  /**
   *
   */
  int BulletWorld::
  get_num_ghosts() const {
    LightMutexHolder holder(get_global_lock());
  
    return _ghosts.size();
  }
  
  /**
   *
   */
  BulletGhostNode *BulletWorld::
  get_ghost(int idx) const {
    LightMutexHolder holder(get_global_lock());
  
    nassertr(idx >= 0 && idx < (int)_ghosts.size(), nullptr);
    return _ghosts[idx];
  }
  
  /**
   *
   */
  int BulletWorld::
  get_num_characters() const {
    LightMutexHolder holder(get_global_lock());
  
    return _characters.size();
  }
  
  /**
   *
   */
  BulletBaseCharacterControllerNode *BulletWorld::
  get_character(int idx) const {
    LightMutexHolder holder(get_global_lock());
  
    nassertr(idx >= 0 && idx < (int)_characters.size(), nullptr);
    return _characters[idx];
  }
  
  /**
   *
   */
  int BulletWorld::
  get_num_vehicles() const {
    LightMutexHolder holder(get_global_lock());
  
    return _vehicles.size();
  }
  
  /**
   *
   */
  BulletVehicle *BulletWorld::
  get_vehicle(int idx) const {
    LightMutexHolder holder(get_global_lock());
  
    nassertr(idx >= 0 && idx < (int)_vehicles.size(), nullptr);
    return _vehicles[idx];
  }
  
  /**
   *
   */
  int BulletWorld::
  get_num_constraints() const {
    LightMutexHolder holder(get_global_lock());
  
    return _constraints.size();
  }
  
  /**
   *
   */
  BulletConstraint *BulletWorld::
  get_constraint(int idx) const {
    LightMutexHolder holder(get_global_lock());
  
    nassertr(idx >= 0 && idx < (int)_constraints.size(), nullptr);
    return _constraints[idx];
  }
  
  /**
   *
   */
  int BulletWorld::
  get_num_manifolds() const {
    LightMutexHolder holder(get_global_lock());
  
    return _world->getDispatcher()->getNumManifolds();
  }
  
  /**
   *
   */
  BulletClosestHitRayResult BulletWorld::
  ray_test_closest(const LPoint3 &from_pos, const LPoint3 &to_pos, const CollideMask &mask) const {
    LightMutexHolder holder(get_global_lock());
  
    nassertr(!from_pos.is_nan(), BulletClosestHitRayResult::empty());
    nassertr(!to_pos.is_nan(), BulletClosestHitRayResult::empty());
  
    const btVector3 from = LVecBase3_to_btVector3(from_pos);
    const btVector3 to = LVecBase3_to_btVector3(to_pos);
  
    BulletClosestHitRayResult cb(from, to, mask);
    _world->rayTest(from, to, cb);
    return cb;
  }
  
  /**
   *
   */
  BulletAllHitsRayResult BulletWorld::
  ray_test_all(const LPoint3 &from_pos, const LPoint3 &to_pos, const CollideMask &mask) const {
    LightMutexHolder holder(get_global_lock());
  
    nassertr(!from_pos.is_nan(), BulletAllHitsRayResult::empty());
    nassertr(!to_pos.is_nan(), BulletAllHitsRayResult::empty());
  
    const btVector3 from = LVecBase3_to_btVector3(from_pos);
    const btVector3 to = LVecBase3_to_btVector3(to_pos);
  
    BulletAllHitsRayResult cb(from, to, mask);
    _world->rayTest(from, to, cb);
    return cb;
  }
  
  /**
   * Performs a sweep test against all other shapes that match the given group
   * mask.  The provided shape must be a convex shape; it is an error to invoke
   * this method using a non-convex shape.
   */
  BulletClosestHitSweepResult BulletWorld::
  sweep_test_closest(BulletShape *shape, const TransformState &from_ts, const TransformState &to_ts, const CollideMask &mask, PN_stdfloat penetration) const {
    LightMutexHolder holder(get_global_lock());
  
    nassertr(shape, BulletClosestHitSweepResult::empty());
  
    const btConvexShape *convex = (const btConvexShape *) shape->ptr();
    nassertr(convex->isConvex(), BulletClosestHitSweepResult::empty());
    
    nassertr(!from_ts.is_invalid(), BulletClosestHitSweepResult::empty());
    nassertr(!to_ts.is_invalid(), BulletClosestHitSweepResult::empty());
  
    const btVector3 from_pos = LVecBase3_to_btVector3(from_ts.get_pos());
    const btVector3 to_pos = LVecBase3_to_btVector3(to_ts.get_pos());
    const btTransform from_trans = LMatrix4_to_btTrans(from_ts.get_mat());
    const btTransform to_trans = LMatrix4_to_btTrans(to_ts.get_mat());
  
    BulletClosestHitSweepResult cb(from_pos, to_pos, mask);
    _world->convexSweepTest(convex, from_trans, to_trans, cb, penetration);
    return cb;
  }
  
  /**
   * Performs a test if two bodies should collide or not, based on the collision
   * filter setting.
   */
  bool BulletWorld::
  filter_test(PandaNode *node0, PandaNode *node1) const {
    LightMutexHolder holder(get_global_lock());
  
    nassertr(node0, false);
    nassertr(node1, false);
    nassertr(_filter_cb, false);
  
    btCollisionObject *obj0 = get_collision_object(node0);
    btCollisionObject *obj1 = get_collision_object(node1);
  
    nassertr(obj0, false);
    nassertr(obj1, false);
  
    btBroadphaseProxy *proxy0 = obj0->getBroadphaseHandle();
    btBroadphaseProxy *proxy1 = obj1->getBroadphaseHandle();
  
    nassertr(proxy0, false);
    nassertr(proxy1, false);
  
    return _filter_cb->needBroadphaseCollision(proxy0, proxy1);
  }
  
  /**
   * Performas a test for all bodies which are currently in contact with the
   * given body.  The test returns a BulletContactResult object which may
   * contain zero, one or more contacts.
   *
   * If the optional parameter use_filter is set to TRUE this test will consider
   * filter settings.  Otherwise all objects in contact are reported, no matter
   * if they would collide or not.
   */
  BulletContactResult BulletWorld::
  contact_test(PandaNode *node, bool use_filter) const {
    LightMutexHolder holder(get_global_lock());
  
    btCollisionObject *obj = get_collision_object(node);
  
    BulletContactResult cb;
  
    if (obj) {
  #if BT_BULLET_VERSION >= 281
      if (use_filter) {
        cb.use_filter(_filter_cb, obj->getBroadphaseHandle());
      }
  #endif
  
      _world->contactTest(obj, cb);
    }
  
    return cb;
  }
  
  /**
   * Performas a test if the two bodies given as parameters are in contact or
   * not.  The test returns a BulletContactResult object which may contain zero
   * or one contacts.
   */
  BulletContactResult BulletWorld::
  contact_test_pair(PandaNode *node0, PandaNode *node1) const {
    LightMutexHolder holder(get_global_lock());
  
    btCollisionObject *obj0 = get_collision_object(node0);
    btCollisionObject *obj1 = get_collision_object(node1);
  
    BulletContactResult cb;
  
    if (obj0 && obj1) {
  
      _world->contactPairTest(obj0, obj1, cb);
    }
  
    return cb;
  }
  
  /**
   *
   */
  BulletPersistentManifold *BulletWorld::
  get_manifold(int idx) const {
    LightMutexHolder holder(get_global_lock());
  
    nassertr(idx < _dispatcher->getNumManifolds(), nullptr);
  
    btPersistentManifold *ptr = _dispatcher->getManifoldByIndexInternal(idx);
    return (ptr) ? new BulletPersistentManifold(ptr) : nullptr;
  }
  
  /**
   *
   */
  btCollisionObject *BulletWorld::
  get_collision_object(PandaNode *node) {
  
    if (node->is_of_type(BulletRigidBodyNode::get_class_type())) {
      return ((BulletRigidBodyNode *)node)->get_object();
    }
    else if (node->is_of_type(BulletGhostNode::get_class_type())) {
      return ((BulletGhostNode *)node)->get_object();
    }
    else if (node->is_of_type(BulletBaseCharacterControllerNode::get_class_type())) {
      return ((BulletBaseCharacterControllerNode *)node)->get_ghost();
    }
    else if (node->is_of_type(BulletSoftBodyNode::get_class_type())) {
      return ((BulletSoftBodyNode *)node)->get_object();
    }
  
    return nullptr;
  }
  
  /**
   *
   */
  void BulletWorld::
  set_group_collision_flag(unsigned int group1, unsigned int group2, bool enable) {
    LightMutexHolder holder(get_global_lock());
  
    if (_filter_algorithm != FA_groups_mask) {
      bullet_cat.warning() << "filter algorithm is not 'groups-mask'" << endl;
    }
  
    _filter_cb2._collide[group1].set_bit_to(group2, enable);
    _filter_cb2._collide[group2].set_bit_to(group1, enable);
  }
  
  /**
   *
   */
  bool BulletWorld::
  get_group_collision_flag(unsigned int group1, unsigned int group2) const {
    LightMutexHolder holder(get_global_lock());
  
    return _filter_cb2._collide[group1].get_bit(group2);
  }
  
  /**
   *
   */
  void BulletWorld::
  set_force_update_all_aabbs(bool force) {
    LightMutexHolder holder(get_global_lock());
    _world->setForceUpdateAllAabbs(force);
  }
  
  /**
   *
   */
  bool BulletWorld::
  get_force_update_all_aabbs() const {
    LightMutexHolder holder(get_global_lock());
    return _world->getForceUpdateAllAabbs();
  }
  
  /**
   *
   */
  void BulletWorld::
  set_contact_added_callback(CallbackObject *obj) {
    LightMutexHolder holder(get_global_lock());
  
    _world->getSolverInfo().m_solverMode |= SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION;
    _world->getSolverInfo().m_solverMode |= SOLVER_USE_2_FRICTION_DIRECTIONS;
    _world->getSolverInfo().m_solverMode |= SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;
  
    bullet_contact_added_callback = obj;
  }
  
  /**
   *
   */
  void BulletWorld::
  clear_contact_added_callback() {
    LightMutexHolder holder(get_global_lock());
  
    _world->getSolverInfo().m_solverMode &= ~SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION;
    _world->getSolverInfo().m_solverMode &= ~SOLVER_USE_2_FRICTION_DIRECTIONS;
    _world->getSolverInfo().m_solverMode &= ~SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;
  
    bullet_contact_added_callback = nullptr;
  }
  
  /**
   *
   */
  void BulletWorld::
  set_tick_callback(CallbackObject *obj, bool is_pretick) {
    LightMutexHolder holder(get_global_lock());
  
    nassertv(obj != nullptr);
    _tick_callback_obj = obj;
    _world->setInternalTickCallback(&BulletWorld::tick_callback, this, is_pretick);
  }
  
  /**
   *
   */
  void BulletWorld::
  clear_tick_callback() {
    LightMutexHolder holder(get_global_lock());
  
    _tick_callback_obj = nullptr;
    _world->setInternalTickCallback(nullptr);
  }
  
  /**
   *
   */
  void BulletWorld::
  tick_callback(btDynamicsWorld *world, btScalar timestep) {
  
    nassertv(world->getWorldUserInfo());
  
    BulletWorld *w = static_cast<BulletWorld *>(world->getWorldUserInfo());
    CallbackObject *obj = w->_tick_callback_obj;
    if (obj) {
      BulletTickCallbackData cbdata(timestep);
      // Release the global lock that we are holding during the tick callback
      // and allow interactions with bullet world in the user callback
      get_global_lock().release();
      obj->do_callback(&cbdata);
      // Acquire the global lock again and protect the execution
      get_global_lock().acquire();
    }
  }
  
  /**
   *
   */
  void BulletWorld::
  set_filter_callback(CallbackObject *obj) {
    LightMutexHolder holder(get_global_lock());
  
    nassertv(obj != nullptr);
  
    if (_filter_algorithm != FA_callback) {
      bullet_cat.warning() << "filter algorithm is not 'callback'" << endl;
    }
  
    _filter_cb3._filter_callback_obj = obj;
  }
  
  /**
   *
   */
  void BulletWorld::
  clear_filter_callback() {
    LightMutexHolder holder(get_global_lock());
  
    _filter_cb3._filter_callback_obj = nullptr;
  }
  
  /**
   *
   */
  bool BulletWorld::btFilterCallback1::
  needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const {
  
    btCollisionObject *obj0 = (btCollisionObject *) proxy0->m_clientObject;
    btCollisionObject *obj1 = (btCollisionObject *) proxy1->m_clientObject;
  
    nassertr(obj0, false);
    nassertr(obj1, false);
  
    PandaNode *node0 = (PandaNode *) obj0->getUserPointer();
    PandaNode *node1 = (PandaNode *) obj1->getUserPointer();
  
    nassertr(node0, false);
    nassertr(node1, false);
  
    CollideMask mask0 = node0->get_into_collide_mask();
    CollideMask mask1 = node1->get_into_collide_mask();
  
    return (mask0 & mask1) != 0;
  }
  
  /**
   *
   */
  bool BulletWorld::btFilterCallback2::
  needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const {
  
    btCollisionObject *obj0 = (btCollisionObject *) proxy0->m_clientObject;
    btCollisionObject *obj1 = (btCollisionObject *) proxy1->m_clientObject;
  
    nassertr(obj0, false);
    nassertr(obj1, false);
  
    PandaNode *node0 = (PandaNode *) obj0->getUserPointer();
    PandaNode *node1 = (PandaNode *) obj1->getUserPointer();
  
    nassertr(node0, false);
    nassertr(node1, false);
  
    CollideMask mask0 = node0->get_into_collide_mask();
    CollideMask mask1 = node1->get_into_collide_mask();
  
  // cout << mask0 << "   " << mask1 << endl;
  
    for (size_t i = 0; i < 32; ++i) {
      if (mask0.get_bit(i)) {
        if ((_collide[i] & mask1) != 0)
  // cout << "collide: i=" << i << " _collide[i]" << _collide[i] << endl;
          return true;
      }
    }
  
    return false;
  }
  
  /**
   *
   */
  bool BulletWorld::btFilterCallback3::
  needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const {
  
    nassertr(_filter_callback_obj, false);
  
    btCollisionObject *obj0 = (btCollisionObject *) proxy0->m_clientObject;
    btCollisionObject *obj1 = (btCollisionObject *) proxy1->m_clientObject;
  
    nassertr(obj0, false);
    nassertr(obj1, false);
  
    PandaNode *node0 = (PandaNode *) obj0->getUserPointer();
    PandaNode *node1 = (PandaNode *) obj1->getUserPointer();
  
    nassertr(node0, false);
    nassertr(node1, false);
  
    BulletFilterCallbackData cbdata(node0, node1);
    _filter_callback_obj->do_callback(&cbdata);
    return cbdata.get_collide();
  }
  
  /**
   *
   */
  ostream &
  operator << (ostream &out, BulletWorld::BroadphaseAlgorithm algorithm) {
  
    switch (algorithm) {
    case BulletWorld::BA_sweep_and_prune:
      return out << "sap";
  
    case BulletWorld::BA_dynamic_aabb_tree:
      return out << "aabb";
    };
  
    return out << "**invalid BulletWorld::BroadphaseAlgorithm(" << (int)algorithm << ")**";
  }
  
  /**
   *
   */
  istream &
  operator >> (istream &in, BulletWorld::BroadphaseAlgorithm &algorithm) {
    string word;
    in >> word;
  
    if (word == "sap") {
      algorithm = BulletWorld::BA_sweep_and_prune;
    }
    else if (word == "aabb") {
      algorithm = BulletWorld::BA_dynamic_aabb_tree;
    }
    else {
      bullet_cat.error()
        << "Invalid BulletWorld::BroadphaseAlgorithm: " << word << "\n";
      algorithm = BulletWorld::BA_dynamic_aabb_tree;
    }
  
    return in;
  }
  
  /**
   *
   */
  ostream &
  operator << (ostream &out, BulletWorld::FilterAlgorithm algorithm) {
  
    switch (algorithm) {
    case BulletWorld::FA_mask:
      return out << "mask";
    case BulletWorld::FA_groups_mask:
      return out << "groups-mask";
    case BulletWorld::FA_callback:
      return out << "callback";
    };
    return out << "**invalid BulletWorld::FilterAlgorithm(" << (int)algorithm << ")**";
  }
  
  /**
   *
   */
  istream &
  operator >> (istream &in, BulletWorld::FilterAlgorithm &algorithm) {
    string word;
    in >> word;
  
    if (word == "mask") {
      algorithm = BulletWorld::FA_mask;
    }
    else if (word == "groups-mask") {
      algorithm = BulletWorld::FA_groups_mask;
    }
    else if (word == "callback") {
      algorithm = BulletWorld::FA_callback;
    }
    else {
      bullet_cat.error()
        << "Invalid BulletWorld::FilterAlgorithm: " << word << "\n";
      algorithm = BulletWorld::FA_mask;
    }
    return in;
  }
  