/**
   * Finds the closest height point to a position. Will always be left/above that position.
   *
   * @param position the position to check at
   * @param block the block to get height values from
   * @return an index to the height position of the given block.
   */
  protected int findClosestHeightIndex(Vector3f position, TerrainBlock block) {

    int x = (int) position.x;
    int z = (int) position.z;

    if (x < 0 || x >= block.getSize() - 1) {
      return -1;
    }
    if (z < 0 || z >= block.getSize() - 1) {
      return -1;
    }

    return z * block.getSize() + x;
  }
  /**
   * Request the triangles (in world coord space) of a TerrainBlock that correspond to the given
   * grid location. The triangles are stored in the class fields _gridTriA and _gridTriB.
   *
   * @param gridX grid row
   * @param gridY grid column
   * @param block the TerrainBlock we are working with
   * @return true if the grid point is valid for the given block, false if it is off the block.
   */
  protected boolean getTriangles(float gridX, float gridY, TerrainBlock block) {
    _calcVec1.set(gridX, 0, gridY);
    int index = findClosestHeightIndex(_calcVec1, block);
    if (index == -1) return false;
    float h3 = block.getHeightMap()[index + block.getSize()];
    float h4 = block.getHeightMap()[index + block.getSize() + 1];
    float h1 = block.getHeightMap()[index];
    float h2 = block.getHeightMap()[index + 1];

    final Vector3f scaleVec =
        _calcVec1.set(_tracer.getGridSpacing()).multLocal(block.getWorldScale());

    _gridTriA.get(0).x = (gridX);
    _gridTriA.get(0).y = (h1);
    _gridTriA.get(0).z = (gridY);
    _gridTriA.get(0).multLocal(scaleVec).addLocal(_tracer.getGridOrigin());

    _gridTriA.get(1).x = (gridX);
    _gridTriA.get(1).y = (h3);
    _gridTriA.get(1).z = (gridY + 1);
    _gridTriA.get(1).multLocal(scaleVec).addLocal(_tracer.getGridOrigin());

    _gridTriA.get(2).x = (gridX + 1);
    _gridTriA.get(2).y = (h2);
    _gridTriA.get(2).z = (gridY);
    _gridTriA.get(2).multLocal(scaleVec).addLocal(_tracer.getGridOrigin());

    _gridTriB.get(0).x = (gridX + 1);
    _gridTriB.get(0).y = (h2);
    _gridTriB.get(0).z = (gridY);
    _gridTriB.get(0).multLocal(scaleVec).addLocal(_tracer.getGridOrigin());

    _gridTriB.get(1).x = (gridX);
    _gridTriB.get(1).y = (h3);
    _gridTriB.get(1).z = (gridY + 1);
    _gridTriB.get(1).multLocal(scaleVec).addLocal(_tracer.getGridOrigin());

    _gridTriB.get(2).x = (gridX + 1);
    _gridTriB.get(2).y = (h4);
    _gridTriB.get(2).z = (gridY + 1);
    _gridTriB.get(2).multLocal(scaleVec).addLocal(_tracer.getGridOrigin());

    return true;
  }
  /**
   * Ask for the point of intersection between the given ray and the terrain.
   *
   * @param worldPick our pick ray, in world space.
   * @param store if not null, the results will be stored in this vector and the vector returned. If
   *     store is null, a new vector will be created.
   * @return null if no pick is found. Otherwise it returns a Vector3f (the store param, if that was
   *     not null) populated with the pick coordinates.
   */
  public Vector3f getTerrainIntersection(final Ray worldPick, final Vector3f store) {
    if (_root == null || _root.getWorldBound() == null) return null;

    if (!_root.getWorldBound().intersects(worldPick)) return null;

    // Set up our working ray
    _workRay.set(worldPick);

    // Grab all terrain blocks and do a grid walk on each starting from
    // closest.
    _pr.clear();
    _root.findPick(_workRay, _pr);

    for (int i = 0, max = _pr.getNumber(); i < max; i++) {
      PickData pd = _pr.getPickData(i);
      if (pd == null) continue;

      Geometry g = pd.getTargetMesh();
      if (g instanceof TerrainBlock) {
        TerrainBlock block = (TerrainBlock) g;

        _tracer.getGridSpacing().set(block.getWorldScale()).multLocal(block.getStepScale());
        _tracer.setGridOrigin(block.getWorldTranslation());

        _workRay
            .getOrigin()
            .set(worldPick.getDirection())
            .multLocal(pd.getDistance() - .1f)
            .addLocal(worldPick.getOrigin());

        _tracer.startWalk(_workRay);

        if (_tracer.isRayPerpendicularToGrid()) {
          // no intersection
          return null;
        }

        final Vector3f intersection = store != null ? store : new Vector3f();
        final Vector2f loc = _tracer.getGridLocation();

        while (loc.x >= -1 && loc.x <= block.getSize() && loc.y >= -1 && loc.y <= block.getSize()) {

          // check the triangles of main square for intersection.
          if (checkTriangles(loc.x, loc.y, _workRay, intersection, block)) {
            // we found an intersection, so return that!
            return intersection;
          }

          // because of how we get our height coords, we will
          // sometimes be off be a grid spot, so we check the next
          // grid space up.
          int dx = 0, dz = 0;
          Direction d = _tracer.getLastStepDirection();
          switch (d) {
            case PositiveX:
            case NegativeX:
              dx = 0;
              dz = 1;
              break;
            case PositiveZ:
            case NegativeZ:
              dx = 1;
              dz = 0;
              break;
          }

          if (checkTriangles(loc.x + dx, loc.y + dz, _workRay, intersection, block)) {
            // we found an intersection, so return that!
            return intersection;
          }

          _tracer.next();
        }
      }
    }

    return null;
  }