sphereTrimesh method

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

Implementation

bool sphereTrimesh(
  Sphere si,
  Trimesh sj,
  Vec3 xi,
  Vec3 xj,
  Quaternion qi,
  Quaternion qj,
  Body bi,
  Body bj,
  [
    Shape? rsi,
    Shape? rsj,
    bool justTest = false
  ]
){
  final edgeVertexA = _sphereTrimeshEdgeVertexA;
  final edgeVertexB = _sphereTrimeshEdgeVertexB;
  final edgeVector = _sphereTrimeshEdgeVector;
  final edgeVectorUnit = _sphereTrimeshEdgeVectorUnit;
  final local = _sphereTrimeshlocal;
  final localSphereAABB = _sphereTrimeshLocalSphereAABB;
  final tmp = _sphereTrimeshTmp;
  final v2 = _sphereTrimeshV2;
  final relpos = _sphereTrimeshRelpos;
  final triangles = _sphereTrimeshTriangles;

  // Convert sphere position to local in the trimesh
  Transform.pointToLocalFrame(xj, qj, xi, local);

  // Get the aabb of the sphere locally in the trimesh
  final sphereRadius = si.radius;
  localSphereAABB.lowerBound.set(
    local.x - sphereRadius,
    local.y - sphereRadius,
    local.z - sphereRadius
  );
  localSphereAABB.upperBound.set(
    local.x + sphereRadius,
    local.y + sphereRadius,
    local.z + sphereRadius
  );

  sj.getTrianglesInAABB(localSphereAABB, triangles); //TODO fix retreived triangles

  final v = _sphereTrimeshV;
  final radiusSquared = si.radius * si.radius;
  for (int i = 0; i < sj.indices.length/3; i++){//triangles.length
    for (int j = 0; j < 3; j++) {

      sj.getVertex(sj.indices[i*3 + j], v);//i->triangles[i]
      // Check vertex overlap in sphere
      v.vsub(local, relpos);

      if (relpos.lengthSquared() <= radiusSquared) {
        // Safe up
        v2.copy(v);
        Transform.pointToWorldFrame(xj, qj, v2, v);

        v.vsub(xi, relpos);

        if (justTest) {
          return true;
        }

        final r = createContactEquation(bi, bj, si, sj, rsi, rsj);
        r.ni.copy(relpos);
        r.ni.normalize();

        // ri is the vector from sphere center to the sphere surface
        r.ri.copy(r.ni);
        r.ri.scale(si.radius, r.ri);

        r.ri.vadd(xi, r.ri);
        r.ri.vsub(bi.position, r.ri);
        r.rj.copy(v);
        r.rj.vsub(bj.position, r.rj);

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

  // Check all edges
  for (int i = 0; i < sj.indices.length/3; i++) {
    for (int j = 0; j < 3; j++) {
      sj.getVertex(sj.indices[i*3 + j], edgeVertexA);
      sj.getVertex(sj.indices[i*3 + ((j + 1) % 3)], edgeVertexB);
      edgeVertexB.vsub(edgeVertexA, edgeVector);

      // Project sphere position to the edge
      local.vsub(edgeVertexB, tmp);
      final positionAlongEdgeB = tmp.dot(edgeVector);

      local.vsub(edgeVertexA, tmp);
      double positionAlongEdgeA = tmp.dot(edgeVector);

      if (positionAlongEdgeA > 0 && positionAlongEdgeB < 0) {
        // Now check the orthogonal distance from edge to sphere center
        local.vsub(edgeVertexA, tmp);
        edgeVectorUnit.copy(edgeVector);
        edgeVectorUnit.normalize();
        positionAlongEdgeA = tmp.dot(edgeVectorUnit);
        edgeVectorUnit.scale(positionAlongEdgeA, tmp);
        tmp.vadd(edgeVertexA, tmp); // tmp is now the sphere center position projected to the edge, defined locally in the trimesh frame

        final dist = tmp.distanceTo(local);
        if (dist < si.radius) {
          if (justTest) {
            return true;
          }
          final r = createContactEquation(bi, bj, si, sj, rsi, rsj);

          tmp.vsub(local, r.ni);
          r.ni.normalize();
          r.ni.scale(si.radius, r.ri);
          r.ri.vadd(xi, r.ri);
          r.ri.vsub(bi.position, r.ri);

          Transform.pointToWorldFrame(xj, qj, tmp, tmp);
          tmp.vsub(bj.position, r.rj);

          Transform.vectorToWorldFrame(qj, r.ni, r.ni);
          Transform.vectorToWorldFrame(qj, r.ri, r.ri);

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

  // Triangle faces
  final va = _sphereTrimeshVa;
  final vb = _sphereTrimeshVb;
  final vc = _sphereTrimeshVc;
  final normal = _sphereTrimeshNormal;
  for (int i = 0; i < sj.indices.length/3; i++) {//N = triangles.length; i != N
    sj.getTriangleVertices( sj.indices[i], va, vb, vc);
    sj.getFaceNormal(sj.indices[i], normal);

    local.vsub(va, tmp);
    double dist = tmp.dot(normal);
    normal.scale(dist, tmp);
    local.vsub(tmp, tmp);

    // tmp is now the sphere position projected to the triangle plane
    dist = tmp.distanceTo(local);
    if (Ray.pointInTriangle(tmp, va, vb, vc) && dist < si.radius) {
      if (justTest) {
        return true;
      }
      final r = createContactEquation(bi, bj, si, sj, rsi, rsj);

      tmp.vsub(local, r.ni);
      r.ni.normalize();
      r.ni.scale(si.radius, r.ri);
      r.ri.vadd(xi, r.ri);
      r.ri.vsub(bi.position, r.ri);

      Transform.pointToWorldFrame(xj, qj, tmp, tmp);
      tmp.vsub(bj.position, r.rj);

      Transform.vectorToWorldFrame(qj, r.ni, r.ni);
      Transform.vectorToWorldFrame(qj, r.ri, r.ri);

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

  triangles.clear();
  return false;
}