/**
* Given a pointer to an entity and a std container of pointers to nearby entities, this function
* checks to see if there is an overlap between entities. If there is, then the entities are moved
* away from each other
*/
public static <T extends BaseGameEntity, conT extends List<? extends BaseGameEntity>>
void EnforceNonPenetrationContraint(T entity, final conT others) {
ListIterator<? extends BaseGameEntity> it = others.listIterator();
// iterate through all entities checking for any overlap of bounding
// radii
while (it.hasNext()) {
BaseGameEntity curOb = it.next();
// make sure we don't check against this entity
if (curOb == entity) {
continue;
}
// calculate the distance between the positions of the entities
Vector2D ToEntity = sub(entity.Pos(), curOb.Pos());
double DistFromEachOther = ToEntity.Length();
// if this distance is smaller than the sum of their radii then this
// entity must be moved away in the direction parallel to the
// ToEntity vector
double AmountOfOverLap = curOb.BRadius() + entity.BRadius() - DistFromEachOther;
if (AmountOfOverLap >= 0) {
// move the entity a distance away equivalent to the amount of overlap.
entity.SetPos(add(entity.Pos(), mul(div(ToEntity, DistFromEachOther), AmountOfOverLap)));
}
} // next entity
}
