/**
* 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);
}