private void addRigidBody(PhysicsRigidBody node) {
    if (physicsBodies.containsKey(node.getObjectId())) {
      logger.log(Level.WARNING, "RigidBody {0} already exists in PhysicsSpace, cannot add.", node);
      return;
    }
    physicsBodies.put(node.getObjectId(), node);

    // Workaround
    // It seems that adding a Kinematic RigidBody to the dynamicWorld prevent it from being non
    // kinematic again afterward.
    // so we add it non kinematic, then set it kinematic again.
    boolean kinematic = false;
    if (node.isKinematic()) {
      kinematic = true;
      node.setKinematic(false);
    }
    addRigidBody(physicsSpaceId, node.getObjectId());
    if (kinematic) {
      node.setKinematic(true);
    }

    logger.log(Level.FINE, "Adding RigidBody {0} to physics space.", node.getObjectId());
    if (node instanceof PhysicsVehicle) {
      logger.log(
          Level.FINE,
          "Adding vehicle constraint {0} to physics space.",
          Long.toHexString(((PhysicsVehicle) node).getVehicleId()));
      physicsVehicles.put(((PhysicsVehicle) node).getVehicleId(), (PhysicsVehicle) node);
      addVehicle(physicsSpaceId, ((PhysicsVehicle) node).getVehicleId());
    }
  }
 @Override
 protected void finalize() throws Throwable {
   super.finalize();
   Logger.getLogger(this.getClass().getName())
       .log(Level.FINE, "Finalizing PhysicsSpace {0}", Long.toHexString(physicsSpaceId));
   finalizeNative(physicsSpaceId);
 }
 private void addJoint(PhysicsJoint joint) {
   if (physicsJoints.containsKey(joint.getObjectId())) {
     logger.log(Level.WARNING, "Joint {0} already exists in PhysicsSpace, cannot add.", joint);
     return;
   }
   logger.log(
       Level.FINE, "Adding Joint {0} to physics space.", Long.toHexString(joint.getObjectId()));
   physicsJoints.put(joint.getObjectId(), joint);
   addConstraintC(physicsSpaceId, joint.getObjectId(), !joint.isCollisionBetweenLinkedBodys());
   //        dynamicsWorld.addConstraint(joint.getObjectId(),
   // !joint.isCollisionBetweenLinkedBodys());
 }
 private void removeRigidBody(PhysicsRigidBody node) {
   if (!physicsBodies.containsKey(node.getObjectId())) {
     logger.log(
         Level.WARNING, "RigidBody {0} does not exist in PhysicsSpace, cannot remove.", node);
     return;
   }
   if (node instanceof PhysicsVehicle) {
     logger.log(
         Level.FINE,
         "Removing vehicle constraint {0} from physics space.",
         Long.toHexString(((PhysicsVehicle) node).getVehicleId()));
     physicsVehicles.remove(((PhysicsVehicle) node).getVehicleId());
     removeVehicle(physicsSpaceId, ((PhysicsVehicle) node).getVehicleId());
   }
   logger.log(
       Level.FINE,
       "Removing RigidBody {0} from physics space.",
       Long.toHexString(node.getObjectId()));
   physicsBodies.remove(node.getObjectId());
   removeRigidBody(physicsSpaceId, node.getObjectId());
 }
 private void removeJoint(PhysicsJoint joint) {
   if (!physicsJoints.containsKey(joint.getObjectId())) {
     logger.log(Level.WARNING, "Joint {0} does not exist in PhysicsSpace, cannot remove.", joint);
     return;
   }
   logger.log(
       Level.FINE,
       "Removing Joint {0} from physics space.",
       Long.toHexString(joint.getObjectId()));
   physicsJoints.remove(joint.getObjectId());
   removeConstraint(physicsSpaceId, joint.getObjectId());
   //        dynamicsWorld.removeConstraint(joint.getObjectId());
 }
 private void removeGhostObject(PhysicsGhostObject node) {
   if (!physicsGhostObjects.containsKey(node.getObjectId())) {
     logger.log(
         Level.WARNING, "GhostObject {0} does not exist in PhysicsSpace, cannot remove.", node);
     return;
   }
   physicsGhostObjects.remove(node.getObjectId());
   logger.log(
       Level.FINE,
       "Removing ghost object {0} from physics space.",
       Long.toHexString(node.getObjectId()));
   removeCollisionObject(physicsSpaceId, node.getObjectId());
 }
 private void addGhostObject(PhysicsGhostObject node) {
   if (physicsGhostObjects.containsKey(node.getObjectId())) {
     logger.log(
         Level.WARNING, "GhostObject {0} already exists in PhysicsSpace, cannot add.", node);
     return;
   }
   physicsGhostObjects.put(node.getObjectId(), node);
   logger.log(
       Level.FINE,
       "Adding ghost object {0} to physics space.",
       Long.toHexString(node.getObjectId()));
   addCollisionObject(physicsSpaceId, node.getObjectId());
 }
 private void addCharacter(PhysicsCharacter node) {
   if (physicsCharacters.containsKey(node.getObjectId())) {
     logger.log(Level.WARNING, "Character {0} already exists in PhysicsSpace, cannot add.", node);
     return;
   }
   physicsCharacters.put(node.getObjectId(), node);
   logger.log(
       Level.FINE, "Adding character {0} to physics space.", Long.toHexString(node.getObjectId()));
   addCharacterObject(physicsSpaceId, node.getObjectId());
   addAction(physicsSpaceId, node.getControllerId());
   //        dynamicsWorld.addCollisionObject(node.getObjectId(),
   // CollisionFilterGroups.CHARACTER_FILTER, (short) (CollisionFilterGroups.STATIC_FILTER |
   // CollisionFilterGroups.DEFAULT_FILTER));
   //        dynamicsWorld.addAction(node.getControllerId());
 }
 private void removeCharacter(PhysicsCharacter node) {
   if (!physicsCharacters.containsKey(node.getObjectId())) {
     logger.log(
         Level.WARNING, "Character {0} does not exist in PhysicsSpace, cannot remove.", node);
     return;
   }
   physicsCharacters.remove(node.getObjectId());
   logger.log(
       Level.FINE,
       "Removing character {0} from physics space.",
       Long.toHexString(node.getObjectId()));
   removeAction(physicsSpaceId, node.getControllerId());
   removeCharacterObject(physicsSpaceId, node.getObjectId());
   //        dynamicsWorld.removeAction(node.getControllerId());
   //        dynamicsWorld.removeCollisionObject(node.getObjectId());
 }