/**
   * Calculates the minimum bounding sphere of 4 points. Used in welzl's algorithm.
   *
   * @param O The 1st point inside the sphere.
   * @param A The 2nd point inside the sphere.
   * @param B The 3rd point inside the sphere.
   * @param C The 4th point inside the sphere.
   * @see #calcWelzl(java.nio.FloatBuffer)
   */
  private void setSphere(final Vector3 O, final Vector3 A, final Vector3 B, final Vector3 C) {
    final Vector3 a = A.subtract(O, null);
    final Vector3 b = B.subtract(O, null);
    final Vector3 c = C.subtract(O, null);

    final double Denominator =
        2.0
            * (a.getX() * (b.getY() * c.getZ() - c.getY() * b.getZ())
                - b.getX() * (a.getY() * c.getZ() - c.getY() * a.getZ())
                + c.getX() * (a.getY() * b.getZ() - b.getY() * a.getZ()));
    if (Denominator == 0) {
      _center.set(0, 0, 0);
      setRadius(0);
    } else {
      final Vector3 o =
          a.cross(b, null)
              .multiplyLocal(c.lengthSquared())
              .addLocal(c.cross(a, null).multiplyLocal(b.lengthSquared()))
              .addLocal(b.cross(c, null).multiplyLocal(a.lengthSquared()))
              .divideLocal(Denominator);

      setRadius(o.length() * radiusEpsilon);
      O.add(o, _center);
    }
  }
 @Override
 public void apply(final double dt, final Particle particle, final int index) {
   final Vector3 pVelocity = particle.getVelocity();
   // determine if the particle is in the inner or outer zone
   final double pDist = particle.getPosition().distanceSquared(_swarmPoint);
   final Vector3 workVect = Vector3.fetchTempInstance();
   final Vector3 workVect2 = Vector3.fetchTempInstance();
   final Matrix3 workMat = Matrix3.fetchTempInstance();
   workVect.set(_swarmPoint).subtractLocal(particle.getPosition()).normalizeLocal();
   workVect2.set(pVelocity).normalizeLocal();
   if (pDist > _swarmRangeSQ) {
     // IN THE OUTER ZONE...
     // Determine if the angle between particle velocity and a vector to
     // the swarmPoint is less than the accepted deviance
     final double angle = workVect.smallestAngleBetween(workVect2);
     if (angle < _deviance) {
       // if it is, increase the speed speedBump over time
       if (pVelocity.lengthSquared() < maxSpeedSQ) {
         final double change = _speedBump * dt;
         workVect2.multiplyLocal(change); // where workVector2 = pVelocity.normalizeLocal()
         pVelocity.addLocal(workVect2);
       }
     } else {
       final Vector3 axis = workVect2.crossLocal(workVect);
       // if it is not, shift the velocity to bring it back in line
       if ((Double.doubleToLongBits(pVelocity.lengthSquared()) & 0x1d) != 0) {
         workMat.fromAngleAxis(_turnSpeed * dt, axis);
       } else {
         workMat.fromAngleAxis(-_turnSpeed * dt, axis);
       }
       workMat.applyPost(pVelocity, pVelocity);
     }
   } else {
     final Vector3 axis = workVect2.crossLocal(workVect);
     // IN THE INNER ZONE...
     // Alter the heading based on how fast we are going
     if ((index & 0x1f) != 0) {
       workMat.fromAngleAxis(_turnSpeed * dt, axis);
     } else {
       workMat.fromAngleAxis(-_turnSpeed * dt, axis);
     }
     workMat.applyPost(pVelocity, pVelocity);
   }
   Vector3.releaseTempInstance(workVect);
   Vector3.releaseTempInstance(workVect2);
   Matrix3.releaseTempInstance(workMat);
 }
  /**
   * Calculates the minimum bounding sphere of 3 points. Used in welzl's algorithm.
   *
   * @param O The 1st point inside the sphere.
   * @param A The 2nd point inside the sphere.
   * @param B The 3rd point inside the sphere.
   * @see #calcWelzl(java.nio.FloatBuffer)
   */
  private void setSphere(final Vector3 O, final Vector3 A, final Vector3 B) {
    final Vector3 a = A.subtract(O, null);
    final Vector3 b = B.subtract(O, null);
    final Vector3 acrossB = a.cross(b, null);

    final double Denominator = 2.0 * acrossB.dot(acrossB);

    if (Denominator == 0) {
      _center.set(0, 0, 0);
      setRadius(0);
    } else {

      final Vector3 o =
          acrossB
              .cross(a, null)
              .multiplyLocal(b.lengthSquared())
              .addLocal(b.cross(acrossB, null).multiplyLocal(a.lengthSquared()))
              .divideLocal(Denominator);
      setRadius(o.length() * radiusEpsilon);
      O.add(o, _center);
    }
  }
  private BoundingVolume merge(
      final double otherRadius, final ReadOnlyVector3 otherCenter, final BoundingSphere store) {
    // check for infinite bounds... is so, return infinite bounds with center at origin
    if (Double.isInfinite(otherRadius) || Double.isInfinite(getRadius())) {
      store.setCenter(Vector3.ZERO);
      store.setRadius(Double.POSITIVE_INFINITY);
      return store;
    }

    final Vector3 diff = otherCenter.subtract(_center, _compVect1);
    final double lengthSquared = diff.lengthSquared();
    final double radiusDiff = otherRadius - getRadius();
    final double radiusDiffSqr = radiusDiff * radiusDiff;

    // if one sphere wholly contains the other
    if (radiusDiffSqr >= lengthSquared) {
      // if we contain the other
      if (radiusDiff <= 0.0) {
        store.setCenter(_center);
        store.setRadius(_radius);
        return store;
      }
      // else the other contains us
      else {
        store.setCenter(otherCenter);
        store.setRadius(otherRadius);
        return store;
      }
    }

    // distance between sphere centers
    final double length = Math.sqrt(lengthSquared);

    // init a center var using our center
    final Vector3 rCenter = _compVect2;
    rCenter.set(_center);

    // if our centers are at least a tiny amount apart from each other...
    if (length > MathUtils.EPSILON) {
      // place us between the two centers, weighted by radii
      final double coeff = (length + radiusDiff) / (2.0 * length);
      rCenter.addLocal(diff.multiplyLocal(coeff));
    }

    // set center on our resulting bounds
    store.setCenter(rCenter);

    // Set radius
    store.setRadius(0.5 * (length + getRadius() + otherRadius));
    return store;
  }
  /**
   * <code>averagePoints</code> selects the sphere center to be the average of the points and the
   * sphere radius to be the smallest value to enclose all points.
   *
   * @param points the list of points to contain.
   */
  public void averagePoints(final Vector3[] points) {
    _center.set(points[0]);

    for (int i = 1; i < points.length; i++) {
      _center.addLocal(points[i]);
    }

    final double quantity = 1.0 / points.length;
    _center.multiplyLocal(quantity);

    double maxRadiusSqr = 0;
    for (int i = 0; i < points.length; i++) {
      final Vector3 diff = points[i].subtract(_center, _compVect1);
      final double radiusSqr = diff.lengthSquared();
      if (radiusSqr > maxRadiusSqr) {
        maxRadiusSqr = radiusSqr;
      }
    }

    setRadius(Math.sqrt(maxRadiusSqr) + radiusEpsilon - 1f);
  }