float ofAngle(const ofVec2f & vec) {
float ang = vec.angle(ofVec2f(0, -1));
ang -= 180;
ang *= -1;
return ang;
}
void CageBehavior::caging(vector<RobotState>* robots) {
int n = robots->size();
// update diameter extremities
if (robots->at(1).getTouch()) {
ofVec2f pos = robots->at(1).getPosition();
//ofVec2f goal = robots->at(1).getGoal();
robots->at(1).setGoal(pos);
}
ofVec2f p1 = robots->at(1).getGoal();
ofVec2f goal = ofVec2f(o.x, o.y - cagecenter);
ofVec2f vec = goal - p1;
float orient = vec.angle(ofVec2f(1, 0));
float mag = p1.distance(goal);
o = ofVec2f(p1.x, p1.y+cagecenter);
ofVec2f left = ofVec2f(p1.x - d / 2, p1.y + cagecenter);
ofVec2f right = ofVec2f(p1.x + d / 2, p1.y + cagecenter);
ofVec2f axis = o + (right - o).rotate(-orient);
int start = 2;
start = checksection(orient);
if ((abs(preaxis.angle(axis)) >= 5))
{
pho = pho + 2;
if (pho >= 180)
{
pho = 180;
}
for (int i = 0; i<nmid; i++) {
//
if (i < nmid)
{
int k = i;
float a = pho * (nmid / 2 - 1 - k + 0.5) / (float)(nmid / 2);
ofVec2f p = o + (preaxis - o).rotate(a);
if ((prestart + i) <= 9)
{
robots->at(prestart + i).setGoal(p);
robots->at(prestart + i).setColor(ofColor(0, 0, 255));
}
else
{
robots->at(prestart + i - 8).setGoal(p);
robots->at(prestart + i - 8).setColor(ofColor(0, 0, 255));
}
}
else
{
int k = i - (nmid / 2);
float a = pho * (nmid / 2 - 1 - k + 0.5) / (float)(nmid / 2);
ofVec2f p_ = o + (preaxis - o).rotate(-a);
if ((start + i) <= 9)
{
robots->at(start + i).setGoal(p_);
robots->at(start + i).setColor(ofColor(0, 0, 255));
}
else
{
robots->at(start + i - 8).setGoal(p_);
robots->at(start + i - 8).setColor(ofColor(0, 0, 255));
}
}
}
}
else {
if (mag > 0)
{
pho = pho - 2;
if (pho <= 130)
{
pho = 130;
}
}
else
{
pho = pho + 2;
if (pho >= 180)
{
pho = 180;
}
}
for (int i = 0; i<nmid; i++) {
//
if (i < nmid)
{
int k = i;
float a = pho * (nmid / 2 - 1 - k + 0.5) / (float)(nmid / 2);
ofVec2f p = o + (axis - o).rotate(a);
if ((start + i) <= 9)
{
robots->at(start + i).setGoal(p);
robots->at(start + i).setColor(ofColor(0, 0, 255));
}
else
{
robots->at(start + i - 8).setGoal(p);
robots->at(start + i - 8).setColor(ofColor(0, 0, 255));
}
}
else
{
int k = i - (nmid / 2);
float a = pho * (nmid / 2 - 1 - k + 0.5) / (float)(nmid / 2);
ofVec2f p_ = o + (axis - o).rotate(-a);
if ((start + i) <= 9)
{
robots->at(start + i).setGoal(p_);
robots->at(start + i).setColor(ofColor(0, 0, 255));
}
else
{
robots->at(start + i - 8).setGoal(p_);
robots->at(start + i - 8).setColor(ofColor(0, 0, 255));
}
}
}
}
// put remaining robots aside
discard(robots, nmid + 2, n-1);
// translate
int translating = 0;
for (int i=2; i < 2+nmid; i++)
if (robots->at(i).getTouch())
translating = i;
if (translating > 0) {
ofVec2f goal = robots->at(translating).getGoal();
ofVec2f pos = robots->at(translating).getPosition();
ofVec2f delta = pos - goal;
for (int i=1; i < 2+nmid; i++) {
ofVec2f goali = robots->at(i).getGoal();
robots->at(i).setGoal(goali + delta);
}
}
preaxis = axis;
prestart = start;
}