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;
  }
  protected void stepDown(CollisionWorld collisionWorld, double dt) {
    Transform start = new Transform();
    Transform end = new Transform();

    // phase 3: down
    double additionalDownStep = (wasOnGround /*&& !onGround()*/) ? stepHeight : 0.0f;
    Vector3d step_drop = new Vector3d();
    step_drop.scale(currentStepOffset + additionalDownStep, upAxisDirection[upAxis]);
    double downVelocity =
        (additionalDownStep == 0.0f && verticalVelocity < 0.0f ? -verticalVelocity : 0.0f) * dt;
    Vector3d gravity_drop = new Vector3d();
    gravity_drop.scale(downVelocity, upAxisDirection[upAxis]);
    targetPosition.sub(step_drop);
    targetPosition.sub(gravity_drop);

    start.setIdentity();
    end.setIdentity();

    start.origin.set(currentPosition);
    end.origin.set(targetPosition);

    KinematicClosestNotMeConvexResultCallback callback =
        new KinematicClosestNotMeConvexResultCallback(
            ghostObject, upAxisDirection[upAxis], maxSlopeCosine);
    callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
    callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;

    if (useGhostObjectSweepTest) {
      ghostObject.convexSweepTest(
          convexShape,
          start,
          end,
          callback,
          collisionWorld.getDispatchInfo().allowedCcdPenetration);
    } else {
      collisionWorld.convexSweepTest(convexShape, start, end, callback);
    }

    if (callback.hasHit()) {
      // we dropped a fraction of the height -> hit floor
      currentPosition.interpolate(currentPosition, targetPosition, callback.closestHitFraction);
      verticalVelocity = 0.0f;
      verticalOffset = 0.0f;
    } else {
      // we dropped the full height
      currentPosition.set(targetPosition);
    }
  }
  protected void stepUp(CollisionWorld world) {
    // phase 1: up
    Transform start = new Transform();
    Transform end = new Transform();
    targetPosition.scaleAdd(
        stepHeight + (verticalOffset > 0.0 ? verticalOffset : 0.0f),
        upAxisDirection[upAxis],
        currentPosition);

    start.setIdentity();
    end.setIdentity();

    /* FIXME: Handle penetration properly */
    start.origin.scaleAdd(
        convexShape.getMargin() + addedMargin, upAxisDirection[upAxis], currentPosition);
    end.origin.set(targetPosition);

    // Find only sloped/flat surface hits, avoid wall and ceiling hits...
    Vector3d up = new Vector3d();
    up.scale(-1f, upAxisDirection[upAxis]);
    KinematicClosestNotMeConvexResultCallback callback =
        new KinematicClosestNotMeConvexResultCallback(ghostObject, up, 0.0f);
    callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
    callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;

    if (useGhostObjectSweepTest) {
      ghostObject.convexSweepTest(
          convexShape, start, end, callback, world.getDispatchInfo().allowedCcdPenetration);
    } else {
      world.convexSweepTest(convexShape, start, end, callback);
    }

    if (callback.hasHit()) {
      // we moved up only a fraction of the step height
      currentStepOffset = stepHeight * callback.closestHitFraction;
      currentPosition.interpolate(currentPosition, targetPosition, callback.closestHitFraction);
      verticalVelocity = 0.0f;
      verticalOffset = 0.0f;
    } else {
      currentStepOffset = stepHeight;
      currentPosition.set(targetPosition);
    }
  }
Пример #4
0
  public void removeObjectFromCollisionDomain(MTComponent group) {
    if (collisionGroups.containsKey(group)) {
      collisionGroups.remove(group);
    }

    ArrayList<CollisionObject> colObjs = this.getAllObjectsForCollisionGroup(group);
    Iterator<CollisionObject> iterColObjs = colObjs.iterator();

    while (iterColObjs.hasNext()) {
      collisionWorld.removeCollisionObject(iterColObjs.next());
    }

    if (colObjectsForGroup.containsKey(group)) {
      this.colObjectsForGroup.remove(group);
    }
  }
Пример #5
0
  public void addClusterToCollisionDomain(Cluster3DExt cluster) {
    ArrayList<CollisionObject> colObjs = new ArrayList<CollisionObject>();

    for (int i = 0; i < cluster.getChildren().length; i++) {
      colObjs.addAll(
          this.getAllObjectsForCollisionGroup(
              cluster.getChildren()[i])); // save all collision objects in on object
      removeObjectFromCollisionDomain(
          cluster.getChildren()[i]); // remove current object from collision world	
    }

    groupId = (short) (groupId << 1); // shift groupId so every group has a unique bit value

    for (int i = 0; i < colObjs.size(); i++) {
      collisionWorld.addCollisionObject(colObjs.get(i), groupId, (short) ~groupId);
    }
    colObjectsForGroup.put(cluster, colObjs);
  }
  protected void stepForwardAndStrafe(CollisionWorld collisionWorld, Vector3d walkMove) {
    // printf("m_normalizedDirection=%f,%f,%f\n",
    // 	m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
    // phase 2: forward and strafe
    Transform start = new Transform();
    Transform end = new Transform();
    targetPosition.add(currentPosition, walkMove);
    start.setIdentity();
    end.setIdentity();

    double fraction = 1.0f;
    Vector3d distance2Vec = new Vector3d();
    distance2Vec.sub(currentPosition, targetPosition);
    double distance2 = distance2Vec.lengthSquared();
    // printf("distance2=%f\n",distance2);

    /*if (touchingContact) {
    	if (normalizedDirection.dot(touchingNormal) > 0.0f) {
    		updateTargetPositionBasedOnCollision(touchingNormal);
    	}
    }*/

    int maxIter = 10;

    while (fraction > 0.01f && maxIter-- > 0) {
      start.origin.set(currentPosition);
      end.origin.set(targetPosition);

      KinematicClosestNotMeConvexResultCallback callback =
          new KinematicClosestNotMeConvexResultCallback(
              ghostObject, upAxisDirection[upAxis], -1.0f);
      callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
      callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;

      double margin = convexShape.getMargin();
      convexShape.setMargin(margin + addedMargin);

      if (useGhostObjectSweepTest) {
        ghostObject.convexSweepTest(
            convexShape,
            start,
            end,
            callback,
            collisionWorld.getDispatchInfo().allowedCcdPenetration);
      } else {
        collisionWorld.convexSweepTest(convexShape, start, end, callback);
      }

      convexShape.setMargin(margin);

      fraction -= callback.closestHitFraction;

      if (callback.hasHit()) {
        // we moved only a fraction
        Vector3d hitDistanceVec = new Vector3d();
        hitDistanceVec.sub(callback.hitPointWorld, currentPosition);
        // double hitDistance = hitDistanceVec.length();

        // if the distance is farther than the collision margin, move
        // if (hitDistance > addedMargin) {
        //	//printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
        //	currentPosition.interpolate(currentPosition, targetPosition,
        // callback.closestHitFraction);
        // }

        updateTargetPositionBasedOnCollision(callback.hitNormalWorld);

        Vector3d currentDir = new Vector3d();
        currentDir.sub(targetPosition, currentPosition);
        distance2 = currentDir.lengthSquared();
        if (distance2 > BulletGlobals.SIMD_EPSILON) {
          currentDir.normalize();
          // see Quake2: "If velocity is against original velocity, stop ead to avoid tiny
          // oscilations in sloping corners."
          if (currentDir.dot(normalizedDirection) <= 0.0f) {
            break;
          }
        } else {
          // printf("currentDir: don't normalize a zero vector\n");
          break;
        }
      } else {
        // we moved whole way
        currentPosition.set(targetPosition);
      }

      // if (callback.m_closestHitFraction == 0.f)
      //    break;
    }
  }
Пример #7
0
  public void addObjectsToCollisionDomain() {
    Iterator<Entry<MTComponent, ArrayList<MTTriangleMesh>>> groupIter =
        collisionGroups.entrySet().iterator();

    int collidesWith = 0;
    while (groupIter.hasNext()) {
      collidesWith = ~groupId;

      Entry<MTComponent, ArrayList<MTTriangleMesh>> element = groupIter.next();

      // CollisionShape shape = createMeshShapeFromMTMeshTriangle(element.getValue());
      Iterator<MTTriangleMesh> iter = element.getValue().iterator();

      while (iter.hasNext()) {
        MTComponent comp = iter.next();
        CollisionShape shape = createMeshShapeFromMTMeshTriangle((MTTriangleMesh) comp);

        /*	Transform startTransform = new Transform();
        startTransform.setIdentity();

        Vector3f vec = new Vector3f();
        Vector3D translate = currentMesh.getCenterPointRelativeToParent();
        vec.x = translate.x;
        vec.y = translate.y;
        vec.z = translate.z;

        startTransform.origin.set(vec);*/

        Transform startTransform = new Transform();
        startTransform.setIdentity();
        Matrix mat = element.getKey().getGlobalMatrix();

        Vector3f vec = new Vector3f(0.0f, 0.0f, 0.0f);
        vec.x = mat.m03;
        vec.y = mat.m13;
        vec.z = mat.m23;

        // startTransform.transform(vec);
        // Matrix4f mat = CollisionManager.convertMT4JMatrixToMatrix4f(mesh.get);

        // startTransform.set(mat);
        // startTransform.origin.set(vec);
        // mat4f.m03 = mat4f.m03 + vec.x;
        // mat4f.m13 = mat4f.m13 + vec.y;
        // mat4f.m23 = mat4f.m23 + vec.z;
        // mat4f.setTranslation(vec);

        startTransform.origin.set(vec);

        Vector3f scale = new Vector3f(); // get scale value of global matrix

        Vector3D xVec = new Vector3D(mat.m00, mat.m01, mat.m02);
        Vector3D yVec = new Vector3D(mat.m10, mat.m11, mat.m12);
        Vector3D zVec = new Vector3D(mat.m20, mat.m21, mat.m22);

        scale.x = xVec.length();
        scale.y = yVec.length();
        scale.z = zVec.length();

        float[] scaleVals = new float[3];
        scale.get(scaleVals);

        for (int i = 0; i < 3; i++) // get rotation value by extracting scalation
        {

          try {
            float[] colvals = mat.getRow(i);

            for (int j = 0; j < 3; j++) {
              colvals[j] = colvals[j] / scaleVals[i];
            }
            startTransform.basis.setRow(i, colvals);
          } catch (Exception e) {
            // TODO Auto-generated catch block
            e.printStackTrace();
          }
        }

        // startTransform.set(mat4f);

        float mass = 5f; // fake mass value only needed correctly if used with dynamic engine
        shape.setLocalScaling(scale);
        GImpactMeshShape sh = (GImpactMeshShape) shape;
        sh.updateBound();

        RigidBody body = localCreateRigidBody(mass, startTransform, shape);

        // get Center Of Mass of Triangle Mesh and write it to MTTriangleMesh
        Vector3f vecCOM = new Vector3f();
        body.getCenterOfMassPosition(vecCOM);
        Vector3D vecComMT4J = new Vector3D(vecCOM.x, vecCOM.y, vecCOM.z);
        // mesh.setCenterOfMass(vecComMT4J);
        // mesh.setMass(100.0f);
        // add Object to Collision World

        collisionWorld.addCollisionObject(body, groupId, (short) collidesWith);

        colObjectToComponent.put(body, comp);

        addCollisionObjectToGroup(
            comp.getParent(), body); // save association between collision objects and groups
      }
      groupId = (short) (groupId << 1); // shift groupId so every group has a unique bit value
    }
    SimulatePreDrawAction calcDynamics =
        new SimulatePreDrawAction(collisionWorld, this, scene.getCanvas());
    calcDynamics.setCurrentTimeStep(1.f / 1000000000000.0f);
    scene.registerPreDrawAction(calcDynamics);
  }