void Boid::cohesion(deque<Boid*>& neighbours) {
Vector2D vForce;
float neighborCount = 0;
Vector2D vCenterOfMass;
deque<Boid*>::iterator itBoid;
for(itBoid = neighbours.begin(); itBoid != neighbours.end(); ++itBoid) {
if(*itBoid == this) continue;
vCenterOfMass.add((*itBoid)->getVelocity());
++neighborCount;
}
if(neighborCount > 0) {
vCenterOfMass.devide(neighborCount);
if(vCenterOfMass.distanceToSQ(getVelocity()) > 5.0f * 5.0f) {
Vector2D vel = vCenterOfMass;
Vector2D pos = position;
pos.normalize();
vel.subtract(pos);
vel.subtract(getVelocity());
vel.scale(0.1);
velocity.add(vel);
}
}
}
double PathToTarget(Vector2D * targ)
{
bool isLeft = false;
double ang = AngleToTarget(targ);
if (ang > 0) isLeft = true; // else false
double theta;
if (isLeft)
{
theta = position->getZ() + PI / 2.0;
}
else
{
theta = position->getZ() - PI / 2.0;
}
Vector2D * P = new Vector2D(position->getX(), position->getY());
Vector2D * n = new Vector2D(r * cos(theta), r * sin(theta));
// R = targ - (P + n)
Vector2D * G = P->add(n);
Vector2D * R = targ->extract(G);
Vector2D * ni = n->inv();
double beta = R->angleTo(n->inv());
double beta1 = ni->anticlockAngle(R);
if (!isLeft) beta1 *= -1;
double e = acos(r / R->norm());
if (beta1 < 0)
beta = 2 * PI - beta;
/*
if (beta1 > 0 && !isLeft)
beta = 2 * PI - beta;
*/
double eps = beta - e;
double L = r * eps;
double Rn = R->norm();
double T = sqrt(Rn * Rn + r * r);
return L + T;
}
Vector2D Physics2D::ReferenceFrame::position() const
{
if(bodies.size() == 0)
return customPosition;
else if(bodies.size() == 1) //optimization for 1 body
return bodies[0]->position;
else
{
Vector2D centerOfMass;
double totalMass = 0;
for(unsigned i = 0; i < bodies.size(); i++)
{
const Body2D& b = *(bodies[i]);
centerOfMass.add(b.position.times(b.mass));
totalMass += b.mass;
}
if(totalMass != 0) // this is for safety: obviously a total mass of 0 is something wrong...
centerOfMass.scale(1.0/totalMass);
return centerOfMass;
}
}
Vector2D Physics2D::ReferenceFrame::velocity() const
{
if(bodies.size() == 0)
return customVelocity;
else if(bodies.size() == 1) //optimization for 1 body
return bodies[0]->velocity;
else
{
Vector2D totalVelocity;
double totalMass = 0;
for(unsigned i = 0; i < bodies.size(); i++)
{
const Body2D& b = *(bodies[i]);
totalVelocity.add(b.velocity.times(b.mass));
totalMass += b.mass;
}
if(totalMass != 0) // this is for safety: obviously a total mass of 0 is something wrong...
totalVelocity.scale(1.0/totalMass);
return totalVelocity;
}
}
void MultiDirectionalEar::renderFeedback(Object *object, Environment *environment) {
if (object == environment->getPlayer()) {
Vector2D windowPos = environment->getWindowPos(object->getPosition());
for (int i = 0; i < nearestObjects.size(); i++) {
Object *nearestObject = nearestObjects[i];
if (nearestObject->getVoice() && nearestObject->getVoiceInterval() <= 0.0) {
nearestObject->resetVoiceInterval();
Vector2D objectWindowPos = environment->getWindowPos(nearestObject->getPosition());
Vector2D relativePos = objectWindowPos.add(windowPos.mult(-1.0));
relativePos = relativePos.normalize();
double pan = relativePos.getX();
double nearestObjectDistance = windowPos.distanceTo(objectWindowPos);
double gain = 1.0 - (nearestObjectDistance / 1000.0);
if (gain < 0.0) {
gain = 0.0;
}
al_play_sample(nearestObject->getVoice(), gain, pan, 1.0, ALLEGRO_PLAYMODE_ONCE, NULL);
}
}
}
}
