convexParticle method

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

Implementation

bool convexParticle(
  ConvexPolyhedron sj,
  Particle si,
  Vec3 xj,
  Vec3 xi,
  Quaternion qj,
  Quaternion qi,
  Body bj,
  Body bi,
  [
    Shape? rsi,
    Shape? rsj,
    bool justTest = false
]){
  int penetratedFaceIndex = -1;
  final penetratedFaceNormal = _convexParticlePenetratedFaceNormal;
  final worldPenetrationVec = _convexParticleWorldPenetrationVec;
  double? minPenetration;
  //int numDetectedFaces = 0;

  // Convert particle position xi to local coords in the convex
  final local = _convexParticleLocal;
  final cqj = Quaternion();
  local.copy(xi);
  local.vsub(xj, local); // Convert position to relative the convex origin
  qj.conjugate(cqj);
  cqj.vmult(local, local);

  if (sj.pointIsInside(local)) {
    //if (true|| sj.worldVerticesNeedsUpdate) {
      sj.computeWorldVertices(xj, qj);
    //}
    if (sj.worldFaceNormalsNeedsUpdate) {
      sj.computeWorldFaceNormals(qj);
    }

    // For each world polygon in the polyhedra
    for (int i = 0, nfaces = sj.faces.length; i != nfaces; i++) {
      // Construct world face vertices
      final verts = sj.worldVertices[sj.faces[i][0]];
      final normal = sj.worldFaceNormals[i];
      final convexParticleVertexToParticle = Vec3();
      // Check how much the particle penetrates the polygon plane.
      xi.vsub(verts, convexParticleVertexToParticle);
      final penetration = -normal.dot(convexParticleVertexToParticle);
      if (minPenetration == null || penetration.abs() < minPenetration.abs()) {
        if (justTest) {
          return true;
        }

        minPenetration = penetration;
        penetratedFaceIndex = i;
        penetratedFaceNormal.copy(normal);
        //numDetectedFaces++;
      }
    }

    if (penetratedFaceIndex != -1) {
      // Setup contact
      var r = createContactEquation(bi, bj, si, sj, rsi, rsj);
      penetratedFaceNormal.scale(minPenetration!, worldPenetrationVec);
      // rj is the particle position projected to the face
      worldPenetrationVec.vadd(xi, worldPenetrationVec);
      worldPenetrationVec.vsub(xj, worldPenetrationVec);
      r.rj.copy(worldPenetrationVec);
      //final projectedToFace = xi.vsub(xj).vadd(worldPenetrationVec);
      //projectedToFace.copy(r.rj);

      //qj.vmult(r.rj,r.rj);
      penetratedFaceNormal.negate(r.ni); // Contact normal
      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.vadd(xj, r.rj);
      r.rj.vsub(bj.position, r.rj);

      result.add(r);
      createFrictionEquationsFromContact(r, frictionResult);
    }
    else {
      //print('Point found inside convex, but did not find penetrating face!');
    }
  }
  return false;
}