sphereBox method

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

Implementation

bool sphereBox(
  Sphere si,
  Box sj,
  Vec3 xi,
  Vec3 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.vsub(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.copy(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.copy(sides[(idx + 1) % 3]);
      ns2.copy(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.copy(ns);
          sideNs1.copy(ns1);
          sideNs2.copy(ns2);
          sidePenetrations++;

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

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

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

  // Check corners
  dynamic 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.set(0.0, 0.0, 0.0);
        if (j != 0) {
          rj.vadd(sides[0], rj);
        } else {
          rj.vsub(sides[0], rj);
        }
        if (k != 0) {
          rj.vadd(sides[1], rj);
        } else {
          rj.vsub(sides[1], rj);
        }
        if (l != 0) {
          rj.vadd(sides[2], rj);
        } else {
          rj.vsub(sides[2], rj);
        }

        // World position of corner
        xj.vadd(rj, sphereToCorner);
        sphereToCorner.vsub(xi, sphereToCorner);

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

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

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

  // Check edges
  final edgeTangent = v3pool.get();
  final edgeCenter = v3pool.get();
  final r = v3pool.get(); // r = edge center to sphere center
  final orthogonal = v3pool.get();
  final dist = v3pool.get();
  final 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].cross(sides[j], edgeTangent);
        edgeTangent.normalize();
        sides[j].vadd(sides[k], edgeCenter);
        r.copy(xi);
        r.vsub(edgeCenter, r);
        r.vsub(xj, r);
        final orthonorm = r.dot(edgeTangent); // distance from edge center to sphere center in the tangent direction
        edgeTangent.scale(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.copy(xi);
        dist.vsub(orthogonal, dist);
        dist.vsub(edgeCenter, dist);
        dist.vsub(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.vadd(orthogonal, res.rj); // box rj
          res.rj.copy(res.rj);
          dist.negate(res.ni);
          res.ni.normalize();

          res.ri.copy(res.rj);
          res.ri.vadd(xj, res.ri);
          res.ri.vsub(xi, res.ri);
          res.ri.normalize();
          res.ri.scale(R, res.ri);

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

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

  return false;
}