clipFaceAgainstHull method

void clipFaceAgainstHull(Vec3 separatingNormal, Vec3 posA, Quaternion quatA, List<Vec3> worldVertsB1, double minDist, double maxDist, List<ConvexPolyhedronContactPoint> result,)

Clip a face against a hull. @param worldVertsB1 An array of Vec3 with vertices in the world frame. @param minDist Distance clamping @param Array result Array to store resulting contact points in. Will be objects with properties: point, depth, normal. These are represented in world coordinates.

Implementation

void clipFaceAgainstHull(
  Vec3 separatingNormal,
  Vec3 posA,
  Quaternion quatA,
  List<Vec3> worldVertsB1,
  double minDist,
  double maxDist,
  final List<ConvexPolyhedronContactPoint> result
) {
  final faceANormalWS = Vec3();
  final edge0 = Vec3();
  final worldEdge0 = Vec3();
  final worldPlaneAnormal1 = Vec3();
  final planeNormalWS1 = Vec3();
  final worldA1 = Vec3();
  final localPlaneNormal = Vec3();
  final planeNormalWS = Vec3();
  final hullA = this;
  List<Vec3> worldVertsB2 = [];
  final pVtxIn = worldVertsB1;
  final pVtxOut = worldVertsB2;

  int closestFaceA = -1;
  double dmin = double.infinity;

  // Find the face with normal closest to the separating axis
  for (int face = 0; face < hullA.faces.length; face++) {
    faceANormalWS.copy(hullA.faceNormals[face]!);
    quatA.vmult(faceANormalWS, faceANormalWS);
    final double d = faceANormalWS.dot(separatingNormal);
    if (d < dmin) {
      dmin = d;
      closestFaceA = face;
    }
  }
  if (closestFaceA < 0) {
    return;
  }

  // Get the face and construct connected faces
  final Polygon polyA = Polygon(hullA.faces[closestFaceA],[]);// as number[] & { connectedFaces: number[] }
  polyA.connectedFaces = [];
  for (int i = 0; i < hullA.faces.length; i++) {
    int len = hullA.faces[i].length;
    for (int j = 0; j < len; j++) {
      if (
        /* Sharing a vertex*/
        polyA.contains(hullA.faces[i][j]) &&
        /* Not the one we are looking for connections from */
        i != closestFaceA &&
        /* Not already added */
        !polyA.connectedFaces.contains(i)
      ) {
        polyA.connectedFaces.add(i);
      }
    }
  }

  // Clip the polygon to the back of the planes of all faces of hull A,
  // that are adjacent to the witness face
  final int numVerticesA = polyA.length;
  for (int i = 0; i < numVerticesA; i++) {
    final a = hullA.vertices[polyA[i]];
    final b = hullA.vertices[polyA[(i + 1) % numVerticesA]];
    a.vsub(b, edge0);
    worldEdge0.copy(edge0);
    quatA.vmult(worldEdge0, worldEdge0);
    posA.vadd(worldEdge0, worldEdge0);
    worldPlaneAnormal1.copy(faceNormals[closestFaceA]!);
    quatA.vmult(worldPlaneAnormal1, worldPlaneAnormal1);
    posA.vadd(worldPlaneAnormal1, worldPlaneAnormal1);
    worldEdge0.cross(worldPlaneAnormal1, planeNormalWS1);
    planeNormalWS1.negate(planeNormalWS1);
    worldA1.copy(a);
    quatA.vmult(worldA1, worldA1);
    posA.vadd(worldA1, worldA1);

    final otherFace = polyA.connectedFaces[i];
    localPlaneNormal.copy(faceNormals[otherFace]!);
    final localPlaneEq = getPlaneConstantOfFace(otherFace);
    planeNormalWS.copy(localPlaneNormal);
    quatA.vmult(planeNormalWS, planeNormalWS);
    double planeEqWS = localPlaneEq - planeNormalWS.dot(posA);

    // Clip face against our constructed plane
    clipFaceAgainstPlane(pVtxIn, pVtxOut, planeNormalWS, planeEqWS);

    // Throw away all clipped points, but save the remaining until next clip
    while (pVtxIn.isNotEmpty) {
      pVtxIn.removeAt(0);
    }
    while(pVtxOut.isNotEmpty) {
      pVtxIn.add(pVtxOut.removeAt(0));
    }
  }

  // only keep contact points that are behind the witness face
  localPlaneNormal.copy(faceNormals[closestFaceA]!);

  final localPlaneEq = getPlaneConstantOfFace(closestFaceA);
  planeNormalWS.copy(localPlaneNormal);
  quatA.vmult(planeNormalWS, planeNormalWS);

  final planeEqWS = localPlaneEq - planeNormalWS.dot(posA);
  for (int i = 0; i < pVtxIn.length; i++) {
    double depth = planeNormalWS.dot(pVtxIn[i]) + planeEqWS; // ???

    if (depth <= minDist) {
      logger?.verbose('clamped: depth=$depth to minDist=$minDist');
      depth = minDist;
    }

    if (depth <= maxDist) {
      final point = pVtxIn[i];
      if (depth <= 1e-6) {
        final p = ConvexPolyhedronContactPoint(
          point,
          planeNormalWS,
          depth,
        );
        result.add(p);
      }
    }
  }
}