trimeshTrimesh method

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

Implementation

bool trimeshTrimesh(
  Trimesh 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

  // Construct a temp hull for each triangle
  final va = Vector3.zero();
  final vb = Vector3.zero();
  final vc = Vector3.zero();
  final hullB = ConvexPolyhedron(
    vertices:[va,vb,vc],
    faces: [[0, 1, 2]],
  );
  final triangleNormal = Vector3.zero();

  for (int i = 0; i < si.indices.length / 3; i++) {
    bool intersecting = false;
    si.getTriangleVertices(i, va, vb, vc);
    si.getIndicesNormal(i, triangleNormal);

    //hullB.faceNormals = [triangleNormal];

    trimeshConvex(hullB, sj, xi, xj, qi, qj, bi, bj);


    sj.getTriangleVertices(sj.indices[triangles[i]], va, vb, vc);
    // Vector3 offsetResult = Vector3(
    //   (va.x+vb.x+vc.x)/3,
    //   (va.y+vb.y+vc.y)/3,
    //   (va.z+vb.z+vc.z)/3
    // );
    Transform.pointToWorldFrame(xj, qj, xi, worldPillarOffset);
    if (
      xi.distanceTo(worldPillarOffset) < sj.boundingSphereRadius
    ) {
      intersecting = trimeshConvex(hullB, sj, xj, xi, qj, qi, bj, bi, rsi, rsj, justTest);
    }

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