sphereConvex method

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

Implementation

bool sphereConvex(
  Sphere si,
  ConvexPolyhedron sj,
  Vec3 xi,
  Vec3 xj,
  Quaternion qi,
  Quaternion qj,
  Body bi,
  Body bj,
  [
    Shape? rsi,
    Shape? rsj,
    bool justTest = false
]){
  final v3pool = this.v3pool;
  xi.vsub(xj, _convexToSphere);
  final normals = sj.faceNormals;
  final faces = sj.faces;
  final verts = sj.vertices;
  final R = si.radius;
  bool found = false;

  // Check corners
  for (int i = 0; i != verts.length; i++) {
    final v = verts[i];

    // World position of corner
    final worldCorner = _sphereConvexWorldCorner;
    qj.vmult(v, worldCorner);
    xj.vadd(worldCorner, worldCorner);
    final sphereToCorner = _sphereConvexSphereToCorner;
    worldCorner.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);
      worldCorner.vsub(xj, r.rj);

      // Should be relative to the body.
      r.ri.vadd(xi, r.ri);
      r.ri.vsub(bi.position, r.ri);

      // Should be relative to the body.
      r.rj.vadd(xj, r.rj);
      r.rj.vsub(bj.position, r.rj);

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

  // Check side (plane) intersections
  for (int i = 0, nfaces = faces.length; i != nfaces && found == false; i++) {
    final normal = normals[i]!;
    final face = faces[i];

    // Get world-transformed normal of the face
    final worldNormal = _sphereConvexWorldNormal;
    qj.vmult(normal, worldNormal);

    // Get a world vertex from the face
    final worldPoint = _sphereConvexWorldPoint;
    qj.vmult(verts[face[0]], worldPoint);
    worldPoint.vadd(xj, worldPoint);

    // Get a point on the sphere, closest to the face normal
    final worldSpherePointClosestToPlane = _sphereConvexWorldSpherePointClosestToPlane;
    worldNormal.scale(-R, worldSpherePointClosestToPlane);
    xi.vadd(worldSpherePointClosestToPlane, worldSpherePointClosestToPlane);

    // Vector from a face point to the closest point on the sphere
    final penetrationVec = _sphereConvexPenetrationVec;
    worldSpherePointClosestToPlane.vsub(worldPoint, penetrationVec);

    // The penetration. Negative value means overlap.
    final penetration = penetrationVec.dot(worldNormal);

    final worldPointToSphere = _sphereConvexSphereToWorldPoint;
    xi.vsub(worldPoint, worldPointToSphere);

    if (penetration < 0 && worldPointToSphere.dot(worldNormal) > 0) {
      // Intersects plane. Now check if the sphere is inside the face polygon
      final List<Vec3> faceVerts = []; // Face vertices, in world coords
      for (int j = 0, nVerts = face.length; j != nVerts; j++) {
        final worldVertex = v3pool.get();
        qj.vmult(verts[face[j]], worldVertex);
        xj.vadd(worldVertex, worldVertex);
        faceVerts.add(worldVertex);
      }

      if (_pointInPolygon(faceVerts, worldNormal, xi)) {
        // Is the sphere center in the face polygon?
        if (justTest) {
          return true;
        }
        found = true;
        final r = createContactEquation(bi, bj, si, sj, rsi, rsj);

        worldNormal.scale(-R, r.ri); // Contact offset, from sphere center to contact
        worldNormal.negate(r.ni); // Normal pointing out of sphere

        final penetrationVec2 = v3pool.get();
        worldNormal.scale(-penetration, penetrationVec2);
        final penetrationSpherePoint = v3pool.get();
        worldNormal.scale(-R, penetrationSpherePoint);

        //xi.vsub(xj).vadd(penetrationSpherePoint).vadd(penetrationVec2 , r.rj);
        xi.vsub(xj, r.rj);
        r.rj.vadd(penetrationSpherePoint, r.rj);
        r.rj.vadd(penetrationVec2, r.rj);

        // Should be relative to the body.
        r.rj.vadd(xj, r.rj);
        r.rj.vsub(bj.position, r.rj);

        // Should be relative to the body.
        r.ri.vadd(xi, r.ri);
        r.ri.vsub(bi.position, r.ri);

        v3pool.release([penetrationVec2,penetrationSpherePoint]);

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

        // Release world vertices
        for (int j = 0, nFaceverts = faceVerts.length; j != nFaceverts; j++) {
          v3pool.release([faceVerts[j]]);
        }

        return false; // We only expect *one* face contact
      } else {
        // Edge?
        for (int j = 0; j != face.length; j++) {
          // Get two world transformed vertices
          final v1 = v3pool.get();
          final v2 = v3pool.get();
          qj.vmult(verts[face[(j + 1) % face.length]], v1);
          qj.vmult(verts[face[(j + 2) % face.length]], v2);
          xj.vadd(v1, v1);
          xj.vadd(v2, v2);

          // Construct edge vector
          final edge = _sphereConvexEdge;
          v2.vsub(v1, edge);

          // Construct the same vector, but normalized
          final edgeUnit = _sphereConvexEdgeUnit;
          edge.unit(edgeUnit);

          // p is xi projected onto the edge
          final p = v3pool.get() as Vec3;
          final v1ToXi = v3pool.get() as Vec3;
          xi.vsub(v1, v1ToXi);
          final dot = v1ToXi.dot(edgeUnit);
          edgeUnit.scale(dot, p);
          p.vadd(v1, p);

          // Compute a vector from p to the center of the sphere
          final xiToP = v3pool.get();
          p.vsub(xi, xiToP);

          // Collision if the edge-sphere distance is less than the radius
          // AND if p is in between v1 and v2
          if (dot > 0 && dot * dot < edge.lengthSquared() && xiToP.lengthSquared() < R * R) {
            // Collision if the edge-sphere distance is less than the radius
            // Edge contact!
            if (justTest) {
              return true;
            }
            final r = createContactEquation(bi, bj, si, sj, rsi, rsj);
            p.vsub(xj, r.rj);

            p.vsub(xi, r.ni);
            r.ni.normalize();

            r.ni.scale(R, r.ri);

            // Should be relative to the body.
            r.rj.vadd(xj, r.rj);
            r.rj.vsub(bj.position, r.rj);

            // Should be relative to the body.
            r.ri.vadd(xi, r.ri);
            r.ri.vsub(bi.position, r.ri);

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

            // Release world vertices
            for (int j = 0, nFaceverts = faceVerts.length; j != nFaceverts; j++) {
              v3pool.release([faceVerts[j]]);
            }

            v3pool.release([v1,v2,p,xiToP,v1ToXi]);
            return false;
          }

          v3pool.release([v1,v2,p,xiToP,v1ToXi]);
        }
      }

      // Release world vertices
      List<Vec3> toRelease = [];
      for (int j = 0, nFaceverts = faceVerts.length; j != nFaceverts; j++) {
        toRelease.add(faceVerts[j]);
      }
      v3pool.release(toRelease);
    }
  }

  return false;
}