sphereBox method

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

Implementation

bool sphereBox(
  Sphere si,
  Box sj,
  Vector3 xi,
  Vector3 xj,
  Quaternion qi,
  Quaternion qj,
  Body bi,
  Body bj,
  [
    Shape? rsi,
    Shape? rsj,
    bool justTest = false
]){
  final v3pool = this.v3pool;

  // we refer to the box as body j
  final sides = sphereBoxSides;
  xi.sub2(xj, _boxToSphere);
  sj.getSideNormals(sides, qj);
  final R = si.radius;
  //final penetrating_sides = [];

  // Check side (plane) intersections
  bool found = false;

  // Store the resulting side penetration info
  final sideNs = _sphereBoxSideNs;
  final sideNs1 = _sphereBoxSideNs1;
  final sideNs2 = _sphereBoxSideNs2;

  double? sideH;
  double sidePenetrations = 0;
  double sideDot1 = 0;
  double sideDot2 = 0;
  double? sideDistance;
  for (int idx = 0, nsides = sides.length; idx != nsides && found == false; idx++) {
    // Get the plane side normal (ns)
    final ns = _sphereBoxNs;
    ns.setFrom(sides[idx]);

    final h = ns.length;
    ns.normalize();

    // The normal/distance dot product tells which side of the plane we are
    final dot = _boxToSphere.dot(ns);

    if (dot < h + R && dot > 0) {
      // Intersects plane. Now check the other two dimensions
      final ns1 = _sphereBoxNs1;
      final ns2 = _sphereBoxNs2;
      ns1.setFrom(sides[(idx + 1) % 3]);
      ns2.setFrom(sides[(idx + 2) % 3]);
      final h1 = ns1.length;
      final h2 = ns2.length;
      ns1.normalize();
      ns2.normalize();
      final dot1 = _boxToSphere.dot(ns1);
      final dot2 = _boxToSphere.dot(ns2);
      if (dot1 < h1 && dot1 > -h1 && dot2 < h2 && dot2 > -h2) {
        final dist = (dot - h - R).abs();
        if (sideDistance == null || dist < sideDistance) {
          sideDistance = dist;
          sideDot1 = dot1;
          sideDot2 = dot2;
          sideH = h;
          sideNs.setFrom(ns);
          sideNs1.setFrom(ns1);
          sideNs2.setFrom(ns2);
          sidePenetrations++;

          if (justTest) {
            return true;
          }
        }
      }
    }
  }
  if (sidePenetrations != 0) {
    found = true;
    final r = createContactEquation(bi, bj, si, sj, rsi, rsj);
    sideNs.scale2(-R, r.ri); // Sphere r
    r.ni.setFrom(sideNs);
    r.ni.negate(); // Normal should be out of sphere
    sideNs.scale2(sideH!, sideNs);
    sideNs1.scale2(sideDot1, sideNs1);
    sideNs.add2(sideNs1, sideNs);
    sideNs2.scale2(sideDot2, sideNs2);
    sideNs.add2(sideNs2, r.rj);

    // Make relative to bodies
    r.ri.add2(xi, r.ri);
    r.ri.sub2(bi.position, r.ri);
    r.rj.add2(xj, r.rj);
    r.rj.sub2(bj.position, r.rj);

    result.add(r);
    createFrictionEquationsFromContact(r, frictionResult);
  }

  // Check corners
  Vector3? rj = v3pool.get();
  final sphereToCorner = _sphereBoxSphereToCorner;
  for (int j = 0; j != 2 && !found; j++) {
    for (int k = 0; k != 2 && !found; k++) {
      for (int l = 0; l != 2 && !found; l++) {
        rj.setValues(0.0, 0.0, 0.0);
        if (j != 0) {
          rj.add2(sides[0], rj);
        } else {
          rj.sub2(sides[0], rj);
        }
        if (k != 0) {
          rj.add2(sides[1], rj);
        } else {
          rj.sub2(sides[1], rj);
        }
        if (l != 0) {
          rj.add2(sides[2], rj);
        } else {
          rj.sub2(sides[2], rj);
        }

        // World position of corner
        xj.add2(rj, sphereToCorner);
        sphereToCorner.sub2(xi, sphereToCorner);

        if (sphereToCorner.length2 < R * R) {
          if (justTest) {
            return true;
          }
          found = true;
          final r = createContactEquation(bi, bj, si, sj, rsi, rsj);
          r.ri.setFrom(sphereToCorner);
          r.ri.normalize();
          r.ni.setFrom(r.ri);
          r.ri.scale2(R, r.ri);
          r.rj.setFrom(rj);

          // Make relative to bodies
          r.ri.add2(xi, r.ri);
          r.ri.sub2(bi.position, r.ri);
          r.rj.add2(xj, r.rj);
          r.rj.sub2(bj.position, r.rj);

          result.add(r);
          createFrictionEquationsFromContact(r, frictionResult);
        }
      }
    }
  }
  v3pool.release([rj]);
  rj = null;

  // Check edges
  final Vector3 edgeTangent = v3pool.get();
  final Vector3 edgeCenter = v3pool.get();
  final Vector3 r = v3pool.get(); // r = edge center to sphere center
  final Vector3 orthogonal = v3pool.get();
  final Vector3 dist = v3pool.get();
  final int nSides = sides.length;
  for (int j = 0; j != nSides && !found; j++) {
    for (int k = 0; k != nSides && !found; k++) {
      if (j % 3 != k % 3) {
        // Get edge tangent
        sides[k].cross2(sides[j], edgeTangent);
        edgeTangent.normalize();
        sides[j].add2(sides[k], edgeCenter);
        r.setFrom(xi);
        r.sub2(edgeCenter, r);
        r.sub2(xj, r);
        final orthonorm = r.dot(edgeTangent); // distance from edge center to sphere center in the tangent direction
        edgeTangent.scale2(orthonorm, orthogonal); // Vector from edge center to sphere center in the tangent direction

        // Find the third side orthogonal to this one
        int l = 0;
        while (l == j % 3 || l == k % 3) {
          l++;
        }

        // vec from edge center to sphere projected to the plane orthogonal to the edge tangent
        dist.setFrom(xi);
        dist.sub2(orthogonal, dist);
        dist.sub2(edgeCenter, dist);
        dist.sub2(xj, dist);

        // Distances in tangent direction and distance in the plane orthogonal to it
        final tdist = orthonorm.abs();
        final ndist = dist.length;

        if (tdist < sides[l].length && ndist < R) {
          if (justTest) {
            return true;
          }
          found = true;
          final res = createContactEquation(bi, bj, si, sj, rsi, rsj);
          edgeCenter.add2(orthogonal, res.rj); // box rj
          res.rj.setFrom(res.rj);
          res.ni..setFrom(dist)..negate();
          res.ni.normalize();

          res.ri.setFrom(res.rj);
          res.ri.add2(xj, res.ri);
          res.ri.sub2(xi, res.ri);
          res.ri.normalize();
          res.ri.scale2(R, res.ri);

          // Make relative to bodies
          res.ri.add2(xi, res.ri);
          res.ri.sub2(bi.position, res.ri);
          res.rj.add2(xj, res.rj);
          res.rj.sub2(bj.position, res.rj);

          result.add(res);
          createFrictionEquationsFromContact(res, frictionResult);
        }
      }
    }
  }
  v3pool.release([edgeTangent, edgeCenter, r, orthogonal, dist]);

  return false;
}