예제 #1
0
float ofAngle(const ofVec2f & vec) {
    float ang = vec.angle(ofVec2f(0, -1));
    ang -= 180;
    ang *= -1;
    return ang;
}
예제 #2
0
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;
}