particleConvex method

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

Implementation

bool particleConvex(
  ConvexPolyhedron sj,
  Particle si,
  Vector3 xj,
  Vector3 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(0,0,0,1);
  local.setFrom(xi);
  local.sub2(xj, local); // Convert position to relative the convex origin
  cqj..setFrom(qj)..conjugate();
  cqj.vmult(local, local);
  if (sj.pointIsInside(local)) {
    if (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 = Vector3.zero();
      // Check how much the particle penetrates the polygon plane.
      xi.sub2(verts, convexParticleVertexToParticle);
      final penetration = -normal.dot(convexParticleVertexToParticle);
      if (minPenetration == null || penetration.abs() < minPenetration.abs()) {
        if (justTest) {
          return true;
        }

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

    if (penetratedFaceIndex != -1) {
      // Setup contact
      final r = createContactEquation(bi, bj, si, sj, rsi, rsj);
      penetratedFaceNormal.scale2(minPenetration!, worldPenetrationVec);
      // rj is the particle position projected to the face
      worldPenetrationVec.add2(xi, worldPenetrationVec);
      worldPenetrationVec.sub2(xj, worldPenetrationVec);
      r.rj.setFrom(worldPenetrationVec);
      //final projectedToFace = xi..sub(xj)..add(worldPenetrationVec);
      //projectedToFace.setFrom(r.rj);

      //qj.vmult(r.rj,r.rj);
      r.ni..setFrom(penetratedFaceNormal)..negate(); // Contact normal
      r.ri.setValues(0, 0, 0); // Center of particle

      // Make relative to bodies
      r.ri.add2(xi, r.ri);
      r.ri.sub2(bi.position, r.ri);
      r.rj.add2(xj, r.rj);
      r.rj.sub2(bj.position, r.rj);

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