Skip to content
This repository

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
tree: a810d842e6
Fetching contributors…

Octocat-spinner-32-eaf2f5

Cannot retrieve contributors at this time

file 3131 lines (2424 sloc) 94.894 kb
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170 1171 1172 1173 1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253 1254 1255 1256 1257 1258 1259 1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303 1304 1305 1306 1307 1308 1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321 1322 1323 1324 1325 1326 1327 1328 1329 1330 1331 1332 1333 1334 1335 1336 1337 1338 1339 1340 1341 1342 1343 1344 1345 1346 1347 1348 1349 1350 1351 1352 1353 1354 1355 1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370 1371 1372 1373 1374 1375 1376 1377 1378 1379 1380 1381 1382 1383 1384 1385 1386 1387 1388 1389 1390 1391 1392 1393 1394 1395 1396 1397 1398 1399 1400 1401 1402 1403 1404 1405 1406 1407 1408 1409 1410 1411 1412 1413 1414 1415 1416 1417 1418 1419 1420 1421 1422 1423 1424 1425 1426 1427 1428 1429 1430 1431 1432 1433 1434 1435 1436 1437 1438 1439 1440 1441 1442 1443 1444 1445 1446 1447 1448 1449 1450 1451 1452 1453 1454 1455 1456 1457 1458 1459 1460 1461 1462 1463 1464 1465 1466 1467 1468 1469 1470 1471 1472 1473 1474 1475 1476 1477 1478 1479 1480 1481 1482 1483 1484 1485 1486 1487 1488 1489 1490 1491 1492 1493 1494 1495 1496 1497 1498 1499 1500 1501 1502 1503 1504 1505 1506 1507 1508 1509 1510 1511 1512 1513 1514 1515 1516 1517 1518 1519 1520 1521 1522 1523 1524 1525 1526 1527 1528 1529 1530 1531 1532 1533 1534 1535 1536 1537 1538 1539 1540 1541 1542 1543 1544 1545 1546 1547 1548 1549 1550 1551 1552 1553 1554 1555 1556 1557 1558 1559 1560 1561 1562 1563 1564 1565 1566 1567 1568 1569 1570 1571 1572 1573 1574 1575 1576 1577 1578 1579 1580 1581 1582 1583 1584 1585 1586 1587 1588 1589 1590 1591 1592 1593 1594 1595 1596 1597 1598 1599 1600 1601 1602 1603 1604 1605 1606 1607 1608 1609 1610 1611 1612 1613 1614 1615 1616 1617 1618 1619 1620 1621 1622 1623 1624 1625 1626 1627 1628 1629 1630 1631 1632 1633 1634 1635 1636 1637 1638 1639 1640 1641 1642 1643 1644 1645 1646 1647 1648 1649 1650 1651 1652 1653 1654 1655 1656 1657 1658 1659 1660 1661 1662 1663 1664 1665 1666 1667 1668 1669 1670 1671 1672 1673 1674 1675 1676 1677 1678 1679 1680 1681 1682 1683 1684 1685 1686 1687 1688 1689 1690 1691 1692 1693 1694 1695 1696 1697 1698 1699 1700 1701 1702 1703 1704 1705 1706 1707 1708 1709 1710 1711 1712 1713 1714 1715 1716 1717 1718 1719 1720 1721 1722 1723 1724 1725 1726 1727 1728 1729 1730 1731 1732 1733 1734 1735 1736 1737 1738 1739 1740 1741 1742 1743 1744 1745 1746 1747 1748 1749 1750 1751 1752 1753 1754 1755 1756 1757 1758 1759 1760 1761 1762 1763 1764 1765 1766 1767 1768 1769 1770 1771 1772 1773 1774 1775 1776 1777 1778 1779 1780 1781 1782 1783 1784 1785 1786 1787 1788 1789 1790 1791 1792 1793 1794 1795 1796 1797 1798 1799 1800 1801 1802 1803 1804 1805 1806 1807 1808 1809 1810 1811 1812 1813 1814 1815 1816 1817 1818 1819 1820 1821 1822 1823 1824 1825 1826 1827 1828 1829 1830 1831 1832 1833 1834 1835 1836 1837 1838 1839 1840 1841 1842 1843 1844 1845 1846 1847 1848 1849 1850 1851 1852 1853 1854 1855 1856 1857 1858 1859 1860 1861 1862 1863 1864 1865 1866 1867 1868 1869 1870 1871 1872 1873 1874 1875 1876 1877 1878 1879 1880 1881 1882 1883 1884 1885 1886 1887 1888 1889 1890 1891 1892 1893 1894 1895 1896 1897 1898 1899 1900 1901 1902 1903 1904 1905 1906 1907 1908 1909 1910 1911 1912 1913 1914 1915 1916 1917 1918 1919 1920 1921 1922 1923 1924 1925 1926 1927 1928 1929 1930 1931 1932 1933 1934 1935 1936 1937 1938 1939 1940 1941 1942 1943 1944 1945 1946 1947 1948 1949 1950 1951 1952 1953 1954 1955 1956 1957 1958 1959 1960 1961 1962 1963 1964 1965 1966 1967 1968 1969 1970 1971 1972 1973 1974 1975 1976 1977 1978 1979 1980 1981 1982 1983 1984 1985 1986 1987 1988 1989 1990 1991 1992 1993 1994 1995 1996 1997 1998 1999 2000 2001 2002 2003 2004 2005 2006 2007 2008 2009 2010 2011 2012 2013 2014 2015 2016 2017 2018 2019 2020 2021 2022 2023 2024 2025 2026 2027 2028 2029 2030 2031 2032 2033 2034 2035 2036 2037 2038 2039 2040 2041 2042 2043 2044 2045 2046 2047 2048 2049 2050 2051 2052 2053 2054 2055 2056 2057 2058 2059 2060 2061 2062 2063 2064 2065 2066 2067 2068 2069 2070 2071 2072 2073 2074 2075 2076 2077 2078 2079 2080 2081 2082 2083 2084 2085 2086 2087 2088 2089 2090 2091 2092 2093 2094 2095 2096 2097 2098 2099 2100 2101 2102 2103 2104 2105 2106 2107 2108 2109 2110 2111 2112 2113 2114 2115 2116 2117 2118 2119 2120 2121 2122 2123 2124 2125 2126 2127 2128 2129 2130 2131 2132 2133 2134 2135 2136 2137 2138 2139 2140 2141 2142 2143 2144 2145 2146 2147 2148 2149 2150 2151 2152 2153 2154 2155 2156 2157 2158 2159 2160 2161 2162 2163 2164 2165 2166 2167 2168 2169 2170 2171 2172 2173 2174 2175 2176 2177 2178 2179 2180 2181 2182 2183 2184 2185 2186 2187 2188 2189 2190 2191 2192 2193 2194 2195 2196 2197 2198 2199 2200 2201 2202 2203 2204 2205 2206 2207 2208 2209 2210 2211 2212 2213 2214 2215 2216 2217 2218 2219 2220 2221 2222 2223 2224 2225 2226 2227 2228 2229 2230 2231 2232 2233 2234 2235 2236 2237 2238 2239 2240 2241 2242 2243 2244 2245 2246 2247 2248 2249 2250 2251 2252 2253 2254 2255 2256 2257 2258 2259 2260 2261 2262 2263 2264 2265 2266 2267 2268 2269 2270 2271 2272 2273 2274 2275 2276 2277 2278 2279 2280 2281 2282 2283 2284 2285 2286 2287 2288 2289 2290 2291 2292 2293 2294 2295 2296 2297 2298 2299 2300 2301 2302 2303 2304 2305 2306 2307 2308 2309 2310 2311 2312 2313 2314 2315 2316 2317 2318 2319 2320 2321 2322 2323 2324 2325 2326 2327 2328 2329 2330 2331 2332 2333 2334 2335 2336 2337 2338 2339 2340 2341 2342 2343 2344 2345 2346 2347 2348 2349 2350 2351 2352 2353 2354 2355 2356 2357 2358 2359 2360 2361 2362 2363 2364 2365 2366 2367 2368 2369 2370 2371 2372 2373 2374 2375 2376 2377 2378 2379 2380 2381 2382 2383 2384 2385 2386 2387 2388 2389 2390 2391 2392 2393 2394 2395 2396 2397 2398 2399 2400 2401 2402 2403 2404 2405 2406 2407 2408 2409 2410 2411 2412 2413 2414 2415 2416 2417 2418 2419 2420 2421 2422 2423 2424 2425 2426 2427 2428 2429 2430 2431 2432 2433 2434 2435 2436 2437 2438 2439 2440 2441 2442 2443 2444 2445 2446 2447 2448 2449 2450 2451 2452 2453 2454 2455 2456 2457 2458 2459 2460 2461 2462 2463 2464 2465 2466 2467 2468 2469 2470 2471 2472 2473 2474 2475 2476 2477 2478 2479 2480 2481 2482 2483 2484 2485 2486 2487 2488 2489 2490 2491 2492 2493 2494 2495 2496 2497 2498 2499 2500 2501 2502 2503 2504 2505 2506 2507 2508 2509 2510 2511 2512 2513 2514 2515 2516 2517 2518 2519 2520 2521 2522 2523 2524 2525 2526 2527 2528 2529 2530 2531 2532 2533 2534 2535 2536 2537 2538 2539 2540 2541 2542 2543 2544 2545 2546 2547 2548 2549 2550 2551 2552 2553 2554 2555 2556 2557 2558 2559 2560 2561 2562 2563 2564 2565 2566 2567 2568 2569 2570 2571 2572 2573 2574 2575 2576 2577 2578 2579 2580 2581 2582 2583 2584 2585 2586 2587 2588 2589 2590 2591 2592 2593 2594 2595 2596 2597 2598 2599 2600 2601 2602 2603 2604 2605 2606 2607 2608 2609 2610 2611 2612 2613 2614 2615 2616 2617 2618 2619 2620 2621 2622 2623 2624 2625 2626 2627 2628 2629 2630 2631 2632 2633 2634 2635 2636 2637 2638 2639 2640 2641 2642 2643 2644 2645 2646 2647 2648 2649 2650 2651 2652 2653 2654 2655 2656 2657 2658 2659 2660 2661 2662 2663 2664 2665 2666 2667 2668 2669 2670 2671 2672 2673 2674 2675 2676 2677 2678 2679 2680 2681 2682 2683 2684 2685 2686 2687 2688 2689 2690 2691 2692 2693 2694 2695 2696 2697 2698 2699 2700 2701 2702 2703 2704 2705 2706 2707 2708 2709 2710 2711 2712 2713 2714 2715 2716 2717 2718 2719 2720 2721 2722 2723 2724 2725 2726 2727 2728 2729 2730 2731 2732 2733 2734 2735 2736 2737 2738 2739 2740 2741 2742 2743 2744 2745 2746 2747 2748 2749 2750 2751 2752 2753 2754 2755 2756 2757 2758 2759 2760 2761 2762 2763 2764 2765 2766 2767 2768 2769 2770 2771 2772 2773 2774 2775 2776 2777 2778 2779 2780 2781 2782 2783 2784 2785 2786 2787 2788 2789 2790 2791 2792 2793 2794 2795 2796 2797 2798 2799 2800 2801 2802 2803 2804 2805 2806 2807 2808 2809 2810 2811 2812 2813 2814 2815 2816 2817 2818 2819 2820 2821 2822 2823 2824 2825 2826 2827 2828 2829 2830 2831 2832 2833 2834 2835 2836 2837 2838 2839 2840 2841 2842 2843 2844 2845 2846 2847 2848 2849 2850 2851 2852 2853 2854 2855 2856 2857 2858 2859 2860 2861 2862 2863 2864 2865 2866 2867 2868 2869 2870 2871 2872 2873 2874 2875 2876 2877 2878 2879 2880 2881 2882 2883 2884 2885 2886 2887 2888 2889 2890 2891 2892 2893 2894 2895 2896 2897 2898 2899 2900 2901 2902 2903 2904 2905 2906 2907 2908 2909 2910 2911 2912 2913 2914 2915 2916 2917 2918 2919 2920 2921 2922 2923 2924 2925 2926 2927 2928 2929 2930 2931 2932 2933 2934 2935 2936 2937 2938 2939 2940 2941 2942 2943 2944 2945 2946 2947 2948 2949 2950 2951 2952 2953 2954 2955 2956 2957 2958 2959 2960 2961 2962 2963 2964 2965 2966 2967 2968 2969 2970 2971 2972 2973 2974 2975 2976 2977 2978 2979 2980 2981 2982 2983 2984 2985 2986 2987 2988 2989 2990 2991 2992 2993 2994 2995 2996 2997 2998 2999 3000 3001 3002 3003 3004 3005 3006 3007 3008 3009 3010 3011 3012 3013 3014 3015 3016 3017 3018 3019 3020 3021 3022 3023 3024 3025 3026 3027 3028 3029 3030 3031 3032 3033 3034 3035 3036 3037 3038 3039 3040 3041 3042 3043 3044 3045 3046 3047 3048 3049 3050 3051 3052 3053 3054 3055 3056 3057 3058 3059 3060 3061 3062 3063 3064 3065 3066 3067 3068 3069 3070 3071 3072 3073 3074 3075 3076 3077 3078 3079 3080 3081 3082 3083 3084 3085 3086 3087 3088 3089 3090 3091 3092 3093 3094 3095 3096 3097 3098 3099 3100 3101 3102 3103 3104 3105 3106 3107 3108 3109 3110 3111 3112 3113 3114 3115 3116 3117 3118 3119 3120 3121 3122 3123 3124 3125 3126 3127 3128 3129 3130

#ifndef STG_H
#define STG_H
/*
* Stage : a multi-robot simulator. Part of the Player Project.
*
* Copyright (C) 2001-2009 Richard Vaughan, Brian Gerkey, Andrew
* Howard, Toby Collett, Reed Hedges, Alex Couture-Beil, Jeremy
* Asher, Pooya Karimian
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/

/** \file stage.hh
* Desc: External header file for the Stage library
* Author: Richard Vaughan (vaughan@sfu.ca)
* Date: 1 June 2003
* SVN: $Id$
*/

// C libs
#include <unistd.h>
#include <stdint.h> // for portable int types eg. uint32_t
#include <assert.h>
#include <stdlib.h>
#include <stdio.h>
#include <libgen.h>
#include <string.h>
#include <sys/types.h>
#include <sys/time.h>
#include <pthread.h>

// C++ libs
#include <cmath>
#include <iostream>
#include <vector>
#include <list>
#include <map>
#include <set>
#include <queue>
#include <algorithm>

// FLTK Gui includes
#include <FL/Fl.H>
#include <FL/Fl_Box.H>
#include <FL/Fl_Gl_Window.H>
#include <FL/Fl_Menu_Bar.H>
#include <FL/Fl_Window.H>
#include <FL/fl_draw.H>
#include <FL/gl.h> // FLTK takes care of platform-specific GL stuff
// except GLU
#ifdef __APPLE__
#include <OpenGL/glu.h>
#else
#include <GL/glu.h>
#endif

/** @brief The Stage library uses its own namespace */
namespace Stg
{
  // forward declare
  class Block;
  class Canvas;
  class Cell;
  class Worldfile;
  class World;
  class WorldGui;
  class Model;
  class OptionsDlg;
  class Camera;
  class FileManager;
  class Option;
  
  typedef Model* (*creator_t)( World*, Model*, const std::string& type );
  
  /** Initialize the Stage library. Stage will parse the argument
array looking for parameters in the conventional way. */
  void Init( int* argc, char** argv[] );

  /** returns true iff Stg::Init() has been called. */
  bool InitDone();
  
  /** returns a human readable string indicating the libstage version
number. */
  const char* Version();

  /** Copyright string */
  const char COPYRIGHT[] =
    "Copyright Richard Vaughan and contributors 2000-2009";

  /** Author string */
  const char AUTHORS[] =
    "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, Toby Collett, Jeremy Asher, Alex Couture-Beil and contributors.";

  /** Project website string */
  const char WEBSITE[] = "http://playerstage.org";

  /** Project description string */
  const char DESCRIPTION[] =
    "Robot simulation library\nPart of the Player Project";

  /** Project distribution license string */
  const char LICENSE[] =
    "Stage robot simulation library\n" \
    "Copyright (C) 2000-2009 Richard Vaughan and contributors\n" \
    "Part of the Player Project [http://playerstage.org]\n" \
    "\n" \
    "This program is free software; you can redistribute it and/or\n" \
    "modify it under the terms of the GNU General Public License\n" \
    "as published by the Free Software Foundation; either version 2\n" \
    "of the License, or (at your option) any later version.\n" \
    "\n" \
    "This program is distributed in the hope that it will be useful,\n" \
    "but WITHOUT ANY WARRANTY; without even the implied warranty of\n" \
    "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n" \
    "GNU General Public License for more details.\n" \
    "\n" \
    "You should have received a copy of the GNU General Public License\n" \
    "along with this program; if not, write to the Free Software\n" \
    "Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.\n" \
    "\n" \
    "The text of the license may also be available online at\n" \
    "http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";

  /** Convenient constant */
  const double thousand = 1e3;

  /** Convenient constant */
  const double million = 1e6;

  /** Convenient constant */
  const double billion = 1e9;

  /** convert an angle in radians to degrees. */
  inline double rtod( double r ){ return( r*180.0/M_PI ); }
  
  /** convert an angle in degrees to radians. */
  inline double dtor( double d ){ return( d*M_PI/180.0 ); }
  
  /** Normalize an angle to within +/_ M_PI. */
  inline double normalize( double a )
  {
    while( a < -M_PI ) a += 2.0*M_PI;
    while( a > M_PI ) a -= 2.0*M_PI;
    return a;
  };

  /** take binary sign of a, either -1, or 1 if >= 0 */
  inline int sgn( int a){ return( a<0 ? -1 : 1); }

  /** take binary sign of a, either -1.0, or 1.0 if >= 0. */
  inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); }
  
  /** any integer value other than this is a valid fiducial ID */
  enum { FiducialNone = 0 };

  /** Value that uniquely identifies a model */
  typedef uint32_t id_t;

  /** Metres: floating point unit of distance */
  typedef double meters_t;

  /** Radians: unit of angle */
  typedef double radians_t;

  /** time structure */
  typedef struct timeval time_t;

  /** Milliseconds: unit of (short) time */
  typedef unsigned long msec_t;

  /** Microseconds: unit of (very short) time */
  typedef uint64_t usec_t;

  /** Kilograms: unit of mass */
  typedef double kg_t; // Kilograms (mass)

  /** Joules: unit of energy */
  typedef double joules_t;

  /** Watts: unit of power (energy/time) */
  typedef double watts_t;
  
  class Color
  {
  public:
    double r,g,b,a;

    Color( double r, double g, double b, double a=1.0 );

    /** Look up the color in the X11-style database. If the color is
not found in the database, a cheerful red color will be used
instead. */
    Color( const std::string& name );

    Color();

    bool operator!=( const Color& other ) const;
    bool operator==( const Color& other ) const;
    static Color RandomColor();
    void Print( const char* prefix ) const;

    /** convenient constants */
    static const Color blue, red, green, yellow, magenta, cyan;

    const Color& Load( Worldfile* wf, int entity );

    void GLSet( void ) { glColor4f( r,g,b,a ); }
  };
  
  /** specify a rectangular size */
  class Size
  {
  public:
    meters_t x, y, z;

    Size( meters_t x,
meters_t y,
meters_t z )
      : x(x), y(y), z(z)
    {/*empty*/}

    /** default constructor uses default non-zero values */
    Size() : x( 0.4 ), y( 0.4 ), z( 1.0 )
    {/*empty*/}

    Size& Load( Worldfile* wf, int section, const char* keyword );
    void Save( Worldfile* wf, int section, const char* keyword ) const;

    void Zero() { x=y=z=0.0; }
  };
  
  /** Specify a 3 axis position, in x, y and heading. */
  class Pose
  {
  public:
    meters_t x, y, z;///< location in 3 axes
    radians_t a;///< rotation about the z axis.
    
    Pose( meters_t x,
meters_t y,
meters_t z,
radians_t a )
      : x(x), y(y), z(z), a(a)
    { /*empty*/ }
    
    Pose() : x(0.0), y(0.0), z(0.0), a(0.0)
    { /*empty*/ }
    
    virtual ~Pose(){};
    
    /** return a random pose within the bounding rectangle, with z=0 and
angle random */
    static Pose Random( meters_t xmin, meters_t xmax,
meters_t ymin, meters_t ymax )
    {
      return Pose( xmin + drand48() * (xmax-xmin),
ymin + drand48() * (ymax-ymin),
0,
normalize( drand48() * (2.0 * M_PI) ));
    }
    
    /** Print pose in human-readable format on stdout
@param prefix Character string to prepend to pose output
*/
    virtual void Print( const char* prefix ) const
    {
      printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
prefix, x,y,z,a );
    }

    std::string String() const
    {
      char buf[256];
      snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]",
x,y,z,a );
      return std::string(buf);
    }

    /* returns true iff all components of the velocity are zero. */
    bool IsZero() const
    { return( !(x || y || z || a )); };

    /** Set the pose to zero [0,0,0,0] */
    void Zero()
    { x=y=z=a=0.0; }

    Pose& Load( Worldfile* wf, int section, const char* keyword );
    void Save( Worldfile* wf, int section, const char* keyword );

    inline Pose operator+( const Pose& p ) const
    {
      const double cosa = cos(a);
      const double sina = sin(a);

      return Pose( x + p.x * cosa - p.y * sina,
y + p.x * sina + p.y * cosa,
z + p.z,
normalize(a + p.a) );
    }

    // a < b iff a is closer to the origin than b
    bool operator<( const Pose& p ) const
    {
//return( hypot( y, x ) < hypot( otHer.y, other.x ));
// just compare the squared values to avoid the sqrt()
      return( (y*y+x*x) < (p.y*p.y + p.x*p.x ));
    }

    bool operator==( const Pose& other ) const
    {
      return( x==other.x &&
y==other.y &&
z==other.z &&
a==other.a );
    }

    bool operator!=( const Pose& other ) const
    {
      return( x!=other.x ||
y!=other.y ||
z!=other.z ||
a!=other.a );
    }

meters_t Distance( const Pose& other ) const
{
return hypot( x-other.x, y-other.y );
}
  };
  

  class RaytraceResult
  {
  public:
    Pose pose;
    Model* mod;
    Color color;
    meters_t range;

    RaytraceResult() : pose(), mod(NULL), color(), range(0.0) {}
    RaytraceResult( const Pose& pose, Model* mod, const Color& color, const meters_t range)
       : pose(pose), mod(mod), color(color), range(range) {}
    
  };
  
  
  /** Specify a 4 axis velocity: 3D vector in [x, y, z], plus rotation
about Z (yaw).*/
  class Velocity : public Pose
  {
  public:
    /** @param x velocity vector component along X axis (forward speed), in meters per second.
@param y velocity vector component along Y axis (sideways speed), in meters per second.
@param z velocity vector component along Z axis (vertical speed), in meters per second.
@param a rotational velocity around Z axis (yaw), in radians per second.
*/
    Velocity( double x,
double y,
double z,
double a ) :
      Pose( x, y, z, a )
    { /*empty*/ }
    
    Velocity()
    { /*empty*/ }
    
    
    Velocity& Load( Worldfile* wf, int section, const char* keyword )
    {
       Pose::Load( wf, section, keyword );
       return *this;
    }
    
    /** Print velocity in human-readable format on stdout, with a
prefix string
@param prefix Character string to prepend to output, or NULL.
*/
    virtual void Print( const char* prefix ) const
    {
      if( prefix )
printf( "%s", prefix );
      
      printf( "velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
x,y,z,a );
    }
  };
  
 
  /** Specify an object's basic geometry: position and rectangular
size. */
  class Geom
  {
  public:
    Pose pose;///< position
    Size size;///< extent
    
    /** Print geometry in human-readable format on stdout, with a
prefix string
@param prefix Character string to prepend to output, or NULL.
*/
    void Print( const char* prefix ) const
    {
      if( prefix )
printf( "%s", prefix );

      printf( "geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n",
pose.x,
pose.y,
pose.a,
size.x,
size.y );
    }

    /** Default constructor. Members pose and size use their default constructors. */
    Geom() : pose(), size() {}

    /** construct from a prior pose and size */
    Geom( const Pose& p, const Size& s ) : pose(p), size(s) {}

    void Zero()
    {
      pose.Zero();
      size.Zero();
    }
  };
  
  /** Bound a range of values, from min to max. min and max are initialized to zero. */
  class Bounds
  {
  public:
    /// smallest value in range, initially zero
    double min;
    /// largest value in range, initially zero
    double max;
    
    Bounds() : min(0), max(0) { /* empty*/ }
    Bounds( double min, double max ) : min(min), max(max) { /* empty*/ }

    Bounds& Load( Worldfile* wf, int section, const char* keyword );

    // returns value, but no smaller than min and no larger than max.
    double Constrain( double value );
  };
    
  /** Define a three-dimensional bounding box, initialized to zero */
  class bounds3d_t
  {
  public:
    /// volume extent along x axis, intially zero
    Bounds x;
    /// volume extent along y axis, initially zero
    Bounds y;
    /// volume extent along z axis, initially zero
    Bounds z;

    bounds3d_t() : x(), y(), z() {}
    bounds3d_t( const Bounds& x, const Bounds& y, const Bounds& z)
      : x(x), y(y), z(z) {}
  };
  
  /** Define a field-of-view: an angle and range bounds */
  typedef struct
  {
    Bounds range; ///< min and max range of sensor
    radians_t angle; ///< width of viewing angle of sensor
  } fov_t;
  
  /** Define a point on a 2d plane */
  class point_t
  {
  public:
    meters_t x, y;
    point_t( meters_t x, meters_t y ) : x(x), y(y){}
    point_t() : x(0.0), y(0.0){}

    bool operator+=( const point_t& other )
    { return ((x += other.x) && (y += other.y) ); }

    /** required to put these in sorted containers like std::map */
    bool operator<( const point_t& other ) const
    {
      if( x < other.x ) return true;
      if( other.x < x ) return false;
      return y < other.y;
    }

    bool operator==( const point_t& other ) const
    { return ((x == other.x) && (y == other.y) ); }
  };
  
  /** Define a point in 3d space */
  class point3_t
  {
  public:
    meters_t x,y,z;
    point3_t( meters_t x, meters_t y, meters_t z )
      : x(x), y(y), z(z) {}
  
    point3_t() : x(0.0), y(0.0), z(0.0) {}
  };
  
  /** Define an integer point on the 2d plane */
  class point_int_t
  {
  public:
    int x,y;
    point_int_t( int x, int y ) : x(x), y(y){}
    point_int_t() : x(0), y(0){}

    /** required to put these in sorted containers like std::map */
    bool operator<( const point_int_t& other ) const
    {
      if( x < other.x ) return true;
      if( other.x < x ) return false;
      return y < other.y;
    }

    bool operator==( const point_int_t& other ) const
    { return ((x == other.x) && (y == other.y) ); }
  };
  
  /** create an array of 4 points containing the corners of a unit
square. */
  point_t* unit_square_points_create();
  
  /** Convenient OpenGL drawing routines, used by visualization
code. */
  namespace Gl
  {
    void pose_shift( const Pose &pose );
    void pose_inverse_shift( const Pose &pose );
    void coord_shift( double x, double y, double z, double a );
    void draw_grid( bounds3d_t vol );
    /** Render a string at [x,y,z] in the current color */
    void draw_string( float x, float y, float z, const char *string);
    void draw_string_multiline( float x, float y, float w, float h,
const char *string, Fl_Align align );
    void draw_speech_bubble( float x, float y, float z, const char* str );
    void draw_octagon( float w, float h, float m );
    void draw_octagon( float x, float y, float w, float h, float m );
    void draw_vector( double x, double y, double z );
    void draw_origin( double len );
    void draw_array( float x, float y, float w, float h,
float* data, size_t len, size_t offset,
float min, float max );
    void draw_array( float x, float y, float w, float h,
float* data, size_t len, size_t offset );
    /** Draws a rectangle with center at x,y, with sides of length dx,dy */
    void draw_centered_rect( float x, float y, float dx, float dy );
  } // namespace Gl
  
  void RegisterModels();
  
  
  /** Abstract class for adding visualizations to models. Visualize must be overloaded, and is then called in the models local coord system */
  class Visualizer {
  private:
    const std::string menu_name;
    const std::string worldfile_name;

  public:
    Visualizer( const std::string& menu_name,
const std::string& worldfile_name )
      : menu_name( menu_name ),
worldfile_name( worldfile_name )
    { }

    virtual ~Visualizer( void ) { }
    virtual void Visualize( Model* mod, Camera* cam ) = 0;

    const std::string& GetMenuName() { return menu_name; }
    const std::string& GetWorldfileName() { return worldfile_name; }
  };


  /** Define a callback function type that can be attached to a
record within a model and called whenever the record is set.*/
  typedef int(*model_callback_t)(Model* mod, void* user );

  typedef int(*world_callback_t)(World* world, void* user );
  
  // return val, or minval if val < minval, or maxval if val > maxval
  double constrain( double val, double minval, double maxval );
    
  typedef struct
  {
    int enabled;
    Pose pose;
    meters_t size; ///< rendered as a sphere with this diameter
    Color color;
    msec_t period; ///< duration of a complete cycle
    double duty_cycle; ///< mark/space ratio
  } blinkenlight_t;

  
  /** Defines a rectangle of [size] located at [pose] */
  typedef struct
  {
    Pose pose;
    Size size;
  } rotrect_t; // rotated rectangle
  
  /** load the image file [filename] and convert it to a vector of polygons
*/
  int polys_from_image_file( const std::string& filename,
std::vector<std::vector<point_t> >& polys );


  /** matching function should return true iff the candidate block is
stops the ray, false if the block transmits the ray
*/
  typedef bool (*ray_test_func_t)(Model* candidate,
Model* finder,
const void* arg );

  // STL container iterator macros - __typeof is a gcc extension, so
  // this could be an issue one day.
#define VAR(V,init) __typeof(init) V=(init)

  //#define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();++I)

  // NOTE:
  // this version assumes the container is not modified in the loop,
  // which I think is true everywhere it is used in Stage
#define FOR_EACH(I,C) for(VAR(I,(C).begin()),ite=(C).end();(I)!=ite;++(I))

  /** wrapper for Erase-Remove method of removing all instances of thing from container
*/
  template <class T, class C>
  void EraseAll( T thing, C& cont )
  { cont.erase( std::remove( cont.begin(), cont.end(), thing ), cont.end() ); }
  
  // Error macros - output goes to stderr
#define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
#define PRINT_ERR2(m,a,b) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
#define PRINT_ERR3(m,a,b,c) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_ERR4(m,a,b,c,d) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_ERR5(m,a,b,c,d,e) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)

  // Warning macros
#define PRINT_WARN(m) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_WARN1(m,a) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
#define PRINT_WARN2(m,a,b) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
#define PRINT_WARN3(m,a,b,c) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_WARN4(m,a,b,c,d) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_WARN5(m,a,b,c,d,e) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)

  // Message macros
#ifdef DEBUG
#define PRINT_MSG(m) printf( "Stage: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_MSG1(m,a) printf( "Stage: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
#define PRINT_MSG2(m,a,b) printf( "Stage: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
#define PRINT_MSG3(m,a,b,c) printf( "Stage: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m" (%s %s)\n", a, b, c, d, e,__FILE__, __FUNCTION__)
#else
#define PRINT_MSG(m) printf( "Stage: "m"\n" )
#define PRINT_MSG1(m,a) printf( "Stage: "m"\n", a)
#define PRINT_MSG2(m,a,b) printf( "Stage: "m"\n,", a, b )
#define PRINT_MSG3(m,a,b,c) printf( "Stage: "m"\n", a, b, c )
#define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m"\n", a, b, c, d )
#define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m"\n", a, b, c, d, e )
#endif

  // DEBUG macros
#ifdef DEBUG
#define PRINT_DEBUG(m) printf( "debug: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_DEBUG1(m,a) printf( "debug: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
#define PRINT_DEBUG2(m,a,b) printf( "debug: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
#define PRINT_DEBUG3(m,a,b,c) printf( "debug: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_DEBUG4(m,a,b,c,d) printf( "debug: "m" (%s %s)\n", a, b, c ,d, __FILE__, __FUNCTION__)
#define PRINT_DEBUG5(m,a,b,c,d,e) printf( "debug: "m" (%s %s)\n", a, b, c ,d, e, __FILE__, __FUNCTION__)
#else
#define PRINT_DEBUG(m)
#define PRINT_DEBUG1(m,a)
#define PRINT_DEBUG2(m,a,b)
#define PRINT_DEBUG3(m,a,b,c)
#define PRINT_DEBUG4(m,a,b,c,d)
#define PRINT_DEBUG5(m,a,b,c,d,e)
#endif

  class Block;
  class Model;


  // ANCESTOR CLASS
  /** Base class for Model and World */
  class Ancestor
  {
    friend class Canvas; // allow Canvas access to our private members

  protected:

    /** array contains the number of each type of child model */
    std::map<std::string,unsigned int> child_type_counts;

    std::vector<Model*> children;

    bool debug;

    /** A key-value database for users to associate arbitrary things with this object. */
    std::map<std::string,void*> props;

    std::string token;

    Ancestor& Load( Worldfile* wf, int section );
    void Save( Worldfile* wf, int section );

  public:
    Ancestor();
    virtual ~Ancestor();

    /** get the children of the this element */
    std::vector<Model*>& GetChildren(){ return children;}
    
    /** recursively call func( model, arg ) for each descendant */
    void ForEachDescendant( model_callback_t func, void* arg );
    
    virtual void AddChild( Model* mod );
    virtual void RemoveChild( Model* mod );
    virtual Pose GetGlobalPose() const;
    
    const char* Token() const { return token.c_str(); }
    const std::string& TokenStr() const { return token; }
    
    virtual void SetToken( const std::string& str )
    {
      //printf( "Ancestor::SetToken( %s )\n", str.c_str() );

      if( str.size() > 0 )
token = str;
      else
PRINT_ERR( "Ancestor::SetToken() called with zero length string. Ignored." );
    }

    /** A key-value database for users to associate arbitrary things with this model. */
    void SetProperty( std::string& key, void* value ){ props[ key ] = value; }

    /** A key-value database for users to associate arbitrary things with this model. */
    void* GetProperty( std::string& key )
    {
      std::map<std::string,void*>::iterator it = props.find( key );
      return( it == props.end() ? NULL : it->second );
    }
  };


  class Ray
  {
  public:
    Ray( const Model* mod, const Pose& origin, const meters_t range, const ray_test_func_t func, const void* arg, const bool ztest ) :
      mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest)
    {}

    Ray() : mod(NULL), origin(0,0,0,0), range(0), func(NULL), arg(NULL), ztest(true)
    {}

    const Model* mod;
    Pose origin;
    meters_t range;
    ray_test_func_t func;
    const void* arg;
    bool ztest;
  };


  // defined in stage_internal.hh
  class Region;
  class SuperRegion;
  class BlockGroup;
  class PowerPack;

  class LogEntry
  {
    usec_t timestamp;
    Model* mod;
    Pose pose;

  public:
    LogEntry( usec_t timestamp, Model* mod );

    /** A log of all LogEntries ever created */
    static std::vector<LogEntry> log;

    /** Returns the number of logged entries */
    static size_t Count(){ return log.size(); }

    /** Clear the log */
    static void Clear(){ log.clear(); }

    /** Print the log on stdout */
    static void Print();
  };

  class CtrlArgs
  {
  public:
    std::string worldfile;
    std::string cmdline;

    CtrlArgs( std::string w, std::string c ) : worldfile(w), cmdline(c) {}
  };

  class ModelPosition;

  /// %World class
  class World : public Ancestor
  {
  public:
    friend class Block;
    friend class Model; // allow access to private members
    friend class ModelFiducial;
    friend class Canvas;
    friend class WorkerThread;

  public:
    /** contains the command line arguments passed to Stg::Init(), so
that controllers can read them. */
    static std::vector<std::string> args;
    static std::string ctrlargs;

  private:

    static std::set<World*> world_set; ///< all the worlds that exist
    static bool quit_all; ///< quit all worlds ASAP
    static void UpdateCb( World* world);
    static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids

    bool destroy;
    bool dirty; ///< iff true, a gui redraw would be required

    /** Pointers to all the models in this world. */
    std::set<Model*> models;

    /** pointers to the models that make up the world, indexed by name. */
    std::map<std::string, Model*> models_by_name;

    /** pointers to the models that make up the world, indexed by worldfile entry index */
    std::map<int,Model*> models_by_wfentity;

    /** Keep a list of all models with detectable fiducials. This
avoids searching the whole world for fiducials. */
    std::vector<Model*> models_with_fiducials;

    struct ltx
    {
      bool operator()(const Model* a, const Model* b) const;
    };

    struct lty
    {
      bool operator()(const Model* a, const Model* b) const;
    };

    /** maintain a set of models with fiducials sorted by pose.x, for
quickly finding nearby fidcucials */
    std::set<Model*,ltx> models_with_fiducials_byx;

    /** maintain a set of models with fiducials sorted by pose.y, for
quickly finding nearby fidcucials */
    std::set<Model*,lty> models_with_fiducials_byy;

    /** Add a model to the set of models with non-zero fiducials, if not already there. */
    void FiducialInsert( Model* mod )
    {
      FiducialErase( mod ); // make sure it's not there already
      models_with_fiducials.push_back( mod );
    }

    /** Remove a model from the set of models with non-zero fiducials, if it exists. */
    void FiducialErase( Model* mod )
    {
      EraseAll( mod, models_with_fiducials );
    }

    double ppm; ///< the resolution of the world model in pixels per meter
    bool quit; ///< quit this world ASAP
    bool show_clock; ///< iff true, print the sim time on stdout
    unsigned int show_clock_interval; ///< updates between clock outputs

    //--- thread sync ----
    pthread_mutex_t sync_mutex; ///< protect the worker thread management stuff
    unsigned int threads_working; ///< the number of worker threads not yet finished
    pthread_cond_t threads_start_cond; ///< signalled to unblock worker threads
    pthread_cond_t threads_done_cond; ///< signalled by last worker thread to unblock main thread
    int total_subs; ///< the total number of subscriptions to all models
    unsigned int worker_threads; ///< the number of worker threads to use
    
  protected:

    std::list<std::pair<world_callback_t,void*> > cb_list; ///< List of callback functions and arguments
    bounds3d_t extent; ///< Describes the 3D volume of the world
    bool graphics;///< true iff we have a GUI

    std::set<Option*> option_table; ///< GUI options (toggles) registered by models
    std::list<PowerPack*> powerpack_list; ///< List of all the powerpacks attached to models in the world
    /** World::quit is set true when this simulation time is reached */
    usec_t quit_time;
    std::list<float*> ray_list;///< List of rays traced for debug visualization
    usec_t sim_time; ///< the current sim time in this world in microseconds
    std::map<point_int_t,SuperRegion*> superregions;

    uint64_t updates; ///< the number of simulated time steps executed so far
    Worldfile* wf; ///< If set, points to the worldfile used to create this world

    void CallUpdateCallbacks(); ///< Call all calbacks in cb_list, removing any that return true;

  public:

    uint64_t UpdateCount(){ return updates; }

    bool paused; ///< if true, the simulation is stopped

    virtual void Start(){ paused = false; };
    virtual void Stop(){ paused = true; };
    virtual void TogglePause(){ paused ? Start() : Stop(); };

    bool Paused() const { return( paused ); };

    /** Force the GUI to redraw the world, even if paused. This
imlementation does nothing, but can be overridden by
subclasses. */
    virtual void Redraw( void ){ }; // does nothing

    std::vector<point_int_t> rt_cells;
    std::vector<point_int_t> rt_candidate_cells;

    static const int DEFAULT_PPM = 50; // default resolution in pixels per meter

    /** Attach a callback function, to be called with the argument at
the end of a complete update step */
    void AddUpdateCallback( world_callback_t cb, void* user );

    /** Remove a callback function. Any argument data passed to
AddUpdateCallback is not automatically freed. */
    int RemoveUpdateCallback( world_callback_t cb, void* user );

    /** Log the state of a Model */
    void Log( Model* mod );

    /** hint that the world needs to be redrawn if a GUI is attached */
    void NeedRedraw(){ dirty = true; };
    
    /** Special model for the floor of the world */
    Model* ground;
    
    /** Get human readable string that describes the current simulation
time. */
    virtual std::string ClockString( void ) const;

    Model* CreateModel( Model* parent, const std::string& typestr );

    void LoadModel( Worldfile* wf, int entity );
    void LoadBlock( Worldfile* wf, int entity );
    void LoadBlockGroup( Worldfile* wf, int entity );
    void LoadSensor( Worldfile* wf, int entity );

    virtual Model* RecentlySelectedModel() const { return NULL; }
    
    /** Add the block to every raytrace bitmap cell that intersects
the edges of the polygon.*/
    void MapPoly( const std::vector<point_int_t>& poly,
Block* block,
unsigned int layer );

    SuperRegion* AddSuperRegion( const point_int_t& coord );
    SuperRegion* GetSuperRegion( const point_int_t& org );
    SuperRegion* GetSuperRegionCreate( const point_int_t& org );

    /** convert a distance in meters to a distance in world occupancy
grid pixels */
    int32_t MetersToPixels( meters_t x ) const
    { return (int32_t)floor(x * ppm); };
    
    /** Return the bitmap coordinates corresponding to the location in meters. */
    point_int_t MetersToPixels( const point_t& pt ) const
    { return point_int_t( MetersToPixels(pt.x), MetersToPixels(pt.y)); };

    // dummy implementations to be overloaded by GUI subclasses
    virtual void PushColor( Color col )
    { /* do nothing */ (void)col; };
    virtual void PushColor( double r, double g, double b, double a )
    { /* do nothing */ (void)r; (void)g; (void)b; (void)a; };

    virtual void PopColor(){ /* do nothing */ };

    SuperRegion* CreateSuperRegion( point_int_t origin );
    void DestroySuperRegion( SuperRegion* sr );

    /** trace a ray. */
    RaytraceResult Raytrace( const Ray& ray );
    
    RaytraceResult Raytrace( const Pose& pose,
const meters_t range,
const ray_test_func_t func,
const Model* finder,
const void* arg,
const bool ztest );
    
    void Raytrace( const Pose &gpose, // global pose
const meters_t range,
const radians_t fov,
const ray_test_func_t func,
const Model* model,
const void* arg,
const bool ztest,
std::vector<RaytraceResult>& results );

    /** Enlarge the bounding volume to include this point */
    inline void Extend( point3_t pt );
  
    virtual void AddModel( Model* mod );
    virtual void RemoveModel( Model* mod );

    void AddModelName( Model* mod, const std::string& name );

    void AddPowerPack( PowerPack* pp );
    void RemovePowerPack( PowerPack* pp );

    void ClearRays();
  
    /** store rays traced for debugging purposes */
    void RecordRay( double x1, double y1, double x2, double y2 );

    /** Returns true iff the current time is greater than the time we
should quit */
    bool PastQuitTime();

    static void* update_thread_entry( std::pair<World*,int>* info );
    
    class Event
    {
    public:
      
      Event( usec_t time, Model* mod, model_callback_t cb, void* arg )
: time(time), mod(mod), cb(cb), arg(arg) {}

      usec_t time; ///< time that event occurs
      Model* mod; ///< model to pass into callback
      model_callback_t cb;
      void* arg;

      /** order by time. Break ties by value of Model*, then cb*.
@param event to compare with this one. */
      bool operator<( const Event& other ) const;
    };

    /** Queue of pending simulation events for the main thread to handle. */
    std::vector<std::priority_queue<Event> > event_queues;

    /** Queue of pending simulation events for the main thread to handle. */
    std::vector<std::queue<Model*> > pending_update_callbacks;

    /** Create a new simulation event to be handled in the future.

@param queue_num Specify which queue the event should be on. The main
thread is 0.

@param delay The time from now until the event occurs, in
microseconds.

@param mod The model that should have its Update() method
called at the specified time.
*/
    void Enqueue( unsigned int queue_num, usec_t delay, Model* mod, model_callback_t cb, void* arg )
    { event_queues[queue_num].push( Event( sim_time + delay, mod, cb, arg ) ); }

    /** Set of models that require energy calculations at each World::Update(). */
    std::set<Model*> active_energy;
    void EnableEnergy( Model* m ) { active_energy.insert( m ); };
    void DisableEnergy( Model* m ) { active_energy.erase( m ); };
    
    /** Set of models that require their positions to be recalculated at each World::Update(). */
    std::set<ModelPosition*> active_velocity;
    
    /** The amount of simulated time to run for each call to Update() */
    usec_t sim_interval;

    // debug instrumentation - making sure the number of update callbacks
    // in each thread is consistent with the number that have been
    // registered globally
    int update_cb_count;

    /** consume events from the queue up to and including the current sim_time */
    void ConsumeQueue( unsigned int queue_num );

    /** returns an event queue index number for a model to use for
updates */
    unsigned int GetEventQueue( Model* mod ) const;

  public:
    /** returns true when time to quit, false otherwise */
    static bool UpdateAll();

  /** run all worlds.
* If only non-gui worlds were created, UpdateAll() is
* repeatedly called.
* To simulate a gui world only a single gui world may
* have been created. This world is then simulated.
*/
  static void Run();

    World( const std::string& name = "MyWorld",
double ppm = DEFAULT_PPM );

    virtual ~World();
  
    /** Returns the current simulated time in this world, in microseconds. */
    usec_t SimTimeNow(void) const { return sim_time; }

    /** Returns a pointer to the currently-open worlddfile object, or
NULL if there is none. */
    Worldfile* GetWorldFile() { return wf; };

    /** Returns true iff this World implements a GUI. The base World
class returns false, but subclasses can override this
behaviour. */
    virtual bool IsGUI() const { return false; }

    /** Open the file at the specified location, create a Worldfile
object, read the file and configure the world from the
contents, creating models as necessary. The created object
persists, and can be retrieved later with
World::GetWorldFile(). */
    virtual void Load( const std::string& worldfile_path );

    virtual void UnLoad();

    virtual void Reload();

    /** Save the current world state into a worldfile with the given
filename. @param Filename to save as. */
    virtual bool Save( const char* filename );

    /** Run one simulation timestep. Advances the simulation clock,
executes all simulation updates due at the current time, then
queues up future events. */
    virtual bool Update(void);

    /** Returns true iff either the local or global quit flag was set,
which usually happens because someone called Quit() or
QuitAll(). */
    bool TestQuit() const { return( quit || quit_all ); }

    /** Request the world quits simulation before the next timestep. */
    void Quit(){ quit = true; }

    /** Requests all worlds quit simulation before the next timestep. */
    void QuitAll(){ quit_all = true; }

    /** Cancel a local quit request. */
    void CancelQuit(){ quit = false; }

    /** Cancel a global quit request. */
    void CancelQuitAll(){ quit_all = false; }

    void TryCharge( PowerPack* pp, const Pose& pose );

    /** Get the resolution in pixels-per-metre of the underlying
discrete raytracing model */
    double Resolution() const { return ppm; };
   
    /** Returns a pointer to the model identified by name, or NULL if
nonexistent */
    Model* GetModel( const std::string& name ) const;

    /** Returns a const reference to the set of models in the world. */
    const std::set<Model*> GetAllModels() const { return models; };
  
    /** Return the 3D bounding box of the world, in meters */
    const bounds3d_t& GetExtent() const { return extent; };
  
    /** Return the number of times the world has been updated. */
    uint64_t GetUpdateCount() const { return updates; }

    /// Register an Option for pickup by the GUI
    void RegisterOption( Option* opt );

    /// Control printing time to stdout
    void ShowClock( bool enable ){ show_clock = enable; };

    /** Return the floor model */
    Model* GetGround() {return ground;};

  };
  

  class Block
  {
    friend class BlockGroup;
    friend class Model;
    friend class SuperRegion;
    friend class World;
    friend class Canvas;
    friend class Cell;
  public:

    /** Block Constructor. A model's body is a list of these
blocks. The point data is copied, so pts can safely be freed
after constructing the block.*/
    Block( BlockGroup* group,
const std::vector<point_t>& pts,
const Bounds& zrange );
    
    /** A from-file constructor */
    Block( BlockGroup* group, Worldfile* wf, int entity);
    
    ~Block();
    
    /** render the block into the world's raytrace data structure */
    void Map( unsigned int layer );
    
    /** remove the block from the world's raytracing data structure */
    void UnMap( unsigned int layer );
    
    /** draw the block in OpenGL as a solid single color */
    void DrawSolid(bool topview);

    /** draw the projection of the block onto the z=0 plane */
    void DrawFootPrint();

    /** Translate all points in the block by the indicated amounts */
    void Translate( double x, double y );

    /** Return the center of the block on the X axis */
    double CenterX();

    /** Return the center of the block on the Y axis */
    double CenterY();

    /** Set the center of the block on the X axis */
    void SetCenterX( double y );

    /** Set the center of the block on the Y axis */
    void SetCenterY( double y );

    /** Set the center of the block */
    void SetCenter( double x, double y);

    /** Set the extent in Z of the block */
    void SetZ( double min, double max );

    void AppendTouchingModels( std::set<Model*>& touchers );

    /** Returns the first model that shares a bitmap cell with this model */
    Model* TestCollision();

    void Load( Worldfile* wf, int entity );
    
    void Rasterize( uint8_t* data,
unsigned int width, unsigned int height,
meters_t cellwidth, meters_t cellheight );
    
    BlockGroup* group; ///< The BlockGroup to which this Block belongs.
  private:
    std::vector<point_t> pts; ///< points defining a polygon.
    Bounds local_z; ///< z extent in local coords.
    Bounds global_z; ///< z extent in global coordinates.

    /** record the cells into which this block has been rendered so we
can remove them very quickly. One vector for each of the two
bitmap layers.*/
    std::vector<Cell*> rendered_cells[2];

    void DrawTop();
    void DrawSides();
  };


  class BlockGroup
  {
    friend class Model;
    friend class Block;
    friend class World;
    friend class SuperRegion;

  private:
    std::vector<Block> blocks; ///< Contains the blocks in this group.
    int displaylist; ///< OpenGL displaylist that renders this blockgroup.

  public: Model& mod;

  private:
    void AppendBlock( const Block& block );

    void CalcSize();
    void Clear() ; /** deletes all blocks from the group */

    void AppendTouchingModels( std::set<Model*>& touchers );

    /** Returns a pointer to the first model detected to be colliding
with a block in this group, or NULL, if none are detected. */
    Model* TestCollision();
 
    /** Renders all blocks into the bitmap at the indicated layer.*/
    void Map( unsigned int layer );
    /** Removes all blocks from the bitmap at the indicated layer.*/
    void UnMap( unsigned int layer );

    /** Interpret the bitmap file as a set of rectangles and add them
as blocks to this group.*/
    void LoadBitmap( const std::string& bitmapfile, Worldfile *wf );

    /** Add a new block decribed by a worldfile entry. */
    void LoadBlock( Worldfile* wf, int entity );
    
    /** Render the blockgroup as a bitmap image. */
    void Rasterize( uint8_t* data,
unsigned int width, unsigned int height,
meters_t cellwidth, meters_t cellheight );

    /** Draw the block in OpenGL as a solid single color. */
    void DrawSolid( const Geom &geom);

    /** Re-create the display list for drawing this blockgroup. This
is required whenever a member block or the owning model
changes its appearance.*/
    void BuildDisplayList();

    /** Draw the blockgroup from the cached displaylist. */
    void CallDisplayList();

  public:
    BlockGroup( Model& mod );
    ~BlockGroup();
    
    uint32_t GetCount() const { return blocks.size(); };
    const Block& GetBlock( unsigned int index ) const { return blocks[index]; };
    Block& GetBlockMutable( unsigned int index ) { return blocks[index]; };

    /** Return the extremal points of all member blocks in all three axes. */
    bounds3d_t BoundingBox() const;

    /** Draw the projection of the block group onto the z=0 plane. */
    void DrawFootPrint( const Geom &geom);
  };

  class Camera
  {
  protected:
    double _pitch; //left-right (about y)
    double _yaw; //up-down (about x)
    double _x, _y, _z;

  public:
    Camera() : _pitch( 0 ), _yaw( 0 ), _x( 0 ), _y( 0 ), _z( 0 ) { }
    virtual ~Camera() { }

    virtual void Draw( void ) const = 0;
    virtual void SetProjection( void ) const = 0;

    double yaw( void ) const { return _yaw; }
    double pitch( void ) const { return _pitch; }

    double x( void ) const { return _x; }
    double y( void ) const { return _y; }
    double z( void ) const { return _z; }

    virtual void reset() = 0;
    virtual void Load( Worldfile* wf, int sec ) = 0;

    //TODO data should be passed in somehow else. (at least min/max stuff)
    //virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const = 0;
  };

  class PerspectiveCamera : public Camera
  {
  private:
    double _z_near;
    double _z_far;
    double _vert_fov;
    double _horiz_fov;
    double _aspect;

  public:
    PerspectiveCamera( void );

    virtual void Draw( void ) const;
    virtual void SetProjection( void ) const;
    //void SetProjection( double aspect ) const;
    void update( void );

    void strafe( double amount );
    void forward( double amount );

    void setPose( double x, double y, double z ) { _x = x; _y = y; _z = z; }
    void addPose( double x, double y, double z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; }
    void move( double x, double y, double z );
    void setFov( double horiz_fov, double vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; }
    ///update vertical fov based on window aspect and current horizontal fov
    void setAspect( double aspect ) { _aspect = aspect; }
    void setYaw( double yaw ) { _yaw = yaw; }
    double horizFov( void ) const { return _horiz_fov; }
    double vertFov( void ) const { return _vert_fov; }
    void addYaw( double yaw ) { _yaw += yaw; }
    void setPitch( double pitch ) { _pitch = pitch; }
    void addPitch( double pitch ) { _pitch += pitch; if( _pitch < 0 ) _pitch = 0; else if( _pitch > 180 ) _pitch = 180; }

    double realDistance( double z_buf_val ) const {
      return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) );
    }
    void scroll( double dy ) { _z += dy; }
    double nearClip( void ) const { return _z_near; }
    double farClip( void ) const { return _z_far; }
    void setClip( double near, double far ) { _z_far = far; _z_near = near; }

    void reset() { setPitch( 70 ); setYaw( 0 ); }

    void Load( Worldfile* wf, int sec );
    void Save( Worldfile* wf, int sec );
  };
  
  class OrthoCamera : public Camera
  {
  private:
    double _scale;
    double _pixels_width;
    double _pixels_height;
    double _y_min;
    double _y_max;
  
  public:
    OrthoCamera( void ) :
_scale( 15 ),
_pixels_width(0),
_pixels_height(0),
_y_min(0),
_y_max(0)
{ }

    virtual void Draw() const;

    virtual void SetProjection( double pixels_width,
double pixels_height,
double y_min,
double y_max );

    virtual void SetProjection( void ) const;

    void move( double x, double y );

    void setYaw( double yaw ) { _yaw = yaw; }

    void setPitch( double pitch ) { _pitch = pitch; }

    void addYaw( double yaw ) { _yaw += yaw; }

    void addPitch( double pitch ) {
      _pitch += pitch;
      if( _pitch > 90 )
_pitch = 90;
      else if( _pitch < 0 )
_pitch = 0;
    }
  
    void setScale( double scale ) { _scale = scale; }
    void setPose( double x, double y) { _x = x; _y = y; }
  
    void scale( double scale, double shift_x = 0, double h = 0, double shift_y = 0, double w = 0 );
    void reset( void ) { _pitch = _yaw = 0; }
  
    double scale() const { return _scale; }
  
    void Load( Worldfile* wf, int sec );
    void Save( Worldfile* wf, int sec );
  };


  /** Extends World to implement an FLTK / OpenGL graphical user
interface.
*/
  class WorldGui : public World, public Fl_Window
  {
    friend class Canvas;
    friend class ModelCamera;
    friend class Model;
    friend class Option;

  private:
      
    Canvas* canvas;
    std::vector<Option*> drawOptions;
    FileManager* fileMan; ///< Used to load and save worldfiles
    std::vector<usec_t> interval_log;

    /** Stage attempts to run this many times faster than real
time. If -1, Stage runs as fast as possible. */
    double speedup;

    Fl_Menu_Bar* mbar;
    OptionsDlg* oDlg;
    bool pause_time;

    /** The amount of real time elapsed between $timing_interval
timesteps. */
    usec_t real_time_interval;

    /** The current real time in microseconds. */
    usec_t real_time_now;

    /** The last recorded real time, sampled every $timing_interval
updates. */
    usec_t real_time_recorded;

    /** Number of updates between measuring elapsed real time. */
    uint64_t timing_interval;

    // static callback functions
    static void windowCb( Fl_Widget* w, WorldGui* wg );
    static void fileLoadCb( Fl_Widget* w, WorldGui* wg );
    static void fileSaveCb( Fl_Widget* w, WorldGui* wg );
    static void fileSaveAsCb( Fl_Widget* w, WorldGui* wg );
    static void fileExitCb( Fl_Widget* w, WorldGui* wg );
    static void viewOptionsCb( OptionsDlg* oDlg, WorldGui* wg );
    static void optionsDlgCb( OptionsDlg* oDlg, WorldGui* wg );
    static void helpAboutCb( Fl_Widget* w, WorldGui* wg );
    static void pauseCb( Fl_Widget* w, WorldGui* wg );
    static void onceCb( Fl_Widget* w, WorldGui* wg );
    static void fasterCb( Fl_Widget* w, WorldGui* wg );
    static void slowerCb( Fl_Widget* w, WorldGui* wg );
    static void realtimeCb( Fl_Widget* w, WorldGui* wg );
    static void fasttimeCb( Fl_Widget* w, WorldGui* wg );
    static void resetViewCb( Fl_Widget* w, WorldGui* wg );
    static void moreHelptCb( Fl_Widget* w, WorldGui* wg );

    // GUI functions
    bool saveAsDialog();
    bool closeWindowQuery();

    virtual void AddModel( Model* mod );

    void SetTimeouts();

  protected:

    virtual void PushColor( Color col );
    virtual void PushColor( double r, double g, double b, double a );
    virtual void PopColor();

    void DrawOccupancy() const;
    void DrawVoxels() const;

  public:

    WorldGui(int W,int H,const char*L=0);
    ~WorldGui();

    /** Forces the window to be redrawn, even if paused.*/
    virtual void Redraw( void );

    virtual std::string ClockString() const;
    virtual bool Update();
    virtual void Load( const std::string& filename );
    virtual void UnLoad();
    virtual bool Save( const char* filename );
    virtual bool IsGUI() const { return true; };
    virtual Model* RecentlySelectedModel() const;

    virtual void Start();
    virtual void Stop();

    usec_t RealTimeNow(void) const;
 
    void DrawBoundingBoxTree();
    
    Canvas* GetCanvas( void ) const { return canvas; }

    /** show the window - need to call this if you don't Load(). */
    void Show();

    /** Get human readable string that describes the current global energy state. */
    std::string EnergyString( void ) const;
    virtual void RemoveChild( Model* mod );

    bool IsTopView();
  };


  class StripPlotVis : public Visualizer
  {
  private:

    Model* mod;
    float* data;
    size_t len;
    size_t count;
    unsigned int index;
    float x,y,w,h,min,max;
    Color fgcolor, bgcolor;

  public:
    StripPlotVis( float x, float y, float w, float h,
size_t len,
Color fgcolor, Color bgcolor,
const char* name, const char* wfname );
    virtual ~StripPlotVis();
    virtual void Visualize( Model* mod, Camera* cam );
    void AppendValue( float value );
  };


  class PowerPack
  {
    friend class WorldGui;
    friend class Canvas;

  protected:

    class DissipationVis : public Visualizer
    {
    private:
      unsigned int columns, rows;
      meters_t width, height;

      std::vector<joules_t> cells;

      joules_t peak_value;
      double cellsize;

      static joules_t global_peak_value;

    public:
      DissipationVis( meters_t width,
meters_t height,
meters_t cellsize );

      virtual ~DissipationVis();
      virtual void Visualize( Model* mod, Camera* cam );

      void Accumulate( meters_t x, meters_t y, joules_t amount );
    } event_vis;


    StripPlotVis output_vis;
    StripPlotVis stored_vis;

    /** The model that owns this object */
    Model* mod;
    
    /** Energy stored */
    joules_t stored;

    /** Energy capacity */
    joules_t capacity;

    /** TRUE iff the device is receiving energy */
    bool charging;

    /** Energy dissipated */
    joules_t dissipated;

    // these are used to visualize the power draw
    usec_t last_time;
    joules_t last_joules;
    watts_t last_watts;

  public:
    static joules_t global_stored;
    static joules_t global_capacity;
    static joules_t global_dissipated;
    static joules_t global_input;

  public:
    PowerPack( Model* mod );
    ~PowerPack();

    /** OpenGL visualization of the powerpack state */
    void Visualize( Camera* cam );

    /** Returns the energy capacity minus the current amount stored */
    joules_t RemainingCapacity() const;

    /** Add to the energy store */
    void Add( joules_t j );

    /** Subtract from the energy store */
    void Subtract( joules_t j );

    /** Transfer some stored energy to another power pack */
    void TransferTo( PowerPack* dest, joules_t amount );

    double ProportionRemaining() const
    { return( stored / capacity ); }

    /** Print human-readable status on stdout, prefixed with the
argument string, or NULL */
    void Print( const char* prefix ) const
    {
      if( prefix )
printf( "%s", prefix );

      printf( "PowerPack %.2f/%.2f J\n", stored, capacity );
    }

    joules_t GetStored() const;
    joules_t GetCapacity() const;
    joules_t GetDissipated() const;
    void SetCapacity( joules_t j );
    void SetStored( joules_t j );

    /** Returns true iff the device received energy at the last timestep */
    bool GetCharging() const { return charging; }

    void ChargeStart(){ charging = true; }
    void ChargeStop(){ charging = false; }

    /** Lose energy as work or heat */
    void Dissipate( joules_t j );

    /** Lose energy as work or heat, and record the event */
    void Dissipate( joules_t j, const Pose& p );
  };

   
  /// %Model class
  class Model : public Ancestor
  {
    friend class Ancestor;
    friend class World;
    friend class World::Event;
    friend class WorldGui;
    friend class Canvas;
    friend class Block;
    friend class Region;
    friend class BlockGroup;
    friend class PowerPack;
    friend class Ray;
    friend class ModelFiducial;

  private:
    /** the number of models instatiated - used to assign unique sequential IDs */
    static uint32_t count;
    static std::map<id_t,Model*> modelsbyid;

    /** records if this model has been mapped into the world bitmap*/
    bool mapped;

    std::vector<Option*> drawOptions;
    const std::vector<Option*>& getOptions() const { return drawOptions; }

  protected:

    /** If true, the model always has at least one subscription, so
always runs. Defaults to false. */
    bool alwayson;

    BlockGroup blockgroup;

    /** Iff true, 4 thin blocks are automatically added to the model,
forming a solid boundary around the bounding box of the
model. */
    int boundary;

    /** container for a callback function and a single argument, so
they can be stored together in a list with a single pointer. */
  public:
    class cb_t
    {
    public:
      model_callback_t callback;
      void* arg;

      cb_t( model_callback_t cb, void* arg )
: callback(cb), arg(arg) {}

      cb_t( world_callback_t cb, void* arg )
: callback(NULL), arg(arg) { (void)cb; }

      cb_t() : callback(NULL), arg(NULL) {}

      /** for placing in a sorted container */
      bool operator<( const cb_t& other ) const
      {
if( callback == other.callback )
return( arg < other.arg );
//else
return ((void*)(callback)) < ((void*)(other.callback));
      }

      /** for searching in a sorted container */
      bool operator==( const cb_t& other ) const
      { return( callback == other.callback); }
    };

    class Flag
    {
    private:
      Color color;
      double size;
      int displaylist;

    public:
      void SetColor( const Color& col );
      void SetSize( double sz );

      Color GetColor(){ return color; }
      double GetSize(){ return size; }

      Flag( const Color& color, double size );
      Flag* Nibble( double portion );

      /** Draw the flag in OpenGl. Takes a quadric parameter to save
creating the quadric for each flag */
      void Draw( GLUquadric* quadric );
    };

    typedef enum {
      CB_FLAGDECR,
      CB_FLAGINCR,
      CB_GEOM,
      CB_INIT,
      CB_LOAD,
      CB_PARENT,
      CB_POSE,
      CB_SAVE,
      CB_SHUTDOWN,
      CB_STARTUP,
      CB_UPDATE,
      CB_VELOCITY,
      //CB_POSTUPDATE,
      __CB_TYPE_COUNT // must be the last entry: counts the number of types
    } callback_type_t;

  protected:
    /** A list of callback functions can be attached to any
address. When Model::CallCallbacks( void*) is called, the
callbacks are called.*/
    std::vector<std::set<cb_t> > callbacks;

    /** Default color of the model's blocks.*/
    Color color;

    /** This can be set to indicate that the model has new data that
may be of interest to users. This allows polling the model
instead of adding a data callback. */
    bool data_fresh;

    /** If set true, Update() is not called on this model. Useful
e.g. for temporarily disabling updates when dragging models
with the mouse.*/
    bool disabled;

    /** Container for Visualizers attached to this model. */
    std::list<Visualizer*> cv_list;

    /** Container for flags attached to this model. */
    std::list<Flag*> flag_list;

    /** Model the interaction between the model's blocks and the
surface they touch. @todo primitive at the moment */
    double friction;

    /** Specifies the the size of this model's bounding box, and the
offset of its local coordinate system wrt that its parent. */
    Geom geom;

    /** Records model state and functionality in the GUI, if used */
    class GuiState
    {
    public:
      bool grid;
      bool move;
      bool nose;
      bool outline;

      GuiState();
      GuiState& Load( Worldfile* wf, int wf_entity );
    } gui;

    bool has_default_block;

    /** unique process-wide identifier for this model */
    uint32_t id;
    usec_t interval; ///< time between updates in usec
    usec_t interval_energy; ///< time between updates of powerpack in usec
    usec_t last_update; ///< time of last update in us
    bool log_state; ///< iff true, model state is logged
    meters_t map_resolution;
    kg_t mass;

    /** Pointer to the parent of this model, possibly NULL. */
    Model* parent;

    /** The pose of the model in it's parents coordinate frame, or the
global coordinate frame is the parent is NULL. */
    Pose pose;

    /** Optional attached PowerPack, defaults to NULL */
    PowerPack* power_pack;

    /** list of powerpacks that this model is currently charging,
initially NULL. */
    std::list<PowerPack*> pps_charging;

    /** Visualize the most recent rasterization operation performed by this model */
    class RasterVis : public Visualizer
    {
    private:
      uint8_t* data;
      unsigned int width, height;
      meters_t cellwidth, cellheight;
      std::vector<point_t> pts;

    public:
      RasterVis();
      virtual ~RasterVis( void ){}
      virtual void Visualize( Model* mod, Camera* cam );

      void SetData( uint8_t* data,
unsigned int width,
unsigned int height,
meters_t cellwidth,
meters_t cellheight );

      int subs; //< the number of subscriptions to this model
      int used; //< the number of connections to this model

      void AddPoint( meters_t x, meters_t y );
      void ClearPts();

    } rastervis;

    bool rebuild_displaylist; ///< iff true, regenerate block display list before redraw
    std::string say_string; ///< if non-empty, this string is displayed in the GUI

    bool stack_children; ///< whether child models should be stacked on top of this model or not

    bool stall; ///< Set to true iff the model collided with something else
    int subs; ///< the number of subscriptions to this model
    /** Thread safety flag. Iff true, Update() may be called in
parallel with other models. Defaults to false for
safety. Derived classes can set it true in their constructor to
allow parallel Updates(). */
    bool thread_safe;

    /** Cache of recent poses, used to draw the trail. */
    class TrailItem
    {
    public:
      usec_t time;
      Pose pose;
      Color color;

      TrailItem()
: time(0), pose(), color(){}

      //TrailItem( usec_t time, Pose pose, Color color )
      //: time(time), pose(pose), color(color){}
    };

    /** a ring buffer for storing recent poses */
    std::vector<TrailItem> trail;

    /** current position in the ring buffer */
    unsigned int trail_index;

    /** The maxiumum length of the trail drawn. Default is 20, but can
be set in the world file using the tail_length model
property. */
    static unsigned int trail_length;

    /** Number of world updates between trail records. */
    static uint64_t trail_interval;

    /** Record the current pose in our trail. Delete the trail head if it is full. */
    void UpdateTrail();

    //model_type_t type;
    const std::string type;
    /** The index into the world's vector of event queues. Initially
-1, to indicate that it is not on a list yet. */
    unsigned int event_queue_num;
    bool used; ///< TRUE iff this model has been returned by GetUnusedModelOfType()

    watts_t watts;///< power consumed by this model

    /** If >0, this model can transfer energy to models that have
watts_take >0 */
    watts_t watts_give;

    /** If >0, this model can transfer energy from models that have
watts_give >0 */
    watts_t watts_take;

    Worldfile* wf;
    int wf_entity;
    World* world; // pointer to the world in which this model exists
    WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode

  public:
    
    virtual void SetToken( const std::string& str )
    {
      //printf( "Model::SetToken( %s )\n", str.c_str() );

      if( str.size() > 0 )
{
world->AddModelName( this, str );
Ancestor::SetToken( str );
}
      else
PRINT_ERR( "Model::SetToken() called with zero length string. Ignored." );
    }


    const std::string& GetModelType() const {return type;}
    std::string GetSayString(){return std::string(say_string);}

    /** Returns a pointer to the model identified by name, or NULL if
it doesn't exist in this model. */
    Model* GetChild( const std::string& name ) const;

    /** return the update interval in usec */
    usec_t GetInterval(){ return interval; }

    class Visibility
    {
    public:
      bool blob_return;
      int fiducial_key;
      int fiducial_return;
      bool gripper_return;
      bool obstacle_return;
      double ranger_return; // 0 - 1

      Visibility();

      Visibility& Load( Worldfile* wf, int wf_entity );
      void Save( Worldfile* wf, int wf_entity );
    } vis;

    usec_t GetUpdateInterval() const { return interval; }
    usec_t GetEnergyInterval() const { return interval_energy; }
    // usec_t GetPoseInterval() const { return interval_pose; }

    /** Render the model's blocks as an occupancy grid into the
preallocated array of width by height pixels */
    void Rasterize( uint8_t* data,
unsigned int width, unsigned int height,
meters_t cellwidth, meters_t cellheight );

  private:
    /** Private copy constructor declared but not defined, to make it
impossible to copy models. */
    explicit Model(const Model& original);

    /** Private assignment operator declared but not defined, to make it
impossible to copy models */
    Model& operator=(const Model& original);

  protected:

    /** Register an Option for pickup by the GUI. */
    void RegisterOption( Option* opt );

    void AppendTouchingModels( std::set<Model*>& touchers );

    /** Check to see if the current pose will yield a collision with
obstacles. Returns a pointer to the first entity we are in
collision with, or NULL if no collision exists. Recursively
calls TestCollision() on all descendents. */
    Model* TestCollision();
  
    void CommitTestedPose();

    void Map( unsigned int layer );

    /** Call Map on all layers */
    inline void Map(){ Map(0); Map(1); }

    void UnMap( unsigned int layer );

    /** Call UnMap on all layers */
    inline void UnMap(){ UnMap(0); UnMap(1); }

    void MapWithChildren( unsigned int layer );
    void UnMapWithChildren( unsigned int layer );
  
    // Find the root model, and map/unmap the whole tree.
    void MapFromRoot( unsigned int layer );
    void UnMapFromRoot( unsigned int layer );

    /** raytraces a single ray from the point and heading identified by
pose, in local coords */
    RaytraceResult Raytrace( const Pose &pose,
const meters_t range,
const ray_test_func_t func,
const void* arg,
const bool ztest )
    {
      return world->Raytrace( LocalToGlobal(pose),
range,
func,
this,
arg,
ztest );
    }
    
    /** raytraces multiple rays around the point and heading identified
by pose, in local coords */
    void Raytrace( const Pose &pose,
      const meters_t range,
      const radians_t fov,
      const ray_test_func_t func,
      const void* arg,
      const bool ztest,
std::vector<RaytraceResult>& results )
    {
      return world->Raytrace( LocalToGlobal(pose),
range,
fov,
func,
this,
arg,
ztest,
results );
    }
      
    virtual void UpdateCharge();

    static int UpdateWrapper( Model* mod, void* arg ){ mod->Update(); return 0; }

    /** Calls CallCallback( CB_UPDATE ) */
    void CallUpdateCallbacks( void );

    meters_t ModelHeight() const;

    void DrawBlocksTree();
    virtual void DrawBlocks();
    void DrawBoundingBox();
    void DrawBoundingBoxTree();
    virtual void DrawStatus( Camera* cam );
    void DrawStatusTree( Camera* cam );
  
    void DrawOriginTree();
    void DrawOrigin();
  
    void PushLocalCoords();
    void PopCoords();
  
    /** Draw the image stored in texture_id above the model */
    void DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width=1.0, double height=1.0 );
  
    virtual void DrawPicker();
    virtual void DataVisualize( Camera* cam );
    virtual void DrawSelected(void);

    void DrawTrailFootprint();
    void DrawTrailBlocks();
    void DrawTrailArrows();
    void DrawGrid();
    // void DrawBlinkenlights();
    void DataVisualizeTree( Camera* cam );
    void DrawFlagList();
    void DrawPose( Pose pose );

  public:
    virtual void PushColor( Color col ){ world->PushColor( col ); }
    virtual void PushColor( double r, double g, double b, double a ){ world->PushColor( r,g,b,a ); }
    virtual void PopColor() { world->PopColor(); }

    PowerPack* FindPowerPack() const;

    //void RecordRenderPoint( GSList** head, GSList* link,
    // unsigned int* c1, unsigned int* c2 );

    void PlaceInFreeSpace( meters_t xmin, meters_t xmax,
meters_t ymin, meters_t ymax );

    /** Return a human-readable string describing the model's pose */
    std::string PoseString()
    { return pose.String(); }

    /** Look up a model pointer by a unique model ID */
    static Model* LookupId( uint32_t id )
    { return modelsbyid[id]; }

    /** Constructor */
    Model( World* world,
Model* parent = NULL,
const std::string& type = "model",
const std::string& name = "" );

    /** Destructor */
    virtual ~Model();

    /** Alternate constructor that creates dummy models with only a pose */
Model()
: mapped(false), alwayson(false), blockgroup(*this),
boundary(false), data_fresh(false), disabled(true), friction(0), has_default_block(false), log_state(false), map_resolution(0), mass(0), parent(NULL), rebuild_displaylist(false), stack_children(true), stall(false), subs(0), thread_safe(false),trail_index(0), event_queue_num(0), used(false), watts(0), watts_give(0),watts_take(0),wf(NULL), wf_entity(0), world(NULL)
{}

    void Say( const std::string& str );

    /** Attach a user supplied visualization to a model. */
    void AddVisualizer( Visualizer* custom_visual, bool on_by_default );

    /** remove user supplied visualization to a model - supply the same ptr passed to AddCustomVisualizer */
    void RemoveVisualizer( Visualizer* custom_visual );

    void BecomeParentOf( Model* child );

    void Load( Worldfile* wf, int wf_entity )
    {
      /** Set the worldfile and worldfile entity ID - must be called
before Load() */
      SetWorldfile( wf, wf_entity );
      Load(); // call virtual load
    }

    /** Set the worldfile and worldfile entity ID */
    void SetWorldfile( Worldfile* wf, int wf_entity )
    { this->wf = wf; this->wf_entity = wf_entity; }

    /** configure a model by reading from the current world file */
    virtual void Load();

    /** save the state of the model to the current world file */
    virtual void Save();

    /** Call Init() for all attached controllers. */
    void InitControllers();

    void AddFlag( Flag* flag );
    void RemoveFlag( Flag* flag );

    void PushFlag( Flag* flag );
    Flag* PopFlag();

    unsigned int GetFlagCount() const { return flag_list.size(); }
    
    /** Disable the model. Its pose will not change due to velocity
until re-enabled using Enable(). This is used for example when
dragging a model with the mouse pointer. The model is enabled by
default. */
    void Disable(){ disabled = true; };

    /** Enable the model, so that non-zero velocities will change the
model's pose. Models are enabled by default. */
    void Enable(){ disabled = false; };
  
    /** Load a control program for this model from an external
library */
    void LoadControllerModule( const char* lib );

    /** Sets the redraw flag, so this model will be redrawn at the
earliest opportunity */
    void NeedRedraw();

    /** Force the GUI (if any) to redraw this model */
    void Redraw();

    /** Add a block to this model by loading it from a worldfile
entity */
    void LoadBlock( Worldfile* wf, int entity );

    /** Add a block to this model centered at [x,y] with extent [dx, dy,
dz] */
    void AddBlockRect( meters_t x, meters_t y,
meters_t dx, meters_t dy,
meters_t dz );

    /** remove all blocks from this model, freeing their memory */
    void ClearBlocks();
  
    /** Returns a pointer to this model's parent model, or NULL if this
model has no parent */
    Model* Parent() const { return this->parent; }

    /** Returns a pointer to the world that contains this model */
    World* GetWorld() const { return this->world; }
  
    /** return the root model of the tree containing this model */
    Model* Root(){ return( parent ? parent->Root() : this ); }
  
    bool IsAntecedent( const Model* testmod ) const;

    /** returns true if model [testmod] is a descendent of this model */
    bool IsDescendent( const Model* testmod ) const;

    /** returns true if model [testmod] is a descendent or antecedent of this model */
    bool IsRelated( const Model* testmod ) const;

    /** get the pose of a model in the global CS */
    Pose GetGlobalPose() const;

    /** subscribe to a model's data */
    void Subscribe();

    /** unsubscribe from a model's data */
    void Unsubscribe();

    /** set the pose of model in global coordinates */
    void SetGlobalPose( const Pose& gpose );

    /** set a model's pose in its parent's coordinate system */
    void SetPose( const Pose& pose );

    /** add values to a model's pose in its parent's coordinate system */
    void AddToPose( const Pose& pose );

    /** add values to a model's pose in its parent's coordinate system */
    void AddToPose( double dx, double dy, double dz, double da );

    /** set a model's geometry (size and center offsets) */
    void SetGeom( const Geom& src );
  
    /** Set a model's fiducial return value. Values less than zero
are not detected by the fiducial sensor. */
    void SetFiducialReturn( int fid );
  
    /** Get a model's fiducial return value. */
    int GetFiducialReturn() const { return vis.fiducial_return; }
  
    /** set a model's fiducial key: only fiducial finders with a
matching key can detect this model as a fiducial. */
    void SetFiducialKey( int key );

    Color GetColor() const { return color; }

    /** return a model's unique process-wide identifier */
    uint32_t GetId() const { return id; }

    /** Get the total mass of a model and it's children recursively */
    kg_t GetTotalMass() const;

    /** Get the mass of this model's children recursively. */
    kg_t GetMassOfChildren() const;

    /** Change a model's parent - experimental*/
    int SetParent( Model* newparent);

    /** Get (a copy of) the model's geometry - it's size and local
pose (offset from origin in local coords). */
    Geom GetGeom() const { return geom; }

    /** Get (a copy of) the pose of a model in its parent's coordinate
system. */
    Pose GetPose() const { return pose; }


    // guess what these do?
    void SetColor( Color col );
    void SetMass( kg_t mass );
    void SetStall( bool stall );
    void SetGravityReturn( bool val );
    void SetGripperReturn( bool val );
    void SetStickyReturn( bool val );
    void SetRangerReturn( double val );
    void SetObstacleReturn( bool val );
    void SetBlobReturn( bool val );
    void SetRangerReturn( bool val );
    void SetBoundary( bool val );
    void SetGuiNose( bool val );
    void SetGuiMove( bool val );
    void SetGuiGrid( bool val );
    void SetGuiOutline( bool val );
    void SetWatts( watts_t watts );
    void SetMapResolution( meters_t res );
    void SetFriction( double friction );

    bool DataIsFresh() const { return this->data_fresh; }

    /* attach callback functions to data members. The function gets
called when the member is changed using SetX() accessor method */

    /** Add a callback. The specified function is called whenever the
indicated model method is called, and passed the user
data. @param cb Pointer the function to be called. @param
user Pointer to arbitrary user data, passed to the callback
when called.
*/
    void AddCallback( callback_type_t type,
model_callback_t cb,
void* user );

    int RemoveCallback( callback_type_t type,
model_callback_t callback );

    int CallCallbacks( callback_type_t type );


    virtual void Print( char* prefix ) const;
    virtual const char* PrintWithPose() const;

    /** Given a global pose, returns that pose in the model's local
coordinate system. */
    Pose GlobalToLocal( const Pose& pose ) const;

    /** Return the global pose (i.e. pose in world coordinates) of a
pose specified in the model's local coordinate system */
    Pose LocalToGlobal( const Pose& pose ) const
    {
      return( ( GetGlobalPose() + geom.pose ) + pose );
    }
    
    /** Return a vector of global pixels corresponding to a vector of local points. */
    std::vector<point_int_t> LocalToPixels( const std::vector<point_t>& local ) const;

    /** Return the 2d point in world coordinates of a 2d point
specified in the model's local coordinate system */
    point_t LocalToGlobal( const point_t& pt) const;

    /** returns the first descendent of this model that is unsubscribed
and has the type indicated by the string */
    Model* GetUnsubscribedModelOfType( const std::string& type ) const;

    /** returns the first descendent of this model that is unused
and has the type indicated by the string. This model is tagged as used. */
    Model* GetUnusedModelOfType( const std::string& type );
  
    /** Returns the value of the model's stall boolean, which is true
iff the model has crashed into another model */
    bool Stalled() const { return this->stall; }

    /** Returns the current number of subscriptions. If alwayson, this
is never less than 1.*/
    unsigned int GetSubscriptionCount() const { return subs; }

    /** Returns true if the model has one or more subscribers, else false. */
    bool HasSubscribers() const { return( subs > 0 ); }

    static std::map< std::string, creator_t> name_map;

  protected:
    virtual void Startup();
    virtual void Shutdown();
    virtual void Update();
  };


  // BLOBFINDER MODEL --------------------------------------------------------
  /// %ModelBlobfinder class
  class ModelBlobfinder : public Model
  {
  public:
    /** Sample data */
    class Blob
    {
    public:
      Color color;
      uint32_t left, top, right, bottom;
      meters_t range;
    };

    /** Visualize blobinder data in the GUI. */
    class Vis : public Visualizer
    {
    public:
      Vis( World* world );
      virtual ~Vis( void ){}
      virtual void Visualize( Model* mod, Camera* cam );
    } vis;

  private:
    /** Vector of blobs detected in the field of view. Use GetBlobs()
or GetBlobsMutable() to acess the vector. */
    std::vector<Blob> blobs;

    /** The sensor detects model blocks of each of these colors. Use
AddColor() and RemoveColor()
to add and remove colors at run time.*/
    std::vector<Color> colors;

    // predicate for ray tracing
    static bool BlockMatcher( Block* testblock, Model* finder );

  public:
    radians_t fov; ///< Horizontal field of view in radians, in the range 0 to pi.
    radians_t pan; ///< Horizontal pan angle in radians, in the range -pi to +pi.
    meters_t range; ///< Maximum distance at which blobs are detected (a little artificial, but setting this small saves computation time.
    unsigned int scan_height; ///< Height of the input image in pixels.
    unsigned int scan_width; ///< Width of the input image in pixels.

    // constructor
    ModelBlobfinder( World* world,
Model* parent,
const std::string& type );
    // destructor
    ~ModelBlobfinder();

    virtual void Startup();
    virtual void Shutdown();
    virtual void Update();
    virtual void Load();

    /** Returns a non-mutable const reference to the detected blob
data. Use this if you don't need to modify the model's
internal data, e.g. if you want to copy it into a new
vector.*/
    const std::vector<Blob>& GetBlobs() const { return blobs; }

    /** Returns a mutable reference to the model's internal detected
blob data. Use this with caution, if at all. */
    std::vector<Blob>& GetBlobsMutable() { return blobs; }

    /** Start finding blobs with this color.*/
    void AddColor( Color col );

    /** Stop tracking blobs with this color */
    void RemoveColor( Color col );

    /** Stop tracking all colors. Call this to clear the defaults, then
add colors individually with AddColor(); */
    void RemoveAllColors();
  };



  

  // Light indicator model
  class ModelLightIndicator : public Model
  {
  public:
    ModelLightIndicator( World* world,
Model* parent,
const std::string& type );
    ~ModelLightIndicator();
  
    void SetState(bool isOn);

  protected:
    virtual void DrawBlocks();

  private:
    bool m_IsOn;
  };

  // \todo GRIPPER MODEL --------------------------------------------------------


  class ModelGripper : public Model
  {
  public:

    enum paddle_state_t {
      PADDLE_OPEN = 0, // default state
      PADDLE_CLOSED,
      PADDLE_OPENING,
      PADDLE_CLOSING,
    };

    enum lift_state_t {
      LIFT_DOWN = 0, // default state
      LIFT_UP,
      LIFT_UPPING, // verbed these to match the paddle state
      LIFT_DOWNING,
    };

    enum cmd_t {
      CMD_NOOP = 0, // default state
      CMD_OPEN,
      CMD_CLOSE,
      CMD_UP,
      CMD_DOWN
    };


    /** gripper configuration
*/
    struct config_t
    {
      Size paddle_size; ///< paddle dimensions
      paddle_state_t paddles;
      lift_state_t lift;
      double paddle_position; ///< 0.0 = full open, 1.0 full closed
      double lift_position; ///< 0.0 = full down, 1.0 full up
      Model* gripped;
      bool paddles_stalled; // true iff some solid object stopped the paddles closing or opening
      double close_limit; ///< How far the gripper can close. If < 1.0, the gripper has its mouth full.
      bool autosnatch; ///< if true, cycle the gripper through open-close-up-down automatically
      double break_beam_inset[2]; ///< distance from the end of the paddle
      Model* beam[2]; ///< points to a model detected by the beams
      Model* contact[2]; ///< pointers to a model detected by the contacts
    };

  private:
    virtual void Update();
    virtual void DataVisualize( Camera* cam );

    void FixBlocks();
    void PositionPaddles();
    void UpdateBreakBeams();
    void UpdateContacts();

    config_t cfg;
    cmd_t cmd;

    Block* paddle_left;
    Block* paddle_right;

    static Option showData;

  public:
    static const Size size;

    // constructor
    ModelGripper( World* world,
Model* parent,
const std::string& type );
    // destructor
    virtual ~ModelGripper();
  
    virtual void Load();
    virtual void Save();

    /** Configure the gripper */
    void SetConfig( config_t & newcfg ){ this->cfg = newcfg; FixBlocks(); }

    /** Returns the state of the gripper .*/
    config_t GetConfig(){ return cfg; };

    /** Set the current activity of the gripper. */
    void SetCommand( cmd_t cmd ) { this->cmd = cmd; }
    /** Command the gripper paddles to close. Wrapper for SetCommand( CMD_CLOSE ). */
    void CommandClose() { SetCommand( CMD_CLOSE ); }
    /** Command the gripper paddles to open. Wrapper for SetCommand( CMD_OPEN ). */
    void CommandOpen() { SetCommand( CMD_OPEN ); }
    /** Command the gripper lift to go up. Wrapper for SetCommand( CMD_UP ). */
    void CommandUp() { SetCommand( CMD_UP ); }
    /** Command the gripper lift to go down. Wrapper for SetCommand( CMD_DOWN ). */
    void CommandDown() { SetCommand( CMD_DOWN ); }
  };



  // BUMPER MODEL --------------------------------------------------------
  /// %ModelBumper class
  class ModelBumper : public Model
  {
  public:
    class BumperConfig
    {
     public:
     Pose pose;
     meters_t length;
    };
  
   class BumperSample
   {
    public:
    Model* hit;
    point_t hit_point;
   };
  
  
  public:
    ModelBumper( World* world,
    Model* parent,
    const std::string& type );
    virtual ~ModelBumper();

    virtual void Load();
    
    uint32_t bumper_count;
    BumperConfig* bumpers;
    BumperSample* samples;
    
  protected:
    virtual void Startup();
    virtual void Shutdown();
    virtual void Update();
    virtual void Print (char *prefix);
    
    class BumperVis : public Visualizer
    {
    public:
      BumperVis();
      virtual ~BumperVis();
      virtual void Visualize( Model* mod, Camera* cam);
    } bumpervis;
    
  private:
   static Option showBumperData;
  };

  // FIDUCIAL MODEL --------------------------------------------------------

  /// %ModelFiducial class
  class ModelFiducial : public Model
  {
  public:
    /** Detected fiducial data */
    class Fiducial
    {
    public:
      meters_t range; ///< range to the target
      radians_t bearing; ///< bearing to the target
      Pose geom; ///< size and relative angle of the target
//Pose pose_rel; /// relative pose of the target in local coordinates
      Pose pose; ///< Absolute accurate position of the target in world coordinates (it's cheating to use this in robot controllers!)
      Model* mod; ///< Pointer to the model (real fiducial detectors can't do this!)
      int id; ///< the fiducial identifier of the target (i.e. its fiducial_return value), or -1 if none can be detected.
    };

  private:
    // if neighbor is visible, add him to the fiducial scan
    void AddModelIfVisible( Model* him );

    virtual void Update();
    virtual void DataVisualize( Camera* cam );

    static Option showData;
    static Option showFov;

    std::vector<Fiducial> fiducials;

  public:
    ModelFiducial( World* world,
Model* parent,
const std::string& type );
    virtual ~ModelFiducial();

    virtual void Load();
    void Shutdown( void );

    meters_t max_range_anon;///< maximum detection range
    meters_t max_range_id; ///< maximum range at which the ID can be read
    meters_t min_range; ///< minimum detection range
    radians_t fov; ///< field of view
    radians_t heading; ///< center of field of view
    int key; ///< /// only detect fiducials with a key that matches this one (defaults 0)
    bool ignore_zloc; ///< Are we ignoring the Z-loc of the fiducials we detect compared to the fiducial detector?

    /** Access the dectected fiducials. C++ style. */
    std::vector<Fiducial>& GetFiducials() { return fiducials; }

    /** Access the dectected fiducials, C style. */
    Fiducial* GetFiducials( unsigned int* count )
    {
      if( count ) *count = fiducials.size();
      return &fiducials[0];
    }
  };


  // RANGER MODEL --------------------------------------------------------

  /// %ModelRanger class
  class ModelRanger : public Model
  {
  public:
  public:
    ModelRanger( World* world, Model* parent, const std::string& type );
    virtual ~ModelRanger();

    virtual void Print( char* prefix ) const;

    class Vis : public Visualizer
    {
    public:
      static Option showArea;
      static Option showStrikes;
      static Option showFov;
      static Option showBeams;
      static Option showTransducers;

      Vis( World* world );
      virtual ~Vis( void ){}
      virtual void Visualize( Model* mod, Camera* cam );
    } vis;

    class Sensor
    {
    public:
      Pose pose;
      Size size;
      Bounds range;
      radians_t fov;
      unsigned int sample_count;
      Color color;

      std::vector<meters_t> ranges;
      std::vector<double> intensities;
      std::vector<double> bearings;

      Sensor() : pose( 0,0,0,0 ),
size( 0.02, 0.02, 0.02 ), // teeny transducer
range( 0.0, 5.0 ),
fov( 0.1 ),
sample_count(1),
color( Color(0,0,1,0.15)),
ranges(),
intensities(),
bearings()
      {}

      void Update( ModelRanger* rgr );
      void Visualize( Vis* vis, ModelRanger* rgr ) const;
      std::string String() const;
      void Load( Worldfile* wf, int entity );
    };

    /** returns a const reference to a vector of range and reflectance samples */
    const std::vector<Sensor>& GetSensors() const
    { return sensors; }

    /** returns a mutable reference to a vector of range and reflectance samples */
    std::vector<Sensor>& GetSensorsMutable()
    { return sensors; }

    void LoadSensor( Worldfile* wf, int entity );

  private:
    std::vector<Sensor> sensors;
    
  protected:

    virtual void Startup();
    virtual void Shutdown();
    virtual void Update();
  };

  // BLINKENLIGHT MODEL ----------------------------------------------------
  class ModelBlinkenlight : public Model
  {
  private:
    double dutycycle;
    bool enabled;
    msec_t period;
    bool on;

    static Option showBlinkenData;
  public:
    ModelBlinkenlight( World* world,
Model* parent,
const std::string& type );

    ~ModelBlinkenlight();

    virtual void Load();
    virtual void Update();
    virtual void DataVisualize( Camera* cam );
  };


  // CAMERA MODEL ----------------------------------------------------

  /// %ModelCamera class
  class ModelCamera : public Model
  {
  public:
    typedef struct
    {
      // GL_V3F
      GLfloat x, y, z;
    } ColoredVertex;
  
  private:
    Canvas* _canvas;

    GLfloat* _frame_data; //opengl read buffer
    GLubyte* _frame_color_data; //opengl read buffer

    bool _valid_vertexbuf_cache;
    ColoredVertex* _vertexbuf_cache; //cached unit vectors with appropriate rotations (these must be scalled by z-buffer length)

    int _width; //width of buffer
    int _height; //height of buffer
    static const int _depth = 4;

    int _camera_quads_size;
    GLfloat* _camera_quads;
    GLubyte* _camera_colors;

    static Option showCameraData;

    PerspectiveCamera _camera;
    double _yaw_offset; //position camera is mounted at
    double _pitch_offset;

    ///Take a screenshot from the camera's perspective. return: true for sucess, and data is available via FrameDepth() / FrameColor()
    bool GetFrame();

  public:
    ModelCamera( World* world,
Model* parent,
const std::string& type );

    ~ModelCamera();
  
    virtual void Load();

    ///Capture a new frame ( calls GetFrame )
    virtual void Update();

    ///Draw Camera Model - TODO
    //virtual void Draw( uint32_t flags, Canvas* canvas );

    ///Draw camera visualization
    virtual void DataVisualize( Camera* cam );

    ///width of captured image
    int getWidth( void ) const { return _width; }

    ///height of captured image
    int getHeight( void ) const { return _height; }

    ///get reference to camera used
    const PerspectiveCamera& getCamera( void ) const { return _camera; }

    ///get a reference to camera depth buffer
    const GLfloat* FrameDepth() const { return _frame_data; }

    ///get a reference to camera color image. 4 bytes (RGBA) per pixel
    const GLubyte* FrameColor() const { return _frame_color_data; }

    ///change the pitch
    void setPitch( double pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; }

    ///change the yaw
    void setYaw( double yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; }
  };

  // POSITION MODEL --------------------------------------------------------

  /// %ModelPosition class
  class ModelPosition : public Model
  {
    friend class Canvas;
    friend class World;

  public:
    /** Define a position control method */
    typedef enum
      { CONTROL_ACCELERATION,
CONTROL_VELOCITY,
CONTROL_POSITION
      } ControlMode;

    /** Define a localization method */
    typedef enum
      { LOCALIZATION_GPS,
LOCALIZATION_ODOM
      } LocalizationMode;

    /** Define a driving method */
    typedef enum
      { DRIVE_DIFFERENTIAL,
DRIVE_OMNI,
DRIVE_CAR
      } DriveMode;

  private:
    Velocity velocity;
    Pose goal;///< the current velocity or pose to reach, depending on the value of control_mode
    ControlMode control_mode;
    DriveMode drive_mode;
    LocalizationMode localization_mode; ///< global or local mode
    Velocity integration_error; ///< errors to apply in simple odometry model
    double wheelbase;
    
  public:
    /** Set the min and max acceleration in all 4 DOF */
    Bounds acceleration_bounds[4];
    
    /** Set the min and max velocity in all 4 DOF */
    Bounds velocity_bounds[4];

    // constructor
    ModelPosition( World* world,
Model* parent,
const std::string& type );
    // destructor
    ~ModelPosition();

    /** Get (a copy of) the model's velocity in its local reference
frame. */
    Velocity GetVelocity() const { return velocity; }
    void SetVelocity( const Velocity& val );
    /** get the velocity of a model in the global CS */
    Velocity GetGlobalVelocity() const;
    /* set the velocity of a model in the global coordinate system */
    void SetGlobalVelocity( const Velocity& gvel );

    /** Specify a point in space. Arrays of Waypoints can be attached to
Models and visualized. */
    class Waypoint
    {
    public:
      Waypoint( meters_t x, meters_t y, meters_t z, radians_t a, Color color ) ;
      Waypoint( const Pose& pose, Color color ) ;
      Waypoint();
      void Draw() const;

      Pose pose;
      Color color;
    };

    std::vector<Waypoint> waypoints;

    class WaypointVis : public Visualizer
    {
    public:
      WaypointVis();
      virtual ~WaypointVis( void ){}
      virtual void Visualize( Model* mod, Camera* cam );
    } wpvis;

    class PoseVis : public Visualizer
    {
    public:
      PoseVis();
      virtual ~PoseVis( void ){}
      virtual void Visualize( Model* mod, Camera* cam );
    } posevis;

    /** Set the current pose estimate.*/
    void SetOdom( Pose odom );

    /** Sets the control_mode to CONTROL_VELOCITY and sets
the goal velocity. */
    void SetSpeed( double x, double y, double a );
    void SetXSpeed( double x );
    void SetYSpeed( double y );
    void SetZSpeed( double z );
    void SetTurnSpeed( double a );
    void SetSpeed( Velocity vel );
    /** Set velocity along all axes to to zero. */
    void Stop();

    /** Sets the control mode to CONTROL_POSITION and sets
the goal pose */
    void GoTo( double x, double y, double a );
    void GoTo( Pose pose );
    
    /** Sets the control mode to CONTROL_ACCELERATION and sets the
current accelerations to x, y (meters per second squared) and
a (radians per second squared) */
    void SetAcceleration( double x, double y, double a );

    // localization state
    Pose est_pose; ///< position estimate in local coordinates
    Pose est_pose_error; ///< estimated error in position estimate
    Pose est_origin; ///< global origin of the local coordinate system

  protected:
    virtual void Move();
    virtual void Startup();
    virtual void Shutdown();
    virtual void Update();
    virtual void Load();
  };


  // ACTUATOR MODEL --------------------------------------------------------

  /// %ModelActuator class
  class ModelActuator : public Model
  {
  public:
    /** Define a actuator control method */
    typedef enum
      { CONTROL_VELOCITY,
CONTROL_POSITION
      } ControlMode;
  
    /** Define an actuator type */
    typedef enum
      { TYPE_LINEAR,
TYPE_ROTATIONAL
      } ActuatorType;
  
  private:
    double goal; //< the current velocity or pose to reach depending on the value of control_mode
    double pos;
    double max_speed;
    double min_position;
    double max_position;
    double start_position;
    double cosa;
    double sina;
    ControlMode control_mode;
    ActuatorType actuator_type;
    point3_t axis;
  
    Pose InitialPose;
  public:
    // constructor
    ModelActuator( World* world,
Model* parent,
const std::string& type );
    // destructor
    ~ModelActuator();
  
    virtual void Startup();
    virtual void Shutdown();
    virtual void Update();
    virtual void Load();
  
    /** Sets the control_mode to CONTROL_VELOCITY and sets
the goal velocity. */
    void SetSpeed( double speed );
  
    double GetSpeed() const {return goal;}
  
    /** Sets the control mode to CONTROL_POSITION and sets
the goal pose */
    void GoTo( double pose );
  
    double GetPosition() const {return pos;};
    double GetMaxPosition() const {return max_position;};
    double GetMinPosition() const {return min_position;};

    ActuatorType GetType() const { return actuator_type; }
    point3_t GetAxis() const { return axis; }
  };


}; // end namespace stg

#endif
Something went wrong with that request. Please try again.