convexHeightfield method

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

Implementation

bool convexHeightfield(
  ConvexPolyhedron 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 w = sj.elementSize;
  final radius = si.boundingSphereRadius;
  final worldPillarOffset = _convexHeightfieldTmp2;
  final faceList = convexHeightfieldFaceList;

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

  // Get the index of the data points to test against
  int iMinX = ((localConvexPos.x - radius) / w).floor() - 1;

  int iMaxX = ((localConvexPos.x + radius) / w).ceil() + 1;
  int iMinY = ((localConvexPos.y - radius) / w).floor() - 1;
  int iMaxY = ((localConvexPos.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;
  }

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

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

  for (int i = iMinX; i < iMaxX; i++) {
    for (int j = iMinY; j < iMaxY; j++) {
      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 = convexConvex(
          si,
          sj.pillarConvex,
          xi,
          worldPillarOffset,
          qi,
          qj,
          bi,
          bj,
          null,
          null,
          justTest,
          faceList,
          null
        );
      }

      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 = convexConvex(
          si,
          sj.pillarConvex,
          xi,
          worldPillarOffset,
          qi,
          qj,
          bi,
          bj,
          null,
          null,
          justTest,
          faceList,
          null
        );
      }

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

  return false;
}