void Coordinator::TestAll() {
cout << "Testing Point2D...";
Point2D p = Point2D(03235, 23281, 2372);
Point2D q = Point2D(p.toString());
assert(p.x == q.x && p.y == q.y && p.id == q.id);
q = Point2D(03235, 23281, "2372");
assert(p.x == q.x && p.y == q.y && p.id == q.id);
cout << "ok" << endl;
// cout << "Testing UUO" << endl;
// Uuo u = new Uuo();
cout << "Testing BehaviorOrder...";
BehaviorOrder bo = BehaviorOrder("foobar");
BehaviorOrder bo2 = BehaviorOrder("nadda");
bo2.fromString(bo.toString());
assert(bo2.toString() == bo.toString());
cout << "ok" << endl;
cout << "Testing WaypointOrder...";
WaypointOrder wo = WaypointOrder(Point2D(32, 23, 101));
WaypointOrder wo2 = WaypointOrder(Point2D(-1, -1, -1));
wo2.fromString(wo.toString());
assert(wo.toString() == wo2.toString());
cout << "ok" << endl;
cout << "Testing MarkerMap...";
MarkerMap m = MarkerMap();
//assert(m.toString() == "");
Uuo a = Uuo(3, 38.83, 1, 0.238);
Uuo b = Uuo(2, -28.28, 328.2, 0.0001);
Uuo c = Uuo(-13823, 0, 0, 0.00002);
Uuo d = Uuo(23811, 32.282, 382, 32.0);
m._map.insert(pair<int,Uuo>(3, a));
m._map.insert(pair<int,Uuo>(2, b));
m._map.insert(pair<int,Uuo>(-13823, c));
m._map.insert(pair<int,Uuo>(3282, d));
m._map.insert(pair<int,Uuo>(0, Uuo(328, 23, 2383, 1.0)));
MarkerMap m2 = MarkerMap(m.toString());
// cout << m.toString() << endl;
// cout << "A: " << m2._map[3].toString() << endl; cout << "B: " << a.toString() << endl;
assert(m2._map[3].toString() == a.toString());
assert(m2._map[2].toString() == b.toString());
assert(m2._map[-13823].toString() == c.toString());
// should !=, because ID of Uuo didn't match inserted ID
assert(m2._map[3282].toString() != d.toString());
cout << "ok" << endl;
cout << "Testing MarkerMap PriorityQueue...";
MarkerMap m3 = MarkerMap("id=1,x=-10,y=-88,pH=0.0261963,cc=0,mh=.o:id=10,x=-88,y=-79,pH=0.0261963,cc=0,mh=.o:id=20,x=33,y=-145,pH=0.1,cc=1,mh=.:id=31,x=187,y=-104,pH=0.0261963,cc=0,mh=.o:id=38,x=362,y=-77,pH=0.0261963,cc=0,mh=.o:id=76,x=96,y=-116,pH=0.132275,cc=0,mh=.:id=84,x=-40,y=-81,pH=0.0261963,cc=0,mh=.o:id=95,x=383,y=-87,pH=0.463468,cc=0,mh=.x:id=144,x=338,y=-143,pH=0.0261963,cc=0,mh=.o:id=202,x=143,y=-122,pH=0.132275,cc=1,mh=.:id=204,x=282,y=-104,pH=0.0261963,cc=0,mh=.o:id=232,x=22,y=-86,pH=0.0261963,cc=0,mh=.o:id=245,x=382,y=-132,pH=0.0261963,cc=0,mh=.o:id=292,x=253,y=-148,pH=0.0261963,cc=0,mh=.o:id=322,x=70,y=-98,pH=0.463468,cc=0,mh=.x:id=362,x=287,y=-130,pH=0.132275,cc=1,mh=.:id=398,x=253,y=-76,pH=0.0261963,cc=0,mh=.o:id=424,x=341,y=-91,pH=0.132275,cc=1,mh=.:id=426,x=-51,y=-85,pH=0.132275,cc=1,mh=.:id=429,x=58,y=-140,pH=0.132275,cc=1,mh=.:id=449,x=327,y=-128,pH=0.132275,cc=1,mh=.:id=505,x=391,y=-155,pH=0.132275,cc=1,mh=.:id=510,x=52,y=-121,pH=0.132275,cc=1,mh=.:id=539,x=346,y=-106,pH=0.132275,cc=1,mh=.:id=553,x=212,y=-88,pH=0.132275,cc=1,mh=.:id=558,x=293,y=-139,pH=0.132275,cc=1,mh=.:id=575,x=362,y=-87,pH=0.132275,cc=1,mh=.:id=625,x=183,y=-123,pH=0.132275,cc=1,mh=.:id=626,x=392,y=-96,pH=0.132275,cc=1,mh=.:id=643,x=-131,y=-102,pH=0.0261963,cc=0,mh=.o:id=661,x=227,y=-109,pH=0.132275,cc=1,mh=.:id=672,x=-132,y=-93,pH=0.0261963,cc=0,mh=.o:id=684,x=389,y=-90,pH=0.132275,cc=1,mh=.:id=685,x=59,y=-138,pH=0.132275,cc=1,mh=.:id=727,x=-23,y=-92,pH=0.132275,cc=1,mh=.:id=738,x=-144,y=-84,pH=0.132275,cc=1,mh=.:id=744,x=277,y=-110,pH=0.132275,cc=1,mh=.:id=755,x=280,y=-81,pH=0.132275,cc=1,mh=.:id=767,x=329,y=-80,pH=0.132275,cc=1,mh=.:id=778,x=-135,y=-92,pH=0.0261963,cc=0,mh=.o:id=787,x=301,y=-132,pH=0.132275,cc=1,mh=.:id=794,x=40,y=-84,pH=0.132275,cc=1,mh=.:id=829,x=391,y=-93,pH=0.132275,cc=1,mh=.:id=836,x=302,y=-113,pH=0.132275,cc=1,mh=.:id=847,x=252,y=-90,pH=0.132275,cc=1,mh=.:id=850,x=-108,y=-88,pH=0.132275,cc=1,mh=.:id=918,x=245,y=-146,pH=0.132275,cc=1,mh=.:id=929,x=321,y=-102,pH=0.132275,cc=1,mh=.:id=930,x=92,y=-105,pH=0.132275,cc=1,mh=.:id=968,x=276,y=-75,pH=0.132275,cc=1,mh=.:id=970,x=42,y=-123,pH=0.132275,cc=1,mh=.:id=8888,x=0,y=-20,pH=0.0261963,cc=0,mh=.o");
priority_queue<PriorityNode> pq = m3.getPriorityNodes(slavePose, slavePose.id);
cout << "pq.size(): " << pq.size() << endl;
PriorityNode pn1 = pq.top();
cout << "ok" << endl;
cout << "Testing TSP...";
vector<WaypointOrder> wps;
wps.push_back(WaypointOrder(Point2D(10, 10)));
wps.push_back(WaypointOrder(Point2D(9, 9)));
wps.push_back(WaypointOrder(Point2D(6, 7)));
wps.push_back(WaypointOrder(Point2D(6, 6)));
wps.push_back(WaypointOrder(Point2D(3, 63)));
wps.push_back(WaypointOrder(Point2D(163, 163)));
wps.push_back(WaypointOrder(Point2D(63, 363)));
wps.push_back(WaypointOrder(Point2D(663, 663)));
// wps.push_back(WaypointOrder(Point2D(1, 1)));
// wps.push_back(WaypointOrder(Point2D(6, 99)));
// wps.push_back(WaypointOrder(Point2D(1337, 63)));
// wps.push_back(WaypointOrder(Point2D(163, 163)));
wps = WaypointOrder::optimalPath(wps, Point2D(0, 0), Point2D(30, 30));
cout << "returned min path cost " << WaypointOrder::pathLength(wps, Point2D(0, 0), Point2D(30, 30)) << endl;
for (int i = 0; i < wps.size(); i++) {
cout << wps[i].toString() << endl;
}
cout << "ok" << endl;
}
int main(int argc, char* argv[])
{
CS325Graphics window(argc, argv);
float delta = 0.1;
Point2D p1(CS325Graphics::X_MIN, CS325Graphics::Y_MAX / 4.5);
Point2D p2(CS325Graphics::X_MAX, CS325Graphics::Y_MAX / 4.5);
Point2D p3(CS325Graphics::X_MIN, CS325Graphics::Y_MIN);
Point2D p4(CS325Graphics::X_MAX, CS325Graphics::Y_MAX);
//Points 41, 42, 45, 46 control the sandbox. DON"T MESS WITH THEM!
Point3D p30(0.5, 0.5,-3.5);
Point3D p31(0.5, -0.5,-3.5);
Point3D p32(-0.5,-0.5,-3.5);
Point3D p33(-0.5, 0.5,-3.5);
Point3D p34(0.5, 0.5,-1.5);
Point3D p35(0.5, -0.5,-1.5);
Point3D p36(-0.5,-0.5,-1.5);
Point3D p37(-0.5, 0.5,-1.5);
Point3D p40( -70.8, 28.8, -50.8);
Point3D p41( 50.8,-2.8, 50.8);
Point3D p42(-50.8,-2.8, 50.8);
Point3D p43(-58.8, 25.8, 50.8);
Point3D p44( 50.8, 50.8, -50.8);
Point3D p45( 50.8,-2.8, -50.8);
Point3D p46(-50.8,-2.8, -50.8);
Point3D p47(-84.8,-2.8, -50.8);
Point3D p49(-8.5,22.0, 50.8);
Point3D p48(70,20,50.8);
Point3D p50(3.5, 0.5,-3.5);
Point3D p51(3.5, -0.5,-3.5);
Point3D p52(2.5,-0.5,-3.5);
Point3D p53(2.5, 0.5,-3.5);
Point3D p54(3.5, 0.5,-1.5);
Point3D p55(3.5, -0.5,-1.5);
Point3D p56(2.5,-0.5,-1.5);
Point3D p57(2.5, 0.5,-1.5);
Point3D p60(3.5, 0.5, 13.5);
Point3D p61(3.5, -0.5, 13.5);
Point3D p62(2.5,-0.5, 13.5);
Point3D p63(2.5, 0.5, 13.5);
Point3D p64(3.5, 0.5, 16.5);
Point3D p65(3.5, -0.5, 16.5);
Point3D p66(2.5,-0.5, 16.5);
Point3D p67(2.5, 0.5, 16.5);
Point2D viewPos;
Vector2D viewDir;
Vector3D deltaV;
viewDir.setAngle(0);
// move view position
for(int i = 0; i < MOVE_TEST; i){
/*window.DrawLineOnScreen(p1, p2);*/
//window.DrawLineOnScreen(p4, p3);
window.DrawLineInSpace(p30, p31);
window.DrawLineInSpace(p31, p32);
window.DrawLineInSpace(p32, p33);
window.DrawLineInSpace(p33, p30);
window.DrawLineInSpace(p34, p35);
window.DrawLineInSpace(p35, p36);
window.DrawLineInSpace(p36, p37);
window.DrawLineInSpace(p37, p34);
window.DrawLineInSpace(p30, p34);
window.DrawLineInSpace(p31, p35);
window.DrawLineInSpace(p32, p36);
window.DrawLineInSpace(p33, p37);
window.DrawLineInSpace(p50, p51);
window.DrawLineInSpace(p51, p52);
window.DrawLineInSpace(p52, p53);
window.DrawLineInSpace(p53, p50);
window.DrawLineInSpace(p54, p55);
window.DrawLineInSpace(p55, p56);
window.DrawLineInSpace(p56, p57);
window.DrawLineInSpace(p57, p54);
window.DrawLineInSpace(p50, p54);
window.DrawLineInSpace(p51, p55);
window.DrawLineInSpace(p52, p56);
window.DrawLineInSpace(p53, p57);
window.DrawLineInSpace(p60, p61);
window.DrawLineInSpace(p61, p62);
window.DrawLineInSpace(p62, p63);
window.DrawLineInSpace(p63, p60);
window.DrawLineInSpace(p64, p65);
window.DrawLineInSpace(p65, p66);
window.DrawLineInSpace(p66, p67);
window.DrawLineInSpace(p67, p64);
window.DrawLineInSpace(p60, p64);
window.DrawLineInSpace(p61, p65);
window.DrawLineInSpace(p62, p66);
window.DrawLineInSpace(p63, p67);
//window.DrawLineInSpace(p40, p41);
window.DrawLineInSpace(p41, p42);
window.DrawLineInSpace(p42, p43);
//window.DrawLineInSpace(p43, p40);
//window.DrawLineInSpace(p44, p45);
window.DrawLineInSpace(p45, p46);
//window.DrawLineInSpace(p46, p47);
//window.DrawLineInSpace(p47, p44);
window.DrawLineInSpace(p40, p45);
window.DrawLineInSpace(p41, p45);
window.DrawLineInSpace(p42, p46);
window.DrawLineInSpace(p43, p47);
window.DrawLineInSpace(p40, p47);
window.DrawLineInSpace(p41, p49);
window.DrawLineInSpace(p42, p49);
window.DrawLineInSpace(p41, p48);
window.DrawLineInSpace(p45, p48);
if(GetAsyncKeyState(VK_DOWN)) // the DOWN arrow was pressed, let's do something
{
delta = -.1;
viewPos.setY(viewPos.getY() + cos(-viewDir.getAngle())*delta);
viewPos.setX(viewPos.getX() + sin(-viewDir.getAngle())*delta);
window.SetViewPosition(viewPos);
cout << "view pos: " << viewPos.toString() << endl;
window.DisplayNow();
Sleep(50);
}
if(GetAsyncKeyState(VK_UP)) // the UP arrow was pressed, let's do something
{
delta = .1;
viewPos.setY(viewPos.getY() + cos(-viewDir.getAngle())*delta);
viewPos.setX(viewPos.getX() + sin(-viewDir.getAngle())*delta);
window.SetViewPosition(viewPos);
cout << "view pos: " << viewPos.toString() << endl;
window.DisplayNow();
Sleep(50);
}
if(GetAsyncKeyState(VK_RIGHT)) // the RIGHT arrow was pressed, let's do something
{
delta = .1;
viewDir.setAngle(viewDir.getAngle()+delta);
window.SetViewDirection(viewDir);
cout << "view dir: " << viewDir.getAngle() << endl;
window.DisplayNow();
Sleep(50);
}
if(GetAsyncKeyState(VK_LEFT)) // the LEFT arrow was pressed, let's do something
{
delta = -.1;
viewDir.setAngle(viewDir.getAngle()+delta);
window.SetViewDirection(viewDir);
cout << "view dir: " << viewDir.getAngle() << endl;
window.DisplayNow();
Sleep(50);
}
if(GetAsyncKeyState(VK_ESCAPE))
{
return 1;
}
}
}
