spherePlane method

bool spherePlane(
  1. Sphere si,
  2. Plane sj,
  3. Vec3 xi,
  4. Vec3 xj,
  5. Quaternion qi,
  6. Quaternion qj,
  7. Body bi,
  8. Body bj, [
  9. Shape? rsi,
  10. Shape? rsj,
  11. bool justTest = false,
])

Implementation

bool spherePlane(
  Sphere si,
  Plane sj,
  Vec3 xi,
  Vec3 xj,
  Quaternion qi,
  Quaternion qj,
  Body bi,
  Body bj,
  [
    Shape? rsi,
    Shape? rsj,
    bool justTest = false
]){
  // We will have one contact in this case
  final r = createContactEquation(bi, bj, si, sj, rsi, rsj);

  // Contact normal
  r.ni.set(0, 0, 1);
  qj.vmult(r.ni, r.ni);
  r.ni.negate(r.ni); // body i is the sphere, flip normal
  r.ni.normalize(); // Needed?

  // Vector from sphere center to contact point
  r.ni.scale(si.radius, r.ri);

  // Project down sphere on plane
  xi.vsub(xj, _pointOnPlaneToSphere);
  r.ni.scale(r.ni.dot(_pointOnPlaneToSphere), _planeToSphereOrtho);
  _pointOnPlaneToSphere.vsub(_planeToSphereOrtho, r.rj); // The sphere position projected to plane

  if (-_pointOnPlaneToSphere.dot(r.ni) <= si.radius) {
    if (justTest) {
      return true;
    }

    // Make it relative to the body
    final ri = r.ri;
    final rj = r.rj;
    ri.vadd(xi, ri);
    ri.vsub(bi.position, ri);
    rj.vadd(xj, rj);
    rj.vsub(bj.position, rj);

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

  return false;
}