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