protected boolean recoverFromPenetration(CollisionWorld collisionWorld) {
    boolean penetration = false;

    collisionWorld
        .getDispatcher()
        .dispatchAllCollisionPairs(
            ghostObject.getOverlappingPairCache(),
            collisionWorld.getDispatchInfo(),
            collisionWorld.getDispatcher());

    currentPosition.set(ghostObject.getWorldTransform(new Transform()).origin);

    double maxPen = 0.0f;
    for (int i = 0; i < ghostObject.getOverlappingPairCache().getNumOverlappingPairs(); i++) {
      manifoldArray.clear();

      BroadphasePair collisionPair =
          ghostObject.getOverlappingPairCache().getOverlappingPairArray().getQuick(i);

      if (collisionPair.algorithm != null) {
        collisionPair.algorithm.getAllContactManifolds(manifoldArray);
      }

      for (int j = 0; j < manifoldArray.size(); j++) {
        PersistentManifold manifold = manifoldArray.getQuick(j);
        double directionSign = manifold.getBody0() == ghostObject ? -1.0f : 1.0f;
        for (int p = 0; p < manifold.getNumContacts(); p++) {
          ManifoldPoint pt = manifold.getContactPoint(p);

          double dist = pt.getDistance();
          if (dist < 0.0f) {
            if (dist < maxPen) {
              maxPen = dist;
              touchingNormal.set(pt.normalWorldOnB); // ??
              touchingNormal.scale(directionSign);
            }

            currentPosition.scaleAdd(
                directionSign * dist * 0.2f, pt.normalWorldOnB, currentPosition);

            penetration = true;
          } else {
            // printf("touching %f\n", dist);
          }
        }

        // manifold->clearManifold();
      }
    }

    Transform newTrans = ghostObject.getWorldTransform(new Transform());
    newTrans.origin.set(currentPosition);
    ghostObject.setWorldTransform(newTrans);
    // printf("m_touchingNormal =
    // %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);

    // System.out.println("recoverFromPenetration "+penetration+" "+touchingNormal);

    return penetration;
  }
Exemple #2
0
  void updateServerElements() {
    for (RigidBody rb : rigidBodies) {
      ServerElement element = rigidBodyToServerElement.get(rb);
      if (rb != null && rb.getMotionState() != null) {
        rb.getMotionState().getWorldTransform(tmpTrans);
        element.setTransform(tmpTrans);
      }
      if (!rb.isActive()) {
        rb.activate();
      }
      // log.debug(" "+element.getPosition());
    }

    for (Integer id : serverGameArea.getGameState().getDynamicElements().keySet()) {
      ServerElement elem = serverGameArea.getGameState().getDynamicElements().get(id);
      elem.getCollidees().clear();
    }

    Integer nbManifold = dynamicsWorld.getDispatcher().getNumManifolds();
    for (Integer i = 0; i < nbManifold; i++) {
      PersistentManifold pm = dynamicsWorld.getDispatcher().getManifoldByIndexInternal(i);
      RigidBody rb0 = (RigidBody) pm.getBody0();
      RigidBody rb1 = (RigidBody) pm.getBody1();

      ServerElement elem0 = rigidBodyToServerElement.get(rb0);
      ServerElement elem1 = rigidBodyToServerElement.get(rb1);

      if ((elem0 instanceof ServerBullet) || (elem1 instanceof ServerBullet)) {
        log.info("coll " + elem0 + " " + elem1);
      }

      elem0.addCollidee(elem1);
      elem1.addCollidee(elem0);
    }
  }