public void reconstruct(final Vector3 top, final Vector3 bottom, final double radius) {
// our temp vars
final Vector3 localTranslation = Vector3.fetchTempInstance();
final Vector3 capsuleUp = Vector3.fetchTempInstance();
// first make the capsule the right shape
height = top.distance(bottom);
this.radius = radius;
setGeometryData();
// now orient it in space.
localTranslation.set(_localTransform.getTranslation());
top.add(bottom, localTranslation).multiplyLocal(.5);
// rotation that takes us from 0,1,0 to the unit vector described by top/center.
top.subtract(localTranslation, capsuleUp).normalizeLocal();
final Matrix3 rotation = Matrix3.fetchTempInstance();
rotation.fromStartEndLocal(Vector3.UNIT_Y, capsuleUp);
_localTransform.setRotation(rotation);
Vector3.releaseTempInstance(localTranslation);
Vector3.releaseTempInstance(capsuleUp);
Matrix3.releaseTempInstance(rotation);
updateWorldTransform(false);
}
@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);
}