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);
}
}
}
Position Knowledge::AdjustKickPoint(Vector2D ballPos, Vector2D target, int kickSpeed)
{
Position p;
Vector2D dir = (ballPos - target).normalizedVector();
dir.scale(ROBOT_RADIUS - (35 - kickSpeed));
p.loc = ballPos + dir;
p.dir = (-dir).dir().radian();
return p;
}
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;
}
}
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;
}
}
