/*
d : a=b
red a: a=c
red b: b=d
reduc d: c=d
*/
DEM reduc (DEM ab, int red_arg)
{
DEM ac, bd, cb, cd;
ac = red1 (left(ab), red_arg);
bd = red1 (right(ab), red_arg);
cb = transym1 (ac, ab);
cd = trans (cb, bd);
return cd;
}
DEM red3 (DEM x, int red_arg)
{
DEM t, t1, t2, t3, t4, t5;
int i;
trace_dem ("red3", x);
for (i=0; i<nnr; i++)
{
trace_dem ("", no_red[i]);
trace_dem ("", x);
if (no_red[i] == x)
return x;
}
if (used != NULL && x == left(used))
return used;
if ((flags & FLAG_RED_LAMBDA) && node(x) == node_lambda)
{
t1 = red1 (sd0(x), red_arg);
trace_dem ("DBLambda", t1);
t2 = lambda (t1);
trace_dem ("DBLambda", t2);
return t2;
}
if (atom(x))
return x;
if (node(fnc(x)) == node_lambda)
{
return mksubst (subdem(0,fnc(x)), arg(x));
}
if (used == NULL && atom (fnc(x)))
{
t1 = fnc(x);
if (red_arg)
t2 = red1 (arg(x), red_arg);
else
t2 = arg(x);
t = ap (t1, t2);
return t;
}
t1 = red1 (fnc(x), red_arg);
if (red_arg)
t2 = red1 (arg(x), red_arg);
else
t2 = arg(x);
t3 = ap (t1, t2);
if (left(t3) == right(t3))
return t3;
t4 = red1 (right(t3), red_arg);
if (left(t4) == right(t4))
return t3;
t = trans (t3, t4);
return t;
}
int Tracker::detectRedBall(Mat frame){
Mat roiFrame;
Size size = Size(0, 0);
Vec3b red1(55, 55, 200);
Point upCorner(0, 0);
int foundObjectsCount = 0;
if(pastPosition.count) {
LocationArea lp = pastPosition[0];
size.width = lp.width * 8;
size.height = lp.height * 8;
upCorner = lp.tl() - Point(size);
Point downCorner = lp.br() + Point(size);
Point limit(frame.cols, frame.rows);
correctOffBounds(&upCorner, limit);
correctOffBounds(&downCorner, limit);
roiFrame = Mat(frame, Rect(upCorner, downCorner));
}
else{
roiFrame = frame;
}
LocationArea found = findRedBall(roiFrame);// + upCorner;
found.x += upCorner.x;
found.y += upCorner.y;
pastPosition.insert(found);
return 1;
}
LocationArea Tracker::findRedBall(Mat roiFrame){
vector<Point> points;
Point average_point(-1,-1);
int sumX = 0, sumY = 0;
LocationArea ball_area;
Vec3b red1(55, 55, 200);
for(int r = 0; r < roiFrame.rows; r++){
for(int c = 0; c < roiFrame.cols; c++){
if(red1[0] > roiFrame.at<Vec3b>(r,c)[0] &&
red1[1] > roiFrame.at<Vec3b>(r,c)[1] &&
red1[2] < roiFrame.at<Vec3b>(r,c)[2]) {
points.push_back(Point(c,r));
sumX += c;
sumY += r;
}
}
}
if(!points.empty()){
int size = points.size();
average_point.x = sumX / size;
average_point.y = sumY / size;
double varianceX = 0, varianceY = 0;
int minX = average_point.x, maxX = average_point.x, minY = average_point.y, maxY = average_point.y;
for(vector<Point>::iterator i = points.begin(); i != points.end(); i++){
varianceX += abs(i->x - average_point.x);
varianceY += abs(i->y - average_point.y);
if(maxX < i->x) maxX = i->x;
if(minX > i->x) minX = i->x;
if(maxY < i->y) maxY = i->y;
if(minY > i->y) minY = i->y;//cout << *i << " variance: " << Point(varianceX, varianceY) << endl;
}
//cout << "avg" << average_point << " variance: " << Point(varianceX/size, varianceY/size) << size << endl;
ball_area.x = minX;
ball_area.y = minY;
ball_area.width = maxX - minX;
ball_area.height = maxY - minY;
}
return ball_area;
}
int main(int argc, char *argv[]){
////// start generatora liczb losowych ////
srand(time(0) + getpid()); ///
///////////////////////////////////////////
//if (argc==1 && strcmp(argv[1],"-d")==0)
log4cxx::PropertyConfigurator::configure("../config/log4cxx.properties");
//AI robotAI2(AppConfig::instance().team2[0], TEAM2);
VideoServer::instance().update();
Robot red0(AppConfig::instance().team1[0], TEAM1);
Robot red1(AppConfig::instance().team1[1], TEAM1);
Robot red2(AppConfig::instance().team1[2], TEAM1);
Robot red3(AppConfig::instance().team1[3], TEAM1);
Robot blue0(AppConfig::instance().team2[0], TEAM2);
Robot blue1(AppConfig::instance().team2[1], TEAM2);
Robot blue2(AppConfig::instance().team2[2], TEAM2);
Robot blue3(AppConfig::instance().team2[3], TEAM2);
while(true){
//cout<<"Update\n";
VideoServer::instance().update();
// VideoServer::instance().display();
red0.makeMove();
// red1.makeMove();
// red2.makeMove();
// red3.makeMove();
//
//
// blue0.makeMove();
// blue1.makeMove();
// blue2.makeMove();
// blue3.makeMove();
/*Vector2d dest(2,2);
Vector2d res;
std::cout<<"Planning start\n";
clock_t t1 = clock();
for (long i = 0; i < 1000000; i++){
RRT2 rrt2(AppConfig::instance().team1[0]);
rrt2.plan(dest, res);
}
clock_t t2 = (double) clock() - t1;
std::cout<<"Time: "<<(double)t2 / CLOCKS_PER_SEC<<std::endl;*/
}
cout<<"While is broken\n";
//
// RRTTree rrtTree;
//
// rrtTree.addNode(Vector2d(0,0), 0);
// rrtTree.addNode(Vector2d(2,3), rrtTree.getRoot());
// RRTTreeNode * tmp = rrtTree.addNode(Vector2d(1,0), rrtTree.getRoot());
// rrtTree.addNode(Vector2d(2,1), tmp);
// rrtTree.addNode(Vector2d(2,2), tmp);
//
// RRTTreeNode * node = rrtTree.findNearest(Vector2d(3,3));
// std::cout<<"Nearest: \n";
// std::cout<<node->position<<std::endl;
/*GameState gameState("../config/models.xml");
gameState.update();
gameState.display();
std::cout<<"Robot r:\n";
Robot r("omni_red0",TEAM_1, &gameState);
LOG4CXX_DEBUG(logger, "( ** START ** ) ");
LOG4CXX_DEBUG(logger, "Start time: "<< SimControl::getInstance().getSimTime());
while(true){
//update stanu gry
if (gameState.update())
// gameState.display();
r.doTest();
// else
usleep(100);
//w zaleznosci od stanu gry -> decyzje arbitra
//gol -> restart
//aut -> ustawienie pilki itp
//algorytm robota, oceniajacy stan gry i obliczajacy mozliwe posuniecia
//dla kazdego z robotow z mojej druzyny - > obliczanie ruchow
//ruch robotow przeciwnika
}*/
}
DEM read_dem ()
{
int c;
DEM f, a, b, d, x;
int i;
char buf[200];
DEM s;
DEM d1;
int flags1;
DEM used1;
extern DEM used;
loop:
do c = readchar ();
while (c==' ' || c=='\t' || c=='\n' || c==0);
switch (c)
{
case 'I': return I;
case 'K': return K;
case 'S': return S;
case 'E': return E;
case 'F': return If;
case 'O': return Ord;
case '-':
f = read_dem ();
a = read_dem ();
return ap (f, a);
case '/':
a = read_dem ();
b = read_dem ();
return transym (a, b);
case 'T':
a = read_dem ();
b = read_dem ();
return trans (a, b);
case 'X':
a = read_dem ();
return sym (a);
case '#':
a = read_dem ();
b = read_dem ();
return Axm (a, b);
case 'i':
a = read_dem ();
return defI (a);
case 'k':
a = read_dem ();
b = read_dem ();
return defK (a, b);
case 's':
a = read_dem ();
b = read_dem ();
d = read_dem ();
return defS (a, b, d);
case ')':
a = read_dem ();
b = read_dem ();
return IfNode (a, b);
case '1': return Ext1;
case '2': return Ext2;
case '3': return Ext3;
case '4': return Ext4;
case '5': return Ext5;
case '6': return Ext6;
case 'e': return AE;
case 'f': return EA0;
/*
a = read_dem ();
return EA (a);
*/
case 'm': return MP;
case 'a': return AI;
case 'b': return AK;
case 'c': return AS;
case 'r': return RPA;
case '0': return ZeroIsOrd;
case '+': return SucIsOrd;
case 'w': return LimIsOrd;
case 'p': return PredIsOrd;
case 'n': return StepIsOrd;
case 'W': return TfI;
case '<':
a = read_dem ();
return left (a);
case '>':
a = read_dem ();
return right (a);
case '\'':
a = read_dem ();
return rep(a);
case '%':
/*printf ("*1*");*/
a = read_dem ();
/*printf ("*2*");*/
trace_dem ("read", a);
/*printf ("*3*");*/
b = red (a);
/*printf ("*4*");*/
trace_dem ("red", b);
return b;
/* return red (a); */
case 'R':
a = read_dem ();
return red1 (a, 0);
case '@':
a = read_dem ();
return reduc (a, 1);
case '~':
a = read_dem ();
return reduc (a, 0);
case '$':
a = read_dem ();
return redu (a);
case 'x':
a = read_dem ();
b = read_dem ();
return ext (a, b);
case '\\':
a = read_dem ();
b = read_dem ();
trace_dem ("^(0)", a);
trace_dem ("^(1)", b);
d = exten (a, b);
trace_dem ("^(r)", d);
return d;
case ']':
a = read_dem ();
b = read_dem ();
d = dbextens (a, b);
return d;
case 'l':
a = read_dem ();
b = read_dem ();
return Ext (a, b);
/* return Lambda (a, b); */
case 'L':
a = read_dem ();
b = read_dem ();
return Lambda (a, b);
case '.':
a = read_dem ();
return DBLambda (a);
case '!':
a = read_dem ();
b = read_dem ();
return DB_lambda (a, b);
/* return DBLambda (DBname (0, a, b)); */
case '?':
a = read_dem ();
b = read_dem ();
return DB_Subst (a, b);
case '_':
a = read_dem ();
b = read_dem ();
d = read_dem ();
return Subst (a, b, d);
case ':':
a = read_dem ();
b = read_dem ();
d = read_dem ();
return ap (exten(a,d) ,b);
case 'V':
x = read_dem ();
d = read_dem ();
a = mk_dem (node(d), 0, NULL,
DB_lambda (x, subdem(0,d)),
DB_lambda (x, subdem(1,d)),
subdem(2,d) == NULL ? NULL :
DB_lambda (x, subdem(2,d)),
NULL, NULL, NULL);
return a;
case 'A':
x = read_dem ();
d = read_dem ();
a = mk_dem (node(d), 0, NULL,
ap (x, subdem(0,d)),
ap (x, subdem(1,d)),
subdem(2,d) == NULL ? NULL :
ap (x, subdem(2,d)),
NULL, NULL, NULL);
return a;
case '"':
a = read_dem ();
/* return NoRed (a); */
no_red[nnr++] = a;
return a;
case '|':
a = read_dem ();
no_red[nnr++] = a;
b = read_dem ();
return b;
case 'u':
used1 = used;
used = read_dem ();
a = read_dem ();
used = used1;
return a;
case '(':
flags1 = flags;
i = 0;
for (;;)
{
c = readchar ();
if (c == ')')
break;
buf[i++] = c;
}
buf[i] = 0;
sscanf (buf, "%x", &flags);
a = read_dem ();
if ((flags & FLAG_PERM) == 0)
flags = flags1;
return a;
case ',':
a = read_dem ();
return step (a);
case '*':
a = read_dem ();
return rstep (a);
case '`':
a = read_dem ();
return list_ap (a, nil);
case '&':
c = readchar ();
switch (c)
{
case '/': return itransym;
case 'i': return idefI;
case 'k': return idefK;
case 's': return idefS;
case '<': return ileft;
case '>': return iright;
case '=': return ieq;
case '#': return inode;
case '0': return isubdem0;
case '1': return isubdem1;
case '2': return isubdem2;
case '%': return ired;
case '$': return iredu;
case '\\': return iext;
case ',': return istep;
case '*': return irstep;
default:
fprintf (stderr, "Undefined &%c.\n",
c);
return I;
}
break;
case '[':
/* trace_dem ("read symbol", I); */
for (i=0; i<sizeof(buf); i++)
{
c = readchar();
if (c == ']')
{
buf[i] = 0;
#ifdef TRACE1
printf ("buf=<%s>\n", buf);
#endif
if (buf[0] >= '0' && buf[0] <= '9')
{
#ifdef TRACE
printf ("\nDBVar <%s>", buf);
#endif
d1 = DBVar (atoi(buf));
trace_dem ("", d);
return d1;
}
s = Sym(buf);
#ifdef TRACE1
trace_dem ("read symbol", s);
#endif
if (subdem(0,s) == NULL)
{
#ifdef TRACE1
trace_dem ("return symbol", s);
#endif
return s;
}
else
{
#ifdef TRACE
trace_dem ("return value of", s);
#endif
return subdem(0,s);
}
}
buf[i] = c;
}
fprintf (stderr, "Symbol too long\n");
return Sym(buf);
default:
return defined_dems[(unsigned char)c];
/*
printf ("Illegal character 0x%02X\n", c);
goto loop;
*/
}
}
void iniciar(int cantidadDeMensajes){
int laCantidadDeDatosPorBridge=cantidadDeDatosPorBridge(cantidadDeMensajes);
printf("laCantidadDeDatosPorBridge: %d\n", laCantidadDeDatosPorBridge);
LAN red1(1);
LAN red2(2);
LAN red3(3);
LAN red4(4);
LAN red5(5);
int dirsB1[16]={0,0,0,0, 1,1,1,1, 2,2,2,2, 2,2,2,2};
int dirsB2[16]={2,2,2,2, 2,2,2,2 ,0,0,0,0, 1,1,1,1};
Bridge B1 (1,&red1,&red2,&red5,dirsB1);
Bridge B2 (2,&red3,&red4,&red5,dirsB2);
B1.start();
B2.start();
//red1
Equipo a(1,&red1);
Equipo b(2,&red1);
Equipo c(3,&red1);
Equipo d(4,&red1);
//red2
Equipo e(5,&red2);
Equipo f(6,&red2);
Equipo g(7,&red2);
Equipo h(8,&red2);
//red3
Equipo i(9,&red3);
Equipo j(10,&red3);
Equipo k(11,&red3);
Equipo l(12,&red3);
//red4
Equipo m(13,&red4);
Equipo n(14,&red4);
Equipo o(15,&red4);
Equipo p(16,&red4);
//return;
a.start(cantidadDeMensajes);
b.start(cantidadDeMensajes);
c.start(cantidadDeMensajes);
d.start(cantidadDeMensajes);
e.start(cantidadDeMensajes);
f.start(cantidadDeMensajes);
g.start(cantidadDeMensajes);
h.start(cantidadDeMensajes);
i.start(cantidadDeMensajes);
j.start(cantidadDeMensajes);
k.start(cantidadDeMensajes);
l.start(cantidadDeMensajes);
m.start(cantidadDeMensajes);
n.start(cantidadDeMensajes);
o.start(cantidadDeMensajes);
p.start(cantidadDeMensajes);
a.wait();
b.wait();
c.wait();
d.wait();
e.wait();
f.wait();
g.wait();
h.wait();
i.wait();
j.wait();
k.wait();
l.wait();
m.wait();
n.wait();
o.wait();
p.wait();
}
DEM red (DEM x)
{
DEM r;
r = red1 (x, 1);
return r;
}
