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; } } }