sphereHeightfield method

bool sphereHeightfield(Sphere si, Heightfield sj, Vec3 xi, Vec3 xj, Quaternion qi, Quaternion qj, Body bi, Body bj, [ Shape? rsi, Shape? rsj, bool justTest = false, ])

Implementation

bool sphereHeightfield(
  Sphere si,
  Heightfield sj,
  Vec3 xi,
  Vec3 xj,
  Quaternion qi,
  Quaternion qj,
  Body bi,
  Body bj,
  [
    Shape? rsi,
    Shape? rsj,
    bool justTest = false
]){
  final data = sj.data;
  final radius = si.radius;
  final w = sj.elementSize;
  final worldPillarOffset = _sphereHeightfieldTmp2;

  // Get sphere position to heightfield local!
  final local = _sphereHeightfieldTmp1;
  Transform.pointToLocalFrame(xj, qj, xi, local);

  // Get the index of the data points to test against
  int iMinX = ((local.x - radius) / w).floor() - 1;
  int iMaxX = ((local.x + radius) / w).ceil() + 1;
  int iMinY = ((local.y - radius) / w).floor() - 1;
  int iMaxY = ((local.y + radius) / w).ceil() + 1;
  // Bail out if we are out of the terrain
  if (iMaxX < 0 || iMaxY < 0 || iMinX > data.length || iMinY > data[0].length) {
    return false;
  }

  // Clamp index to edges
  if (iMinX < 0) {
    iMinX = 0;
  }
  if (iMaxX < 0) {
    iMaxX = 0;
  }
  if (iMinY < 0) {
    iMinY = 0;
  }
  if (iMaxY < 0) {
    iMaxY = 0;
  }
  if (iMinX >= data.length) {
    iMinX = data.length - 1;
  }
  if (iMaxX >= data.length) {
    iMaxX = data.length - 1;
  }
  if (iMaxY >= data[0].length) {
    iMaxY = data[0].length - 1;
  }
  if (iMinY >= data[0].length) {
    iMinY = data[0].length - 1;
  }

  List<double> minMax = [];
  sj.getRectMinMax(iMinX, iMinY, iMaxX, iMaxY, minMax);
  final min = minMax[0];
  final max = minMax[1];

  // Bail out if we can't touch the bounding height box
  if (local.z - radius > max || local.z + radius < min) {
    return false;
  }

  final result = this.result;
  for (int i = iMinX; i < iMaxX; i++) {
    for (int j = iMinY; j < iMaxY; j++) {
      final numContactsBefore = result.length;

      bool intersecting = false;

      // Lower triangle
      sj.getConvexTrianglePillar(i, j, false);
      Transform.pointToWorldFrame(xj, qj, sj.pillarOffset, worldPillarOffset);
      if (
        xi.distanceTo(worldPillarOffset) <
        sj.pillarConvex.boundingSphereRadius + si.boundingSphereRadius
      ) {
        intersecting = sphereConvex(
          si,
          sj.pillarConvex,
          xi,
          worldPillarOffset,
          qi,
          qj,
          bi,
          bj,
          si,
          sj,
          justTest
        );
      }

      if (justTest && intersecting) {
        return true;
      }

      // Upper triangle
      sj.getConvexTrianglePillar(i, j, true);
      Transform.pointToWorldFrame(xj, qj, sj.pillarOffset, worldPillarOffset);
      if (
        xi.distanceTo(worldPillarOffset) <
        sj.pillarConvex.boundingSphereRadius + si.boundingSphereRadius
      ) {
        intersecting = sphereConvex(
          si,
          sj.pillarConvex,
          xi,
          worldPillarOffset,
          qi,
          qj,
          bi,
          bj,
          si,
          sj,
          justTest
        );
      }

      if (justTest && intersecting) {
        return true;
      }

      final numContacts = result.length - numContactsBefore;

      if (numContacts > 2) {
        return false;
      }
      /*
        // Skip all but 1
        for (let k = 0; k < numContacts - 1; k++) {
            result.pop();
        }
      */
    }
  }
  return false;
}