particleTrimesh method

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

Implementation

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

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

  const sphereRadius = 0.001;
  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

  final tmp = _sphereTrimeshV2;
  final relpos = _sphereTrimeshRelpos;
  final v = Vec3();
  final v2 = Vec3();
  final normal = Vec3();

  // For each world polygon in the polyhedra
  for (int i = 0; i < sj.indices.length/3; i++) {
    for (int j = 0; j < 3; j++) {
      sj.getNormal(sj.indices[i*3 + j], normal);
      sj.getVertex(sj.indices[i*3 + j], v);
      // Check vertex overlap in sphere
      v.vsub(local, relpos);

      if (relpos.lengthSquared() < 1) {
        v2.copy(v);
        Transform.pointToWorldFrame(xj, qj, v2, v);

        xi.vsub(bj.position, relpos);
        if (justTest) {
          return true;
        }

        var r = createContactEquation(bi, bj, si, sj, rsi, rsj);
        r.ni.copy(normal); // Contact normal is the plane normal
        r.ni.normalize();

        normal.scale(0.0001, tmp);
        // rj is the particle position projected to the face
        tmp.vadd(xi, tmp);
        tmp.vsub(xj, tmp);

        normal.negate(r.ni);
        r.ri.set(0,0,0); // Center of particle

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

        r.rj.copy(tmp);
        r.rj.vadd(xj, r.rj);
        r.rj.vsub(bj.position, r.rj);

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

  triangles.clear();

  return false;
}