particleTrimesh method

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

Implementation

bool particleTrimesh(
  Particle si,
  Trimesh sj,
  Vector3 xi,
  Vector3 xj,
  Quaternion qi,
  Quaternion qj,
  Body bi,
  Body bj,
  [
    Shape? rsi,
    Shape? rsj,
    bool justTest = false
]){
  final local = Vector3.zero();
  final localSphereAABB = _sphereTrimeshLocalSphereAABB;
  final triangles = _sphereTrimeshTriangles;
  final worldPillarOffset = _convexHeightfieldTmp2;

  Transform.pointToLocalFrame(xj, qj, xi, local);

  final sphereRadius = si.boundingSphereRadius;
  localSphereAABB.lowerBound.setValues(
    local.x - sphereRadius,
    local.y - sphereRadius,
    local.z - sphereRadius
  );
  localSphereAABB.upperBound.setValues(
    local.x + sphereRadius,
    local.y + sphereRadius,
    local.z + sphereRadius
  );

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

  final va = Vector3.zero();
  final vb = Vector3.zero();
  final vc = Vector3.zero();
  final hullB = ConvexPolyhedron(
    vertices:[va,vb,vc],
    faces: [[0, 1, 2]],
  );
  // For each world polygon in the polyhedra
  for (int i = 0; i < triangles.length; i++) {
    bool intersecting = false;
    sj.getTriangleVertices(sj.indices[triangles[i]], va, vb, vc);
    Transform.pointToWorldFrame(xj, qj, xi, worldPillarOffset);
    if (
      xi.distanceTo(worldPillarOffset) < sj.boundingSphereRadius
    ) {
      intersecting = particleConvex(hullB, si, xj, xi, qj, qi, bj, bi, rsi, rsj, justTest);
    }

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

  triangles.clear();

  return false;
}