diff --git a/CMakeLists.txt b/CMakeLists.txt
index d1845e6bc..1cc95beb0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,7 +15,7 @@ OPTION (BUILD_LSPTEST "Build Player plugin tests" OFF)
 OPTION (CPACK_CFG "[release building] generate CPack configuration files" ON)
 
 # todo - this doesn't work yet. Run Stage headless with -g.
-# OPTION (BUILD_GUI "Build FLTK-based GUI. If OFF, build a gui-less Stage useful e.g. for headless compute clusters." ON ) 
+# OPTION (BUILD_GUI "Build FLTK-based GUI. If OFF, build a gui-less Stage useful e.g. for headless compute clusters." ON )
 
 cmake_minimum_required( VERSION 2.4 FATAL_ERROR )
 
@@ -56,7 +56,7 @@ SET (CMAKE_CXX_FLAGS_PROFILE " -ggdb -pg  ${FORCE_ARCH} ${WALL} " CACHE INTERNAL
 #####################################
 # Set the default build type
 IF (NOT CMAKE_BUILD_TYPE)
-  SET (CMAKE_BUILD_TYPE "release" CACHE STRING 
+  SET (CMAKE_BUILD_TYPE "release" CACHE STRING
     "Choose the type of build, options are: release (default) debug profile" FORCE)
 ENDIF (NOT CMAKE_BUILD_TYPE)
 STRING(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE)
@@ -67,16 +67,12 @@ ENABLE_TESTING()
 
 SET(RGBFILE ${CMAKE_INSTALL_PREFIX}/share/stage/rgb.txt )
 
-# Create the config.h file
-# config.h belongs with the source (and not in CMAKE_CURRENT_BINARY_DIR as in Brian's original version)
-CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/config.h.in 
-					${CMAKE_CURRENT_SOURCE_DIR}/config.h @ONLY)
 
 message( STATUS "Checking for libtool" )
 find_path( LTDL_INCLUDE_DIR ltdl.h DOC "Libtool include dir" )
 find_library( LTDL_LIB ltdl DOC "Libtool lib" )
 
-include_directories( 
+include_directories(
 	${OPENGL_INCLUDE_DIR}
 	${LTDL_INCLUDE_DIR}
 )
@@ -93,7 +89,7 @@ find_package( PNG REQUIRED )
 # SET( PNG_LIBRARIES /opt/X11/lib/libpng.dylib )
 # SET( PNG_INCLUDE_DIR /opt/X11/include )
 
-set (FLTK_SKIP_FLUID TRUE) 
+set (FLTK_SKIP_FLUID TRUE)
 find_package( FLTK REQUIRED )
 find_package( OpenGL REQUIRED )
 
@@ -103,22 +99,74 @@ ENDIF( NOT OPENGL_GLU_FOUND )
 
 SET( INDENT "  * " )
 # MESSAGE( STATUS ${INDENT} "JPEG_INCLUDE_DIR = ${JPEG_INCLUDE_DIR}" )
-# MESSAGE( STATUS ${INDENT} "JPEG_LIBRARIES = ${JPEG_LIBRARIES}" ) 
+# MESSAGE( STATUS ${INDENT} "JPEG_LIBRARIES = ${JPEG_LIBRARIES}" )
 # MESSAGE( STATUS ${INDENT} "PNG_INCLUDE_DIR = ${PNG_INCLUDE_DIR}" )
-# MESSAGE( STATUS ${INDENT} "PNG_LIBRARIES = ${PNG_LIBRARIES}" ) 
+# MESSAGE( STATUS ${INDENT} "PNG_LIBRARIES = ${PNG_LIBRARIES}" )
 # MESSAGE( STATUS ${INDENT} "FLTK_LIBRARIES=${FLTK_LIBRARIES}" )
 # MESSAGE( STATUS ${INDENT} "FLTK_INCLUDE_DIR=${FLTK_INCLUDE_DIR}" )
 # MESSAGE( STATUS ${INDENT} "OpenGL GLU found at ${OPENGL_INCLUDE_DIR}" )
 # MESSAGE( STATUS "    OPENGL_INCLUDE_DIR = ${OPENGL_INCLUDE_DIR}")
 # MESSAGE( STATUS "    OPENGL_glu_LIBRARY = ${OPENGL_glu_LIBRARY}")
 
+
+# Try to find BOOST
+message( STATUS "Checking for Boost" )
+
+OPTION (Boost_USE_STATIC_LIBS "Use the static versions of the Boost libraries" ON)
+MARK_AS_ADVANCED (Boost_USE_STATIC_LIBS)
+OPTION (USE_BOOST_RANDOM "Use the Boost random number generator library" ON)
+MARK_AS_ADVANCED (USE_BOOST_RANDOM)
+
+#If Boost is to be used, then check fo rit
+IF (USE_BOOST_RANDOM)
+  SET (BOOST_COMPONENTS random)
+  # FIND_PACKAGE (Boost COMPONENTS ${BOOST_COMPONENTS})
+  FIND_PACKAGE (Boost)
+  IF (Boost_FOUND)
+    include_directories(${Boost_INCLUDE_DIRS})
+    SET (HAVE_BOOST_RANDOM TRUE CACHE INTERNAL "Is the Boost::Random library present")
+
+    #  Note: I'm going to leave the stuff below in; it is used to compile against the boost
+    # libs; it seems just using the includes is fine enough though.
+    #IF (Boost_RANDOM_FOUND)
+    #  SET (HAVE_BOOST_RANDOM TRUE CACHE INTERNAL "Is the Boost::Random library present")
+    #  GET_FILENAME_COMPONENT (boostRandomLib ${Boost_RANDOM_LIBRARY} NAME_WE CACHE INTERNAL)
+    #  # Chop off the lib at the front, too, if present
+    #  STRING (REGEX REPLACE "^lib" "" boostRandomLib ${boostRandomLib})
+
+      MESSAGE( STATUS ${INDENT} "Boost Random found!")
+    #ELSE(Boost_RANDOM_FOUND)
+    #  SET (HAVE_BOOST_RANDOM FALSE)
+    #  MESSAGE( STATUS ${INDENT} "Boost Random NOT found!")
+    #ENDIF(Boost_RANDOM_FOUND)
+  ELSE(Boost_FOUND)
+    MESSAGE( STATUS ${INDENT} "Boost NOT found!")
+    SET (HAVE_BOOST_RANDOM FALSE )
+  ENDIF(Boost_FOUND)
+
+ELSE (USE_BOOST_RANDOM)
+  MESSAGE( STATUS ${INDENT} "Boost diabled" )
+  SET (HAVE_BOOST_RANDOM FALSE)
+  SET (boostRandomLib )
+ENDIF (USE_BOOST_RANDOM)
+
+
+
+
+# Create the config.h file
+# config.h belongs with the source (and not in CMAKE_CURRENT_BINARY_DIR as in Brian's original version)
+CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/config.h.in
+                   ${CMAKE_CURRENT_SOURCE_DIR}/config.h @ONLY)
+
+
+
 MESSAGE( STATUS "Checking for optional libraries..." )
 
 # Player does not have a CMake package, but does provide pkgconfig info
 
 include(FindPkgConfig)
-   
-pkg_search_module( PLAYER playercore>=2.1.0 ) 
+
+pkg_search_module( PLAYER playercore>=2.1.0 )
 IF( PLAYER_FOUND )
   MESSAGE( STATUS ${INDENT} "Player version ${PLAYER_VERSION} detected at ${PLAYER_PREFIX}" )
   MESSAGE( STATUS "    PLAYER_CFLAGS: ${PLAYER_CFLAGS}" )
@@ -153,15 +201,15 @@ CONFIGURE_FILE (${CMAKE_CURRENT_SOURCE_DIR}/stage-config-version.cmake.in ${CMAK
 INSTALL (FILES ${CMAKE_CURRENT_BINARY_DIR}/stage-config.cmake ${CMAKE_CURRENT_BINARY_DIR}/stage-config-version.cmake DESTINATION ${PROJECT_LIB_DIR}/cmake/${PROJECT_NAME})
 
 MESSAGE( STATUS "Installation path CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}" )
-
 # all targets need these include directories
-include_directories( . 
-		     libstage 
-		     replace 
+include_directories( .
+		     libstage
+		     replace
 		     ${FLTK_INCLUDE_DIR}
                      ${PNG_INCLUDE_DIR}
                      ${JPEG_INCLUDE_DIR}
 		     ${CMAKE_INCLUDE_PATH}
+		     ${Boost_INCLUDE_DIRS}
 )
 
 
@@ -170,11 +218,11 @@ ADD_SUBDIRECTORY(libstage)
 ADD_SUBDIRECTORY(examples)
 ADD_SUBDIRECTORY(assets)
 ADD_SUBDIRECTORY(worlds)
-#ADD_SUBDIRECTORY(avonstage)		 
+#ADD_SUBDIRECTORY(avonstage)
 
 IF ( BUILD_PLAYER_PLUGIN AND PLAYER_FOUND )
   ADD_SUBDIRECTORY(libstageplugin)
-ENDIF ( BUILD_PLAYER_PLUGIN AND PLAYER_FOUND )	 
+ENDIF ( BUILD_PLAYER_PLUGIN AND PLAYER_FOUND )
 
 
 # generate a cpack config file used to create packaged tarballs
diff --git a/config.h.in b/config.h.in
index ae08c6078..b92c13d22 100644
--- a/config.h.in
+++ b/config.h.in
@@ -9,5 +9,8 @@
 
 #cmakedefine BUILD_GUI
 
+#cmakedefine HAVE_BOOST_RANDOM
+
+
 #endif
 
diff --git a/examples/ctrl/CMakeLists.txt b/examples/ctrl/CMakeLists.txt
index a9c367980..dc6300c25 100644
--- a/examples/ctrl/CMakeLists.txt
+++ b/examples/ctrl/CMakeLists.txt
@@ -8,6 +8,7 @@ SET( PLUGINS
   rasterize
   lasernoise
   dynamic
+  wander_wifi
 )
 
 # create a library module for each plugin and link libstage to each
diff --git a/examples/ctrl/wander_wifi.cc b/examples/ctrl/wander_wifi.cc
new file mode 100644
index 000000000..0e453766e
--- /dev/null
+++ b/examples/ctrl/wander_wifi.cc
@@ -0,0 +1,221 @@
+#include "stage.hh"
+using namespace Stg;
+
+static const double cruisespeed = 0.4;
+static const double avoidspeed = 0.05;
+static const double avoidturn = 0.5;
+static const double minfrontdistance = 1.0; // 0.6
+static const bool verbose = false;
+static const double stopdist = 0.3;
+static const int avoidduration = 10;
+
+typedef struct
+{
+  uint32_t id;
+  ModelPosition* pos;
+  ModelRanger* laser;
+  ModelWifi* wifi;
+  int avoidcount, randcount;
+} robot_t;
+
+// typedef struct {
+//       Pose gpose;
+//           uint32_t id;
+// } agent_data_t;
+
+class MyWifiMessage : public WifiMessageBase {
+  //This is the stuff I want to share above and beyond what the base message shares.
+  public:
+    MyWifiMessage():WifiMessageBase(){};
+    ~MyWifiMessage(){ };
+    Pose gpose;
+
+    MyWifiMessage(const MyWifiMessage& toCopy) : WifiMessageBase(toCopy)
+    {
+      gpose = toCopy.gpose;
+    };           ///< Copy constructor
+    MyWifiMessage& operator=(const MyWifiMessage& toCopy)
+    {
+      WifiMessageBase::operator=(toCopy);
+      gpose = toCopy.gpose;
+
+      return *this;
+    };///< Equals operator overload
+
+    virtual WifiMessageBase* Clone() { return new MyWifiMessage(*this); };
+
+};//end MyWifiMessage
+
+
+int LaserUpdate( Model* mod, robot_t* robot );
+int PositionUpdate( Model* mod, robot_t* robot );
+int WifiUpdate( Model* mod, robot_t* robot );
+
+// Function to process incoming wifi messages.
+void ProcessMessage( WifiMessageBase * mesg , void* user );
+
+// Stage calls this when the model starts up
+extern "C" int Init( Model* mod, CtrlArgs* args )
+{
+  // local arguments
+//   printf( "\nWander Wifi controller initialised with:\n"
+// 			 "\tworldfile string \"%s\"\n"
+// 			 "\tcmdline string \"%s\"",
+// 			 args->worldfile.c_str(),
+// 			 args->cmdline.c_str() );
+
+  robot_t* robot = new robot_t;
+
+  robot->avoidcount = 0;
+  robot->randcount = 0;
+
+  robot->pos = (ModelPosition*)mod;
+  robot->laser = (ModelRanger*)mod->GetChild( "ranger:1" );
+  robot->laser->AddCallback( Model::CB_UPDATE, (model_callback_t)LaserUpdate, robot );
+  robot->wifi = (ModelWifi*)mod->GetChild("wifi:0");
+  robot->wifi->AddCallback( Model::CB_UPDATE, (model_callback_t)WifiUpdate, robot);
+  //robot->wifi->comm.SetArg( (void*)robot );    // set up the Rx function and argument
+  //robot->wifi->comm.SetReceiveFn( ProcessData );
+  robot->wifi->comm.SetReceiveMsgFn( ProcessMessage, robot );
+  robot->id = robot->wifi->GetId();
+  robot->wifi->Subscribe(); // start the wifi updates
+  robot->laser->Subscribe(); // starts the laser updates
+  robot->pos->Subscribe(); // starts the position updates
+
+  return 0; //ok
+}
+
+
+// inspect the laser data and decide what to do
+int LaserUpdate( Model* mod, robot_t* robot )
+{
+  // get the data
+  const std::vector<meters_t>& scan = robot->laser->GetSensors()[0].ranges;
+  uint32_t sample_count = scan.size();
+  if( sample_count < 1 )
+    return 0;
+
+  bool obstruction = false;
+  bool stop = false;
+
+  // find the closest distance to the left and right and check if
+  // there's anything in front
+  double minleft = 1e6;
+  double minright = 1e6;
+
+  for (uint32_t i = 0; i < sample_count; i++)
+    {
+
+      if( verbose ) printf( "%.3f ", scan[i] );
+
+      if( (i > (sample_count/3))
+      && (i < (sample_count - (sample_count/3)))
+      && scan[i] < minfrontdistance)
+    {
+      if( verbose ) puts( "  obstruction!" );
+      obstruction = true;
+    }
+
+      if( scan[i] < stopdist )
+    {
+      if( verbose ) puts( "  stopping!" );
+      stop = true;
+    }
+
+      if( i > sample_count/2 )
+    minleft = std::min( minleft, scan[i] );
+      else
+    minright = std::min( minright, scan[i] );
+    }
+
+  if( verbose )
+    {
+      puts( "" );
+      printf( "minleft %.3f \n", minleft );
+      printf( "minright %.3f\n ", minright );
+    }
+
+  if( obstruction || stop || (robot->avoidcount>0) )
+    {
+      if( verbose ) printf( "Avoid %d\n", robot->avoidcount );
+
+      robot->pos->SetXSpeed( stop ? 0.0 : avoidspeed );
+
+      /* once we start avoiding, select a turn direction and stick
+     with it for a few iterations */
+      if( robot->avoidcount < 1 )
+        {
+      if( verbose ) puts( "Avoid START" );
+          robot->avoidcount = random() % avoidduration + avoidduration;
+
+      if( minleft < minright  )
+        {
+          robot->pos->SetTurnSpeed( -avoidturn );
+          if( verbose ) printf( "turning right %.2f\n", -avoidturn );
+        }
+      else
+        {
+          robot->pos->SetTurnSpeed( +avoidturn );
+          if( verbose ) printf( "turning left %2f\n", +avoidturn );
+        }
+        }
+
+      robot->avoidcount--;
+    }
+  else
+    {
+      if( verbose ) puts( "Cruise" );
+
+      robot->avoidcount = 0;
+      robot->pos->SetXSpeed( cruisespeed );
+      robot->pos->SetTurnSpeed(  0 );
+    }
+
+  //  if( robot->pos->Stalled() )
+  //     {
+  //        robot->pos->SetSpeed( 0,0,0 );
+  //        robot->pos->SetTurnSpeed( 0 );
+  // }
+
+  return 0; // run again
+}
+
+int PositionUpdate( Model* mod, robot_t* robot )
+{
+  Pose pose = robot->pos->GetPose();
+
+  printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
+      pose.x, pose.y, pose.z, pose.a );
+
+  return 0; // run again
+}
+
+int WifiUpdate( Model* mod, robot_t* robot )
+{
+
+  // We are going to share our global pose with our neighbors.
+  MyWifiMessage my_mesg;
+  my_mesg.gpose = robot->pos->GetGlobalPose();
+  WifiMessageBase * base_ptr = & my_mesg;
+  // Call yupon the comm system to share this message with everyone in range.
+  robot->wifi->comm.SendBroadcastMessage( base_ptr );
+
+  return 0;
+}
+
+
+void ProcessMessage( WifiMessageBase * incoming, void * user )
+{
+  MyWifiMessage * my_mesg = dynamic_cast<MyWifiMessage*>(incoming);
+  if ( my_mesg )
+  {
+    //Yay it's my own special message type!!!
+    printf("Robot [%u]: Neighbor [%u] is at (%.2f %.2f) and heading (%.2f)\n",
+       my_mesg->GetRecipientId(), my_mesg->GetSenderId(), my_mesg->gpose.x, my_mesg->gpose.y, my_mesg->gpose.a );
+  }
+
+  //It's our responsibility to clean up the incoming message
+  delete incoming;
+}//end ProcessMessageData
+
+
diff --git a/libstage/CMakeLists.txt b/libstage/CMakeLists.txt
index ca80659ed..709c7dcd4 100644
--- a/libstage/CMakeLists.txt
+++ b/libstage/CMakeLists.txt
@@ -3,11 +3,12 @@ MESSAGE( STATUS "Configuring libstage" )
 # for config.h
 include_directories(${PROJECT_BINARY_DIR})
 
-set( stageSrcs    
+set( stageSrcs
 	block.cc
 	blockgroup.cc
 	camera.cc
 	color.cc
+	communication.cc
 	file_manager.cc
 	file_manager.hh
 	gl.cc
@@ -30,42 +31,44 @@ set( stageSrcs
 	stage.cc
 	stage.hh
 	texture_manager.cc
-	typetable.cc		
-	world.cc			
-	worldfile.cc		
-	canvas.cc 
+	typetable.cc
+	world.cc
+	worldfile.cc
+	canvas.cc
 	options_dlg.cc
 	options_dlg.hh
-	vis_strip.cc			
-	worldgui.cc 
+	vis_strip.cc
+	worldgui.cc
 	ancestor.cc
+	model_wifi.cc
 )
 
 #	model_getset.cc
 #	model_load.cc
 
 #set_source_files_properties( ${stageSrcs} PROPERTIES COMPILE_FLAGS" )
- 
+
 add_library(stage SHARED ${stageSrcs})
 
 # if fltk-config didn't bring along the OpenGL dependencies (eg. on
-# Debian/Ubuntu), add them explicity 
+# Debian/Ubuntu), add them explicity
 IF (NOT(${FLTK_LDFLAGS} MATCHES "-lGL"))
-  target_link_libraries( stage ${OPENGL_LIBRARIES}) 
+  target_link_libraries( stage ${OPENGL_LIBRARIES})
 ENDIF (NOT(${FLTK_LDFLAGS} MATCHES "-lGL"))
 
 
 # causes the shared library to have a version number
 set_target_properties( stage PROPERTIES
 		       VERSION ${VERSION}
-#           LINK_FLAGS "${FLTK_LDFLAGS}"  
+#           LINK_FLAGS "${FLTK_LDFLAGS}"
 )
 
-target_link_libraries( stage 
-                       ${LTDL_LIB} 
-                       ${JPEG_LIBRARIES} 
+target_link_libraries( stage
+                       ${LTDL_LIB}
+                       ${JPEG_LIBRARIES}
                        ${PNG_LIBRARIES}
                        ${FLTK_LIBRARIES}
+                       ${boostRandomLib}
 )
 
 set( stagebinarySrcs main.cc )
@@ -101,6 +104,6 @@ INSTALL(TARGETS stagebinary stage
 	LIBRARY DESTINATION ${PROJECT_LIB_DIR}
 )
 
-INSTALL(FILES stage.hh 
+INSTALL(FILES stage.hh
         DESTINATION include/${PROJECT_NAME}-${APIVERSION})
 
diff --git a/libstage/communication.cc b/libstage/communication.cc
new file mode 100644
index 000000000..650476cbf
--- /dev/null
+++ b/libstage/communication.cc
@@ -0,0 +1,206 @@
+///////////////////////////////////////////////////////////////////////////
+//
+// File: communication.cc
+// Author: Rahul Balani
+// Date: 27 May 2009
+//
+///////////////////////////////////////////////////////////////////////////
+
+/**
+@ingroup model
+@defgroup model_comm Inter-Robot Data Communication Interface 
+
+Description: To fill
+
+<h2>Worldfile properties</h2>
+
+@par SetReceiveFn()
+
+@verbatim
+Communication::
+@endverbatim
+
+@par SetArg()
+
+@verbatim
+Communication::
+@endverbatim
+
+@par CallReceiveFn()
+
+@verbatim
+Communication::
+@endverbatim
+
+@par Configuration Parameters
+- ReceiveFn
+  - Function to call when a message is received from another robot
+- arg
+  - User defined argument to pass to ReceiveFn along with received message
+*/
+
+//#include <assert.h>
+//#include <sys/time.h>
+//#include <math.h>
+//#include <limits.h>
+#include "stage.hh"
+using namespace Stg;
+
+//----------------------------------Communication System-----------------------------
+
+/**
+ * The contructor for simulated inter-robot communication overlaid on the WIFI model.
+ * We need access to the WIFI model so we can determine who messages are sent to. 
+ */
+Communication::Communication(ModelWifi * parent_p)
+{
+  wifi_p = parent_p;
+
+  PRINT_DEBUG( "Initializing Communication Interface" );
+
+
+}//end comm constructor
+
+
+/**
+ * Destructor for communication system.
+ */
+Communication::~Communication( void )
+{
+}//end Communication destructor
+
+
+/**
+ * Set the message sender based on the configuration of the current comm interface. 
+ */
+void Communication::SetMessageSender(WifiMessageBase * to_send)
+{
+  WifiConfig *config = wifi_p->GetConfig();
+
+  //Set some attributes on the message to send based on who the sender is
+  to_send->SetSenderId(wifi_p->GetId());
+  to_send->SetSenderIp(config->GetIp());
+  to_send->SetSenderNetmask(config->GetNetmask());
+  to_send->SetSenderMac(config->GetMac());
+}//end SetMessageSender
+
+
+/**
+ * Send the message as a broadcast to the broadcast IP of the sender. 
+ */
+void Communication::SendBroadcastMessage(WifiMessageBase * to_send)
+{
+  SetMessageSender(to_send);
+  to_send->SetBroadcastIp();
+  SendMessage(to_send);
+}//end SendBroadcastMessage
+
+
+/**
+ * Send the message as a unicast to the intended recipient.
+ */
+void Communication::SendUnicastMessage(WifiMessageBase * to_send)
+{
+  SetMessageSender(to_send);
+  SendMessage(to_send);
+}//end SendUnicastMessage
+
+
+/**
+ * Send a wifi message to recipient in range who are a member of the same ESSID, in the same network
+ * and who's IP address/netmask match the recipient of the message.
+ */
+void Communication::SendMessage(WifiMessageBase * to_send) 
+{
+  //Get link and wifi info for the current wifi
+  stg_wifi_data_t *linkdata = wifi_p->GetLinkInfo();
+
+  for (unsigned int i=0; i < linkdata->neighbors.size(); i++) 
+  {
+    //Essid must match
+    if (linkdata->neighbors[i].GetEssid().compare(wifi_p->wifi_config.GetEssid()) == 0)
+    {
+
+      //Check if the networks match
+      in_addr_t sender_network = wifi_p->wifi_config.GetIp() & wifi_p->wifi_config.GetNetmask();
+      in_addr_t recip_network = linkdata->neighbors[i].GetIp() & linkdata->neighbors[i].GetNetmask();
+
+      if ( sender_network == recip_network )
+      {
+
+        //Determine if the message is destined for the broadcast address or
+        //specifically addressed to hte current neighbor
+        if ( ( to_send->GetRecipientIp() == linkdata->neighbors[i].GetIp() ) ||
+             ( to_send->IsBroadcastIp() ) )
+        {
+          //Yay its destined for here!
+          //Get the nieghbor's comm interface and try to call its receive message function
+          Communication * neighbor_comm = linkdata->neighbors[i].GetCommunication();
+          if (neighbor_comm->IsReceiveMsgFnSet())
+          {
+            //Make a deep clone of the message to send (this way if we're really pointing at 
+            //a subclass, we get the subclass copied over, not just the base).
+
+            WifiMessageBase * message_copy = to_send->Clone();
+            message_copy->SetRecipientId(neighbor_comm->wifi_p->GetId());      
+            neighbor_comm->CallReceiveMsgFn(message_copy);
+          }
+        }
+      }//end network match
+    }//end same ESSID
+  }//end for
+
+}//end SendMessageToAllInRange
+
+
+//------------------------------Wifi Messages----------------------------
+
+/**
+ * WifiMessageBase copy constructor.
+ */
+WifiMessageBase::WifiMessageBase(const WifiMessageBase& toCopy)
+{
+  sender_mac = toCopy.sender_mac;
+  sender_ip_in = toCopy.sender_ip_in;
+  sender_netmask_in = toCopy.sender_netmask_in;
+  sender_id = toCopy.sender_id;
+  recipient_ip_in = toCopy.recipient_ip_in;
+  recipient_netmask_in = toCopy.recipient_netmask_in;
+  recipient_id = toCopy.recipient_id;
+  message = toCopy.message;
+
+}
+
+/**
+ * WifiMessageBase equals operator.
+ */
+WifiMessageBase& WifiMessageBase::operator=(const WifiMessageBase& toCopy)
+{
+  sender_mac = toCopy.sender_mac;
+  sender_ip_in = toCopy.sender_ip_in;
+  sender_netmask_in = toCopy.sender_netmask_in;
+  sender_id = toCopy.sender_id;
+  recipient_ip_in = toCopy.recipient_ip_in;
+  recipient_netmask_in = toCopy.recipient_netmask_in;
+  recipient_id = toCopy.recipient_id;
+  message = toCopy.message;
+
+  return *this;
+}
+
+
+/**
+ * Set the message recipient to the broadcast IP.  This means everyone in
+ * radio range should receive the message.
+ */
+void WifiMessageBase::SetBroadcastIp()
+{
+  //Basically all we've got to do is look at the sender's IP and bit-wise OR in
+  //the complement of the netmask. 
+  // Thus sender 192.168.0.1 with netmask 255.255.255.0 would come back as 
+  // broadcast IP 192.168.0.255.
+  recipient_ip_in = ( sender_ip_in | ( ~sender_netmask_in  ) );
+  recipient_netmask_in = sender_netmask_in;
+}//end Setboardcast
+
+
diff --git a/libstage/model.cc b/libstage/model.cc
index 84d1f02a4..8231a8ea2 100644
--- a/libstage/model.cc
+++ b/libstage/model.cc
@@ -345,7 +345,9 @@ Model::Model( World* world,
     }        
 
   // now we can add the basic square shape
-  AddBlockRect( -0.5, -0.5, 1.0, 1.0, 1.0 );
+  if(GetModelType() != "wifi"){
+    AddBlockRect( -0.5, -0.5, 1.0, 1.0, 1.0 ); // this caused a problem with wifi model...
+  }
 
   AddVisualizer( &rastervis, false );
 
diff --git a/libstage/model_wifi.cc b/libstage/model_wifi.cc
index bd7f2c08d..5c92d88d4 100644
--- a/libstage/model_wifi.cc
+++ b/libstage/model_wifi.cc
@@ -1,11 +1,14 @@
 ///////////////////////////////////////////////////////////////////////////
 //
 // File: model_wifi.cc
-// Authors: Benoit Morisset, Richard Vaughan
-// Date: 2 March 2006
+// Authors: Michael Schroeder
+// Date: 10 July 2007
+// Ported to Stage-3.1.0 by Rahul Balani
+// Ported to Stage-3.2.0 by Tyler Gunn
+// Date: 24-Sept-2009
 //
 // CVS info:
-//  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_wifi.cc,v $
+//  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/src/model_wifi.cc,v $
 //  $Author: rtv $
 //  $Revision$
 //
@@ -13,140 +16,694 @@
 
 /**
   @ingroup model
-  @defgroup model_wifi Wifi model 
+  @defgroup model_wifi Wifi model
 
-  @todo Fill this in
+  The wifi model simulates a wifi device. There are currently five radio propagation models
+  implemented. The wifi device reports simulated link information for all corresponding
+  nodes within range.
 
   <h2>Worldfile properties</h2>
 
-  @todo Fill this in
+  @par Simple Wifi Model
+
++@verbatim
+  wifi
+  (
+     # network properties
+    ip "192.168.0.1"
+    mac "08-00-20-ae-fd-7e"
+    essid "test network"
+
+     # radio propagation model
+    model "simple"
+     range 5
+  )
+  @endverbatim
+  For the simple model it is only necessary to specify the range property which serves as the
+  radio propagation radius.
+
+
+  @par Friis Outdoor Model
+
+  @verbatim
+  wifi
+  (
+     # network properties
+    ip "192.168.0.1"
+    mac "08-00-20-ae-fd-7e"
+    essid "test network"
+
+     # radio propagation model
+    model "friis"
+     power 30
+     sensitivity -80
+  )
+  @endverbatim
+  The friis model simulates the path loss for each link according to Friis Transmission Equation. Basically it serves as a free space model. The parameters power(dbm) and sensitivity(dbm) are mandatory.
+
+
+  @par ITU Indoor Propagation Model
+
+  @verbatim
+  wifi
+  (
+     # network properties
+    ip "192.168.0.1"
+    mac "08-00-20-ae-fd-7e"
+    essid "test network"
+
+     # radio propagation model
+    model "ITU indoor"
+     power 30
+     sensitivity -70
+     plc 30
+  )
+  @endverbatim
+  This model estimates the path loss inside rooms or closed areas. The distance power loss
+  coefficient must be set according to the environmental settings. Values vary between 20 to 35.
+  Higher values indicate higher path loss.
+
+
+  @par Log Distance Path Loss Model
+
+  @verbatim
+  wifi
+  (
+     # network properties
+    ip "192.168.0.1"
+    mac "08-00-20-ae-fd-7e"
+    essid "test network"
+
+     # radio propagation model
+    model "log distance"
+     power 30
+     sensitivity -70
+     ple 4
+     sigma 6
+  )
+  @endverbatim
+  The Log Distance Path Loss Model approximates the total path loss inside a building.
+  It uses the path loss distance exponent (ple) which varies between 1.8 and 10 to accommodate for different environmental settings. A Gaussian random variable with zero mean and standard deviation sigma
+  is used to reflect shadow fading.
+
+
+  @par Simple Raytracing Model
+
+  @verbatim
+  wifi
+  (
+     # network properties
+    ip "192.168.0.1"
+    mac "08-00-20-ae-fd-7e"
+    essid "test network"
+
+     # radio propagation model
+    model "raytrace"
+     wall_factor 10
+  )
+  @endverbatim
+  In this model Raytracing is used to reflect wifi-blocking obstacles within range. The wall_factor
+  is used to specify how hard it is for the signal to penetrate obstacles.
+
+
+  @par Details
+  - model
+    - the model to use. Choose between the five existing radio propagation models
+  - ip
+    - the ip to fake
+  - mac
+    - the corresponding mac addess
+  - essid
+    - networks essid
+  - freq
+    - the operating frequency [default 2450 (MHz)]
+  - power
+    - Transmitter output power in dbm [default 45 (dbm)]
+  - sensitivity
+    - Receiver sensitivity in dbm [default -75 (dbm)]
+  - range
+    - propagation radius in meters used by simple model
+  - link_success_rate
+    - (0-100) percent of times that links between robots are successfully establiehed.
+    Note: It is entirely possible for a robot to be able to hear its neighbor, but its
+    neighbour is unable to hear it.
+  - plc
+    - power loss coefficient used by the ITU Indoor model
+  - ple
+    - power loss exponent used by the Log Distance Path Loss Model
+  - sigma
+    - standard deviation used by Log Distance Path Loss Model
+  - range_db
+    - parameter used for visualization of radio wave propagation. It is usually set to a negative value
+  to visualize a certain db range.
+  - wall_factor
+    - factor reflects the strength of obstacles
+    - random_seed <int>\n
+      The seed value for the random number generator.  If not defined, a random
+      seed will be used.  Speciying the seed in the world file is important
+      if you want to be able to repeatedly run the exact same simulation to
+      receive repeatable results.
 
  */
 
 
+#include <assert.h>
 #include <sys/time.h>
-#include "gui.h"
+#include <math.h>
+#include <limits.h>
+#include <string.h>
+#include <sstream>
+#include <stdio.h>
+#include <iomanip>
+#include "stage.hh"
+#include "worldfile.hh"
+#include "option.hh"
+#include <config.h>
+#include <ctime>
+#ifdef HAVE_BOOST_RANDOM
+#include <boost/random.hpp>
+#include <boost/nondet_random.hpp>
+
+#endif
+
+using namespace Stg;
+
+#define RANDOM_LIMIT INT_MAX
+#define STG_WIFI_WATTS     2.5 // wifi power consumption
+#define STG_DEFAULT_WIFI_IP    "192.168.0.1"
+#define STG_DEFAULT_WIFI_NETMASK "255.255.255.0"
+#define STG_DEFAULT_WIFI_MAC "00:00:00:00:00"
+#define STG_DEFAULT_WIFI_ESSID "any"
+#define STG_DEFAULT_WIFI_POWER 45
+#define STG_DEFAULT_WIFI_FREQ  2450
+#define STG_DEFAULT_WIFI_SENSITIVITY -75
+#define STG_DEFAULT_WIFI_RANGE 0
+#define STG_DEFAULT_WIFI_PLC       30
+#define STG_DEFAULT_WIFI_SIGMA  5 // can take values from 2 to 12 or even higher
+#define STG_DEFAULT_WIFI_PATH_LOSS_EXPONENT 2.5 // 2.0 for free space,
+                                                // 1.8 in corridor, higher
+                                                // values indicate more obstacles
+#define STG_DEFAULT_WIFI_RANGE_DB -50 //in dbm
+#define STG_DEFAULT_WIFI_WALL_FACTOR 1
+#define STG_DEFAULT_LINK_SUCCESS_RATE 100
+
+//-----------------------WifiConfig stuff-----------------------------------
+
+const std::string WifiConfig::MODEL_NAME_FRIIS = "friis";
+const std::string WifiConfig::MODEL_NAME_ITU_INDOOR = "ITU indoor";
+const std::string WifiConfig::MODEL_NAME_LOG_DISTANCE = "log distance";
+const std::string WifiConfig::MODEL_NAME_RAYTRACE = "raytrace";
+const std::string WifiConfig::MODEL_NAME_SIMPLE = "simple";
 
-// Number pulled directly from my ass
-#define STG_WIFI_WATTS 2.5
+/**
+ * Constructor for WIFI config.  We'll set everything up using some sane
+ * default values.
+ */
+WifiConfig::WifiConfig()
+{
+  SetEssid(STG_DEFAULT_WIFI_ESSID);
+  SetIp(STG_DEFAULT_WIFI_IP);
+  SetNetmask(STG_DEFAULT_WIFI_NETMASK);
+  SetPower( STG_DEFAULT_WIFI_POWER);
+  SetSensitivity(STG_DEFAULT_WIFI_SENSITIVITY);
+  SetRange(STG_DEFAULT_WIFI_RANGE);
+  SetFreq(STG_DEFAULT_WIFI_FREQ);
+  SetPlc(STG_DEFAULT_WIFI_PLC);
+  SetPle(STG_DEFAULT_WIFI_PATH_LOSS_EXPONENT);
+  SetSigma(STG_DEFAULT_WIFI_SIGMA);
+  SetRangeDb(STG_DEFAULT_WIFI_RANGE_DB);
+  SetWallFactor(STG_DEFAULT_WIFI_WALL_FACTOR);
+  SetLinkSuccessRate(STG_DEFAULT_LINK_SUCCESS_RATE);
+}//end constructor
 
-//#define DEBUG
+/**
+ * Set the model attribute based ona string (i.e. the ones we would expect
+ * to find in the world files.
+ */
+void WifiConfig::SetModel(const std::string & value)
+{
+  if (value.compare("friis") == 0)
+    model = FRIIS;
+  else if (value.compare("ITU indoor") == 0)
+    model = ITU_INDOOR;
+  else if (value.compare("log distance") == 0)
+    model = LOG_DISTANCE;
+  else if (value.compare("raytrace") == 0)
+    model = RAYTRACE;
+  else if (value.compare("simple") == 0)
+    model = SIMPLE;
+}//end SetModel
+
+const std::string & WifiConfig::GetModelString()
+{
+  if ( model == FRIIS )
+    return MODEL_NAME_FRIIS;
+  else if ( model == ITU_INDOOR )
+    return MODEL_NAME_ITU_INDOOR;
+  else if ( model == LOG_DISTANCE )
+    return MODEL_NAME_LOG_DISTANCE;
+  else if ( model == RAYTRACE )
+    return MODEL_NAME_RAYTRACE;
+  else if ( model == SIMPLE )
+    return MODEL_NAME_SIMPLE;
+  else return MODEL_NAME_SIMPLE;
+}
+
+//-----------------------Visualizer Stuff-----------------------------------
 
-#include "stage_internal.h"
+/**
+ * Return list of link information.
+ */
+stg_wifi_data_t* ModelWifi::GetLinkInfo( )
+{
+    return &link_data;
+}
 
-// defined in model.c
-int _model_update( model_t* mod, void* unused );
+/**
+ * Visualizer option to show the wifi links between robots
+ */
+Option ModelWifi::showWifi( "Wifi", "show_wifi", "", true, NULL );
 
-// standard callbacks
-extern "C" {
 
-	// declare functions used as callbacks
-	static int wifi_update( model_t* mod, void* unused );
-	static int wifi_startup( model_t* mod, void* unused );
-	static int wifi_shutdown( model_t* mod, void* unused );
-	static int wifi_load( model_t* mod, void* unused );
+/**
+ * Visualizer method to display links between robots
+ */
+void ModelWifi::DataVisualize( Camera* cam )
+{
+  if (link_data.neighbors.size() == 0) return;
+
+  glPushMatrix();
+
+  unsigned int s, sample_count = link_data.neighbors.size();
+
+  glDepthMask( GL_TRUE );
+
+  if( showWifi )
+  {
+    PushColor( 0, 0, 1, 0.5 );
+    glBegin( GL_LINES );
+    for( s=0; s<sample_count; s++ )
+    {
+      Pose loc = GlobalToLocal(link_data.neighbors[s].GetPose());
+      glVertex2f( 0,0 );
+      glVertex2f( loc.x, loc.y );
+        }
+    glEnd();
+    PopColor();
+  }
+  glPopMatrix();
+}
+
+//------------------------Wifi Model----------------------------------------
+/**
+ * Wifi model constructor
+ */
+ModelWifi::ModelWifi( World* world, Model* parent, const std::string& type )
+    : Model( world, parent, type ),
+    comm(this)
+		{
+  PRINT_DEBUG2( "Constructing ModelWifi %d (%s)\n",
+            id, typestr );
+  //Try to seed the RNG with /dev/random.  If the file is opened okay, then we can rely on /dev/random.
+  //If it wasn't opened okay, then we'll rely on the standard fallback of using the time.  This isn't optimal in the least
+  //and probably results in bad randomness on windows systems.  Oh well. :)
+  FILE* f = fopen("/dev/random", "r");
+  if ( f  )
+  {
+    unsigned int seed_data;
+    size_t size = fread(&seed_data, sizeof(unsigned int), 1, f);
+    (void) size; // avoid warning about unused return value
+    fclose(f);
+    random_seed = seed_data;
+  }
+  else
+  {
+    PRINT_ERR( "NOTE: RNG couldn't seed from /dev/random as /dev/random doesn't exist.  The random numbers may not be optimal in this case.\n");
+    random_seed = static_cast<unsigned int>(std::time(0));
+  }
+  //Get a unique MAC address for this wifi model among all other wifi models
+  //currently defined in the world.
+  wifi_config.SetMac(GetUniqueMac(world));
+
+  //Add this model to the list of wifi models in the world
+  world->WifiInsert(this);
+
+  // assert that Update() is reentrant for this derived model
+  thread_safe = true;
+
+  // no power consumed until we're subscribed
+  this->SetWatts( 0 );
+
+  // Sensible wifi defaults; it doesn't take up any physical space
+			Geom geom;
 
-	// implented by the gui in some other file
-	void gui_wifi_init( model_t* mod );
+  memset( &geom, 0, sizeof(geom));
+  SetGeom( geom );
 
+  // Wifi is invisible
+  SetObstacleReturn( 0 );
+  SetRangerReturn( 0.0 );
+  SetBlobReturn( 0 );
+  SetColor( Color() );
 
-	int 
-		wifi_init( model_t* mod )
-		{
-			// we don't consume any power until subscribed
-			mod->watts = 0.0;
+#ifdef HAVE_BOOST_RANDOM
+  // For communication reliability simulation we need a generator that generates from 0-100 inclusive.
 
-			// override the default methods; these will be called by the simualtion
-			// engine
-			model_add_callback( mod, &mod->startup, wifi_startup, NULL );
-			model_add_callback( mod, &mod->shutdown, wifi_shutdown, NULL );
-			model_add_callback( mod, &mod->load, wifi_load, NULL );
+  comm_reliability_gen = new BoostRandomIntGenerator( random_generator, uniform_distribution_type(0, 100));
+  // For the log method we need a normal distribution of random numbers.  Boost thoughtfully provides us with one! :)
+  normal_gen = new BoostNormalGenerator( random_generator, normal_distribution_type(0,1));
+#endif
+  RegisterOption( &showWifi );
+}//end ModelWifi const
 
-			// sensible wifi defaults; it doesn't take up any physical space
-			Geom geom;
-			geom.pose.x = 0.0;
-			geom.pose.y = 0.0;
-			geom.pose.a = 0.0;
-			geom.size.x = 0.0;
-			geom.size.y = 0.0;
-			model_set_geom( mod, &geom );
-
-			// wifi is invisible
-			model_set_obstacle_return( mod, 0 );
-			model_set_laser_return( mod, LaserTransparent );
-			model_set_blob_return( mod, 0 );
-			model_set_color( mod, (color_t)0 );
-
-			gui_wifi_init(mod);
-
-			return 0;
-		}
+/**
+ * Destructor
+ */
+ModelWifi::~ModelWifi( void )
+{
+  world->WifiErase(this);
 
-	int 
-		wifi_update( model_t* mod, void* unused )
-		{   
-			// no work to do if we're unsubscribed
-			if( mod->subs < 1 )
-				return 0;
+  // It will free the elements of the array as well
+  link_data.neighbors.clear();
+  //g_array_free(link_data.neighbors, TRUE);
 
-			puts("wifi_update");
+#ifdef HAVE_BOOST_RANDOM
+  delete comm_reliability_gen;
+  delete normal_gen;
+#endif
 
-			// Retrieve current configuration
-			wifi_config_t cfg;
-			memcpy(&cfg, mod->cfg, sizeof(cfg));
+}//end ModelWifi dest
 
-			// Retrieve current geometry
-			Geom geom;
-			model_get_geom( mod, &geom );
 
-			// get the sensor's pose in global coords
-			Pose pz;
-			memcpy( &pz, &geom.pose, sizeof(pz) );
-			model_local_to_global( mod, &pz );
+/**
+ * Get a unique Mac address for this WIFI model among all other WIFI models in the current world.
+ */
+std::string ModelWifi::GetUniqueMac( World* world )
+{
+  std::stringstream macSs;
+
 
+  bool duplicate = false;
+  std::string mac_str = "";
 
-			// We'll store current data here
-			wifi_data_t data;
+  //Find MACs until we're sure no other models have this address.
+  do
+  {
+    duplicate = false;
 
-			// DO RADIO PROPAGATION HERE, and fill in the data structure
+    macSs.str("");
+    macSs.clear();
 
-			// publish the new stuff
-			model_set_data( mod, &data, sizeof(data));
+#ifdef HAVE_BOOST_RANDOM
+    // For mac geneartion, we need a generator that generates from 0-255 inclusive.
+    BoostRandomIntGenerator mac_rand_gen(random_generator, uniform_distribution_type(0, 255));
+#endif
 
-			// inherit standard update behaviour
-			_model_update( mod, NULL );
 
-			return 0; //ok
-		}
+    //Start out our unique mac with 02:00:00 as the organization OID; this specifies this MAC as a
+    //locally administered MAC address versus one assigned by a manufacturer.
+    //(for more info see http://en.wikipedia.org/wiki/MAC_address)
+    macSs << "02:00:00";
 
-	int 
-		wifi_startup( model_t* mod, void* unused )
-		{ 
-			PRINT_DEBUG( "wifi startup" );
-			model_add_callback( mod, &mod->startup, wifi_startup, NULL );
-			model_set_watts( mod, STG_WIFI_WATTS );
-			return 0; // ok
-		}
+    //Now we need to generate 3 random hex bytes to make up the rest of the mac.
+    for (unsigned int bNum = 0; bNum < 3; bNum++ )
+    {
+#ifdef HAVE_BOOST_RANDOM
+      int randomByte = mac_rand_gen();
+#else
+      int randomByte = rand() % 256;
+#endif
+      macSs << ":" << std::hex << std::setfill('0') << std::setw(2) << randomByte;
+        }
 
-	int 
-		wifi_shutdown( model_t* mod, void* unused )
+    mac_str = macSs.str();
+
+    //Now, we need to check if any other WIFI model in the world uses this mac.
+    for(int i=0; i<world->num_models_with_wifi; ++i)
+    //FOR_EACH( other_model, *(world->GetModelsWithWifi()) )
 		{
-			PRINT_DEBUG( "wifi shutdown" );
-			model_remove_callback( mod, &mod->startup, wifi_startup );
-			model_set_watts( mod, 0 );
-			return 0; // ok
+      //ModelWifi *other_model_wifi = dynamic_cast<ModelWifi*>(*other_model);
+      ModelWifi *other_model_wifi = dynamic_cast<ModelWifi*>(world->models_with_wifi.at(i));
+
+      if ( other_model_wifi == this )
+      //if ( world->models_with_wifi.at(i) == this )
+        continue;
+
+      if (other_model_wifi->GetConfig()->GetMac().compare( mac_str ) == 0 )
+      //if (world->models_with_wifi.at(i)->GetConfig()->GetMac().compare( mac_str ) == 0 )
+        duplicate = true;
 		}
 
-	int
-		wifi_load( model_t* mod, void* unused )
-		{
-			wifi_config_t cfg;
+  } while (duplicate);
 
-			// TODO: read wifi params from the world file
+  return mac_str;
+}//end AssignUniqueMac
 
-			model_set_cfg( mod, &cfg, sizeof(cfg));
+/**
+ * Load a wifi model from the world file.
+ */
+void ModelWifi::Load( void )
 
-			return 0;
-		}
+		{
 
-} // ends extern "C"
+  std::string ip_str = wifi_config.GetIpString();
+  std::string netmask_str = wifi_config.GetNetmaskString();
+  const std::string & model_str = wifi_config.GetModelString();
+
+  // read wifi parameters from worldfile
+  wifi_config.SetIp           (wf->ReadString(wf_entity, "ip",          ip_str.c_str()));
+  wifi_config.SetNetmask      (wf->ReadString(wf_entity, "netmask",     netmask_str.c_str()));
+  wifi_config.SetMac          (wf->ReadString(wf_entity, "mac",         wifi_config.GetMac().c_str()));
+  wifi_config.SetEssid        (wf->ReadString(wf_entity, "essid",       wifi_config.GetEssid().c_str()));
+  wifi_config.SetPower        (wf->ReadFloat (wf_entity, "power",       wifi_config.GetPower()));
+  wifi_config.SetFreq         (wf->ReadFloat (wf_entity, "freq",        wifi_config.GetFreq()));
+  wifi_config.SetSensitivity  (wf->ReadFloat (wf_entity, "sensitivity", wifi_config.GetSensitivity()));
+  wifi_config.SetRange        (wf->ReadLength(wf_entity, "range",       wifi_config.GetRange()));
+  wifi_config.SetModel        (wf->ReadString(wf_entity, "model",       model_str.c_str()));
+  wifi_config.SetPlc          (wf->ReadFloat (wf_entity, "plc",         wifi_config.GetPlc()));
+  wifi_config.SetPle          (wf->ReadFloat (wf_entity, "ple",         wifi_config.GetPle()));
+  wifi_config.SetSigma        (wf->ReadFloat (wf_entity, "sigma",       wifi_config.GetSigma()));
+  wifi_config.SetLinkSuccessRate(wf->ReadInt (wf_entity, "link_success_rate", wifi_config.GetLinkSuccessRate()));
+  wifi_config.SetRangeDb      (wf->ReadFloat (wf_entity, "range_db",    wifi_config.GetRangeDb()));
+  wifi_config.SetWallFactor   (wf->ReadFloat (wf_entity, "wall_factor", wifi_config.GetWallFactor()));
+  wifi_config.SetPowerDbm     (10.0*log10( wifi_config.GetPower()));
+
+  // read the random seed to use for this run.
+  random_seed=                 wf->ReadInt(wf_entity,"random_seed", random_seed );
+
+  // Initialize the random number generator with the seed.
+#ifdef HAVE_BOOST_RANDOM
+  random_generator.seed(random_seed);
+#else
+  srand(random_seed);
+#endif
+
+  Model::Load();
+}//end Load
+
+
+// Marsaglia polar method to obtain a gaussian random variable
+double nrand()
+{
+  //Uhh why would we re-seed EVERY single time?  That does not make sense.
+  //srand(time(0)+d); // time(0) is different only every second
+  double u1,u2,s;
+
+  do
+  {
+    u1= ((double) rand()/RANDOM_LIMIT)*2.0-1.0;
+    u2= ((double) rand()/RANDOM_LIMIT)*2.0-1.0;
+    s=u1*u1+u2*u2;
+        }
+  while (s>=1.0);
+  return ( u1*sqrt(-2* log(s) /s) );
+		}//end nrand
+
+static bool wifi_raytrace_match( Model* hit, Model* finder,
+        const void* dummy )
+        {
+  // Ignore the model that's looking and things that are invisible to
+  // lasers
+  return( (!hit->IsRelated( finder )) &&
+          ( hit->vis.ranger_return > 0) );
+}//end wifi_raytrace_match
+meters_t calc_distance( Model* mod1,
+                            Model* mod2,
+                            Pose* pose1,
+                            Pose* pose2 )
+{
+  Ray r;
+  r.func = wifi_raytrace_match;
+  // Dummy argument for us
+  r.arg = NULL;
+  r.ztest = true;
+  // Max range of the ray should be the actual distance between two devices
+  r.range = hypot(pose2->x - pose1->x, pose2->y - pose1->y);
+  // Source model/device
+  r.mod = mod1;
+  // Set up the ray origins in global coords
+  Pose rayorg = *pose1;
+  // Direction of the target device
+  rayorg.a = atan2( pose2->y - pose1->y, pose2->x - pose1->x );
+  r.origin = rayorg;
+
+  return mod1->GetWorld()->RaytraceWifi(r.origin, r.range, r.func, r.mod, mod2, r.arg, r.ztest);
+}//end calc_distance
+
+void ModelWifi::AppendLinkInfo( ModelWifi* mod, double db )
+{
+  Pose pose2 = mod->GetGlobalPose();
+  WifiConfig* neighbor_config = mod->GetConfig();
+
+  WifiNeighbor neighbor;
+  neighbor.SetId        ( mod->GetId());
+  neighbor.SetPose      ( mod->GetGlobalPose());
+  neighbor.SetEssid     ( neighbor_config->GetEssid());
+  neighbor.SetMac       ( neighbor_config->GetMac());
+  neighbor.SetIp        ( neighbor_config->GetIp());
+  neighbor.SetNetmask   ( neighbor_config->GetNetmask());
+  neighbor.SetDb        ( db );
+  neighbor.SetFreq      ( neighbor_config->GetFreq());
+  neighbor.SetCommunication( mod->GetCommunication());
+
+  link_data.neighbors.push_back(neighbor);
+}//end AppendLinkInfo
+
+bool ModelWifi::DoesLinkSucceed()
+{
+  //Take into account link success rate
+  //Generate a random number from 0-100.
+  //If the number is > the ssuccess rate, then skip this check
+  if (wifi_config.GetLinkSuccessRate() < 100)
+  {
+#ifdef HAVE_BOOST_RANDOM
+    int random_num = (*comm_reliability_gen)();
+#else
+    int random_num = rand() % 101;
+#endif
+    return !( random_num > wifi_config.GetLinkSuccessRate() );
+        }
+  return true;
+}
+
+void ModelWifi::CompareModels( Model * value, Model * user )
+{
+  // 'user' is a wifi device
+  Model *mod1 = (Model *)user;
+  Model *mod2 = (Model *)value;
+
+  // proceed if models are different and wifi devices
+  if ( (mod1->GetModelType() != mod2->GetModelType()) ||
+       (mod1 == mod2) ||
+       (mod1->IsRelated(mod2)) )
+  return;
+
+  // Get global poses of both devices
+  Pose pose1 = mod1->GetGlobalPose();
+  Pose pose2 = mod2->GetGlobalPose();
+
+  // mod2 is also a wifi device
+  ModelWifi *mod1_w = dynamic_cast<ModelWifi*>(mod1);
+  ModelWifi *mod2_w = dynamic_cast<ModelWifi*>(mod2);
+
+  WifiConfig * cfg1 = mod1_w->GetConfig();
+  WifiConfig * cfg2 = mod2_w->GetConfig();
+
+
+  double distance = hypot( pose2.x-pose1.x, pose2.y-pose1.y );
+
+  // store link information for reachable nodes
+  if ( cfg1->GetModel() == FRIIS )
+  {
+    double fsl = 32.44177+20*log10(cfg1->GetFreq())+20*log10(distance/1000);
+    if( fsl <= ( cfg1->GetPowerDbm()-cfg2->GetSensitivity() ) && DoesLinkSucceed())
+    {
+      mod1_w->AppendLinkInfo( mod2_w, cfg1->GetPowerDbm()-fsl );
+       }
+  }
+  else if ( cfg1->GetModel() == ITU_INDOOR  )
+  {
+    double loss = 20*log10( cfg1->GetFreq() )+( cfg1->GetPlc() )*log10( distance )-28;
+    if( loss <= ( cfg1->GetPowerDbm()-cfg2->GetSensitivity() ) && DoesLinkSucceed())
+    {
+      mod1_w->AppendLinkInfo( mod2_w, cfg1->GetPowerDbm()-loss );
+    }
+  }
+  else if( cfg1->GetModel() == LOG_DISTANCE )
+  {
+    double norand;
+    double loss_0 = 32.44177+20*log10( cfg1->GetFreq() ) + 20*log10( 0.001 ); // path loss in 1 meter
+#ifdef HAVE_BOOST_RANDOM
+    norand = (*normal_gen)();
+#else
+    norand = nrand( );
+#endif
+    double loss = loss_0+10*cfg1->GetPle()*log10( distance )+cfg1->GetSigma()*norand;
+
+    if( loss <= (cfg1->GetPowerDbm()-cfg2->GetSensitivity()) && DoesLinkSucceed())
+    {
+      mod1_w->AppendLinkInfo( mod2_w, cfg1->GetPowerDbm()-loss );
+    }
+  }
+  else if( cfg1->GetModel() == RAYTRACE  )
+  {
+    double loss = 20*log10( cfg1->GetFreq() ) + (cfg1->GetPlc())*log10( distance )-28;
+
+    if ( loss <= ( cfg1->GetPowerDbm() - cfg2->GetSensitivity() ) )
+    {
+      meters_t me = calc_distance( mod1, mod2, &pose1, &pose2 );
+      double new_loss = cfg1->GetWallFactor()*me*100+loss;
+
+      if ( new_loss <=( cfg1->GetPowerDbm()-cfg2->GetSensitivity())  &&DoesLinkSucceed())
+      {
+        mod1_w->AppendLinkInfo( mod2_w, cfg1->GetPowerDbm()-new_loss );
+      }
+    }
+  }
+  else if( cfg1->GetModel() == SIMPLE  )
+  {
+    if (cfg1->GetRange() >= distance && DoesLinkSucceed())
+    {
+      mod1_w->AppendLinkInfo( mod2_w, 1 );
+    }
+  }
+}//end compare_models
+
+
+void ModelWifi::Update( )
+{
+  // no work to do if we're unsubscribed
+  if( subs < 1 )
+    return;
+
+  // Discard old data
+  link_data.neighbors.clear();
+
+  //For all models out there with WIFI, compare to this model.
+  for(int i=0; i<world->num_models_with_wifi; ++i){
+      CompareModels(world->models_with_wifi.at(i), this);
+  }
+  /*FOR_EACH( other_model, *(world->GetModelsWithWifi()) )
+  {
+    CompareModels(*other_model, this);
+  }*/
+  Model::Update();
+}//end Update
+
+
+void ModelWifi::Startup(  void )
+{
+  Model::Startup();
+
+  PRINT_DEBUG( "wifi startup" );
+  // start consuming power
+  SetWatts( STG_WIFI_WATTS );
+}
+
+void ModelWifi::Shutdown( void )
+{
+  PRINT_DEBUG( "wifi shutdown" );
+  SetWatts( 0 );   // stop consuming power
+  //stg_model_fig_clear( mod, "wifi_cfg_fig" );
+  Model::Shutdown();
+}
diff --git a/libstage/stage.hh b/libstage/stage.hh
index d716e307b..7a501937a 100644
--- a/libstage/stage.hh
+++ b/libstage/stage.hh
@@ -1,9 +1,8 @@
-
 #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
@@ -26,7 +25,7 @@
 
 /** \file stage.hh
  *  Desc: External header file for the Stage library
- *  Author: Richard Vaughan (vaughan@sfu.ca) 
+ *  Author: Richard Vaughan (vaughan@sfu.ca)
  *  Date: 1 June 2003
  *  SVN: $Id$
  */
@@ -41,7 +40,8 @@
 #include <string.h>
 #include <sys/types.h>
 #include <sys/time.h>
-#include <pthread.h> 
+#include <pthread.h>
+#include <arpa/inet.h>
 
 // C++ libs
 #include <cmath>
@@ -52,6 +52,9 @@
 #include <set>
 #include <queue>
 #include <algorithm>
+#include <iterator>
+
+
 
 // FLTK Gui includes
 #include <FL/Fl.H>
@@ -66,10 +69,16 @@
 #include <OpenGL/glu.h>
 #else
 #include <GL/glu.h>
-#endif 
+#endif
+
+#include <config.h>
+#ifdef HAVE_BOOST_RANDOM
+#include <boost/random.hpp>
+#endif
+
 
 /** @brief The Stage library uses its own namespace */
-namespace Stg 
+namespace Stg
 {
   // forward declare
   class Block;
@@ -83,37 +92,43 @@ namespace Stg
   class Camera;
   class FileManager;
   class Option;
-  
+
   typedef Model* (*creator_t)( World*, Model*, const std::string& type );
-  
+
+#ifdef HAVE_BOOST_RANDOM
+  /** Define some types for the base boost random number generator. */
+  typedef boost::mt19937 BaseBoostGenerator;
+  typedef boost::mt19937 * BaseBoostGeneratorPtr;
+#endif
+
   /** 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[] =				       
+  const char COPYRIGHT[] =
     "Copyright Richard Vaughan and contributors 2000-2009";
 
   /** Author string */
-  const char AUTHORS[] =					
+  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[] =				       
+  const char DESCRIPTION[] =
     "Robot simulation library\nPart of the Player Project";
 
   /** Project distribution license string */
-  const char LICENSE[] = 
+  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"		\
@@ -146,24 +161,24 @@ namespace Stg
 
   /** 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;	 
+    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 };
 
@@ -193,21 +208,21 @@ namespace Stg
 
   /** 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( const std::string& name );
+
     Color();
-	
+
     bool operator!=( const Color& other ) const;
     bool operator==( const Color& other ) const;
     static Color RandomColor();
@@ -220,68 +235,68 @@ namespace Stg
 
     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, 
+
+    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*/}	
-	
+    {/*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, 
+    radians_t a;///< rotation about the z axis.
+
+    Pose( meters_t x,
+	  meters_t y,
 	  meters_t z,
-	  radians_t a ) 
+	  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*/ }		 
-    
+    { /*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, 
+    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, 
+		   0,
 		   normalize( drand48() * (2.0 * M_PI) ));
     }
-    
+
     /** Print pose in human-readable format on stdout
-	@param prefix Character string to prepend to pose output 
+	@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];
@@ -289,29 +304,29 @@ namespace Stg
 		x,y,z,a );
       return std::string(buf);
     }
-	
+
     /* returns true iff all components of the velocity are zero. */
-    bool IsZero() const 
+    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) );	 
-    }	
-	 
+		   normalize(a + p.a) );
+    }
+
     // a < b iff a is closer to the origin than b
     bool operator<( const Pose& p ) const
     {
@@ -319,29 +334,29 @@ namespace Stg
 	 // 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 && 
+      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 || 
+	       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
   {
@@ -352,12 +367,12 @@ namespace Stg
     meters_t range;
 
     RaytraceResult() : pose(), mod(NULL), color(), range(0.0) {}
-    RaytraceResult( const Pose& pose, Model* mod, const Color& color, const meters_t range) 
+    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
@@ -368,39 +383,39 @@ namespace Stg
 	@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, 
+    Velocity( double x,
+	      double y,
 	      double z,
 	      double a ) :
       Pose( x, y, z, a )
     { /*empty*/ }
-    
+
     Velocity()
-    { /*empty*/ }		 
-    
-    
+    { /*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 
-	
+	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 );		
-    }	 
+	      x,y,z,a );
+    }
   };
-  
- 
+
+
   /** Specify an object's basic geometry: position and rectangular
       size.  */
   class Geom
@@ -408,17 +423,17 @@ namespace Stg
   public:
     Pose pose;///< position
     Size size;///< extent
-    
+
     /** Print geometry in human-readable format on stdout, with a
-	prefix string 
-		  
+	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,
@@ -426,7 +441,7 @@ namespace Stg
 	      size.x,
 	      size.y );
     }
-	 
+
     /** Default constructor. Members pose and size use their default constructors. */
     Geom() : pose(), size() {}
 
@@ -439,7 +454,7 @@ namespace Stg
       size.Zero();
     }
   };
-  
+
   /** Bound a range of values, from min to max. min and max are initialized to zero. */
   class Bounds
   {
@@ -447,8 +462,8 @@ namespace Stg
     /// smallest value in range, initially zero
     double min;
     /// largest value in range, initially zero
-    double max; 
-    
+    double max;
+
     Bounds() : min(0), max(0) { /* empty*/  }
     Bounds( double min, double max ) : min(min), max(max) { /* empty*/  }
 
@@ -457,40 +472,40 @@ namespace Stg
     // 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; 
+    Bounds x;
     /// volume extent along y axis, initially zero
-    Bounds y; 
+    Bounds y;
     /// volume extent along z axis, initially zero
-    Bounds z; 
+    Bounds z;
 
     bounds3d_t() : x(), y(), z() {}
-    bounds3d_t( const Bounds& x, const Bounds& y, const Bounds& 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( 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) ); }  
+
+    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
@@ -499,30 +514,30 @@ namespace Stg
       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( 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( 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
     {
@@ -530,15 +545,15 @@ namespace Stg
       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
@@ -549,43 +564,43 @@ namespace Stg
     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, 
+    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, 
+    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, 
+    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 ) 
+    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; }	 
+    const std::string& GetWorldfileName() { return worldfile_name; }
   };
 
 
@@ -594,11 +609,11 @@ namespace Stg
   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 
+
+  typedef struct
   {
     int enabled;
     Pose pose;
@@ -608,32 +623,32 @@ namespace Stg
     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 
+
+  /** load the image file [filename] and convert it to a vector of polygons
    */
-  int polys_from_image_file( const std::string& filename, 
+  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, 
+  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) 
+  //#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,
@@ -645,19 +660,19 @@ namespace Stg
   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_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_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__)
@@ -665,8 +680,8 @@ namespace Stg
   // 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_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__)
@@ -682,8 +697,8 @@ namespace Stg
   // 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_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__)
@@ -705,7 +720,7 @@ namespace Stg
   class Ancestor
   {
     friend class Canvas; // allow Canvas access to our private members
-	 
+
   protected:
 
     /** array contains the number of each type of child model */
@@ -721,46 +736,46 @@ namespace Stg
     std::string token;
 
     Ancestor& Load( Worldfile* wf, int section );
-    void Save( Worldfile* wf, int section );	 
-	 	 	 
-  public:	
+    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 ) 
+      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 );		
+      std::map<std::string,void*>::iterator it = props.find( key );
       return( it == props.end() ? NULL : it->second );
     }
   };
-	
+
 
   class Ray
   {
@@ -771,15 +786,15 @@ namespace Stg
 
     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;		    
+    bool ztest;
   };
-		
+
 
   // defined in stage_internal.hh
   class Region;
@@ -792,16 +807,16 @@ namespace Stg
     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(); }
 
@@ -829,72 +844,108 @@ namespace Stg
     friend class ModelFiducial;
     friend class Canvas;
     friend class WorkerThread;
+    friend class ModelWifi;
 
-  public: 
+  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 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; 		
+    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 ); 
+      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  
+    /** Keep a list of all models with WIFI.  This avoids searching the whole
+        world for models with WIFI */
+    std::vector<Model*> models_with_wifi;
+    int num_models_with_wifi; // quick fix because of iterator problems
+
+  public:
+    /** Add a model to the set of models with Wifi, if not already there. */
+    void WifiInsert( Model* mod )
+    {
+      if ( mod )
+      {
+        //Try to find the model in the vector
+        std::vector<Model*>::iterator location_iterator = std::find( models_with_wifi.begin(), models_with_wifi.end(), mod );
+
+        //Add it to the vec if it's not already there
+        if ( location_iterator == models_with_wifi.end() ){
+          models_with_wifi.push_back(mod);
+          ++num_models_with_wifi;
+        }
+      }
+    }//WifiInsert
+
+    /** Remove a model from the list of models with wifi, if it exists. */
+    void WifiErase( Model* mod )
+    {
+      if ( mod ) {
+        EraseAll( mod, models_with_wifi);
+        --num_models_with_wifi;
+      }
+    }//WifiErase
+
+  private:
+
+
+
+    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
@@ -902,8 +953,8 @@ namespace Stg
     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:	 
+
+  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
@@ -916,7 +967,7 @@ namespace Stg
     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
 
@@ -925,7 +976,7 @@ namespace Stg
   public:
 
     uint64_t UpdateCount(){ return updates; }
-	 
+
     bool paused; ///< if true, the simulation is stopped
 
     virtual void Start(){ paused = false; };
@@ -933,7 +984,7 @@ namespace Stg
     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. */
@@ -957,15 +1008,15 @@ namespace Stg
 
     /** 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 );	 
+
+    Model* CreateModel( Model* parent, const std::string& typestr );
 
     void LoadModel(      Worldfile* wf, int entity );
     void LoadBlock(      Worldfile* wf, int entity );
@@ -973,7 +1024,7 @@ namespace Stg
     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,
@@ -983,95 +1034,107 @@ namespace Stg
     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 ) 
+    virtual void PushColor( Color col )
     { /* do nothing */  (void)col; };
-    virtual void PushColor( double r, double g, double b, double a ) 
+    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, 			 
+
+    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 Model* model,
 		   const void* arg,
-		   const bool ztest,		      
+		   const bool ztest,
 		   std::vector<RaytraceResult>& results );
-		
+
+    //Raytracing specific to the WIFI model.
+    meters_t RaytraceWifi( const Pose &gpose,
+              const meters_t range,
+              const ray_test_func_t func,
+              const Model* mod,
+              const Model* target,
+              const void* arg,
+              const bool ztest );
+
+    meters_t RaytraceWifi( const Ray& r,
+                               const Model* target );
+
     /** 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 ) 
+
+      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*. 
+
+      /** 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.  
+	thread is 0.
 
 	@param delay The time from now until the event occurs, in
 	microseconds.
@@ -1081,18 +1144,18 @@ namespace Stg
     */
     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
@@ -1107,33 +1170,33 @@ namespace Stg
 
   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 
+    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", 
+
+    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
@@ -1153,12 +1216,12 @@ namespace Stg
 	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; }
 
@@ -1170,37 +1233,37 @@ namespace Stg
 
     /** 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 */ 
+	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 );	
-	 
+    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
   {
@@ -1211,33 +1274,33 @@ namespace Stg
     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 ); 	 
-    
+    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 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(); 
+    void DrawFootPrint();
 
     /** Translate all points in the block by the indicated amounts */
-    void Translate( double x, double y );	 
+    void Translate( double x, double y );
 
     /** Return the center of the block on the X axis */
     double CenterX();
@@ -1252,38 +1315,38 @@ namespace Stg
     void SetCenterY( double y );
 
     /** Set the center of the block */
-    void SetCenter( double x, double y);	 
+    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(); 
+    Model* TestCollision();
 
-    void Load( Worldfile* wf, int entity );  
-    
-    void Rasterize( uint8_t* data, 
-		    unsigned int width, unsigned int height,		
+    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.*/  
+	bitmap layers.*/
     std::vector<Cell*> rendered_cells[2];
 
     void DrawTop();
     void DrawSides();
   };
 
-	
+
   class BlockGroup
   {
     friend class Model;
@@ -1300,34 +1363,34 @@ namespace Stg
   private:
     void AppendBlock( const Block& block );
 
-    void CalcSize();	 
+    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, 
+    void Rasterize( uint8_t* data,
 		    unsigned int width, unsigned int height,
-		    meters_t cellwidth, meters_t cellheight );	 
+		    meters_t cellwidth, meters_t cellheight );
 
     /** Draw the block in OpenGL as a solid single color. */
-    void DrawSolid( const Geom &geom); 
+    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
@@ -1340,10 +1403,10 @@ namespace Stg
   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]; }; 
+    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;
@@ -1352,13 +1415,13 @@ namespace Stg
     void DrawFootPrint( const Geom &geom);
   };
 
-  class Camera 
+  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() { }
@@ -1368,11 +1431,11 @@ namespace Stg
 
     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;
 
@@ -1399,7 +1462,7 @@ namespace Stg
 
     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 );
@@ -1412,7 +1475,7 @@ namespace Stg
     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 ) );
     }
@@ -1420,13 +1483,13 @@ namespace Stg
     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:
@@ -1435,25 +1498,25 @@ namespace Stg
     double _pixels_height;
     double _y_min;
     double _y_max;
-  
+
   public:
-    OrthoCamera( void ) : 
+    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, 
+    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;	}
@@ -1469,15 +1532,15 @@ namespace Stg
       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 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 );
   };
@@ -1486,7 +1549,7 @@ namespace Stg
   /** Extends World to implement an FLTK / OpenGL graphical user
       interface.
   */
-  class WorldGui : public World, public Fl_Window 
+  class WorldGui : public World, public Fl_Window
   {
     friend class Canvas;
     friend class ModelCamera;
@@ -1494,15 +1557,15 @@ namespace Stg
     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; 
+    double speedup;
 
     Fl_Menu_Bar* mbar;
     OptionsDlg* oDlg;
@@ -1511,19 +1574,19 @@ namespace Stg
     /** 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; 
+    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 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 );
@@ -1539,26 +1602,27 @@ namespace Stg
     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();
 
@@ -1566,28 +1630,28 @@ namespace Stg
     virtual void Redraw( void );
 
     virtual std::string ClockString() const;
-    virtual bool Update();	
+    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 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; } 
+
+    Canvas* GetCanvas( void ) const { return canvas; }
 
     /** show the window - need to call this if you don't Load(). */
-    void Show(); 
+    void Show();
 
     /** Get human readable string that describes the current global energy state. */
-    std::string EnergyString( void ) const;	
-    virtual void RemoveChild( Model* mod );	 
+    std::string EnergyString( void ) const;
+    virtual void RemoveChild( Model* mod );
 
     bool IsTopView();
   };
@@ -1596,7 +1660,7 @@ namespace Stg
   class StripPlotVis : public Visualizer
   {
   private:
-	 
+
     Model* mod;
     float* data;
     size_t len;
@@ -1604,14 +1668,14 @@ namespace Stg
     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, 
+    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 );		
+    virtual void Visualize( Model* mod, Camera* cam );
     void AppendValue( float value );
   };
 
@@ -1620,52 +1684,52 @@ namespace Stg
   {
     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; 
+
+      static joules_t global_peak_value;
 
     public:
-      DissipationVis( meters_t width, 
-		      meters_t height, 
+      DissipationVis( meters_t width,
+		      meters_t height,
 		      meters_t cellsize );
 
       virtual ~DissipationVis();
-      virtual void Visualize( Model* mod, Camera* cam );		
-		
+      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;
@@ -1674,27 +1738,27 @@ namespace Stg
   public:
     static joules_t global_stored;
     static joules_t global_capacity;
-    static joules_t global_dissipated;	 
+    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 );	 
+    void TransferTo( PowerPack* dest, joules_t amount );
 
     double ProportionRemaining() const
     { return( stored / capacity ); }
@@ -1702,33 +1766,33 @@ namespace Stg
     /** 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 ); 
-    }		
-	 
+      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 );	
+    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
   {
@@ -1743,7 +1807,7 @@ namespace Stg
     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;
@@ -1754,7 +1818,7 @@ namespace Stg
 
     std::vector<Option*> drawOptions;
     const std::vector<Option*>& getOptions() const { return drawOptions; }
-	 
+
   protected:
 
     /** If true, the model always has at least one subscription, so
@@ -1767,7 +1831,7 @@ namespace Stg
 	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:
@@ -1776,29 +1840,29 @@ namespace Stg
     public:
       model_callback_t callback;
       void* arg;
-			
-      cb_t( model_callback_t cb, void* arg ) 
+
+      cb_t( model_callback_t cb, void* arg )
 	: callback(cb), arg(arg) {}
-			
-      cb_t( world_callback_t cb, void* 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)); 
+	return ((void*)(callback)) < ((void*)(other.callback));
       }
-		 
+
       /** for searching in a sorted container */
       bool operator==( const cb_t& other ) const
-      { return( callback == other.callback);  }			
+      { return( callback == other.callback);  }
     };
-		
+
     class Flag
     {
     private:
@@ -1809,13 +1873,13 @@ namespace Stg
     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 );
@@ -1837,16 +1901,16 @@ namespace Stg
       //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. */
@@ -1855,18 +1919,18 @@ namespace Stg
     /** 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; 
+    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;
@@ -1879,24 +1943,24 @@ namespace Stg
       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	 
+    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  
+    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; 
+    Model* parent;
 
     /** The pose of the model in it's parents coordinate frame, or the
 	global coordinate frame is the parent is NULL. */
@@ -1908,7 +1972,7 @@ namespace Stg
     /** 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
     {
@@ -1917,29 +1981,29 @@ namespace Stg
       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, 
+
+      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 
-		
+    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
@@ -1949,22 +2013,22 @@ namespace Stg
 	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 
-    {																							
+    class TrailItem
+    {
     public:
       usec_t time;
       Pose pose;
       Color color;
-		
-      TrailItem() 
+
+      TrailItem()
 	: time(0), pose(), color(){}
-		 
-      //TrailItem( usec_t time, Pose pose, Color 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;
 
@@ -1975,61 +2039,61 @@ namespace Stg
 	be set in the world file using the trail_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;  
+    //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()  
-	
+    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 ) 
+      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;}	 
+    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:
@@ -2039,24 +2103,24 @@ namespace Stg
       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, 
+    void Rasterize( uint8_t* data,
 		    unsigned int width, unsigned int height,
 		    meters_t cellwidth, meters_t cellheight );
-	
-  private: 
+
+  private:
     /** Private copy constructor declared but not defined, to make it
 	impossible to copy models. */
     explicit Model(const Model& original);
@@ -2071,13 +2135,13 @@ namespace Stg
     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. */		
+	calls TestCollision() on all descendents. */
     Model* TestCollision();
-  
+
     void CommitTestedPose();
 
     void Map( unsigned int layer );
@@ -2092,7 +2156,7 @@ namespace Stg
 
     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 );
@@ -2100,7 +2164,7 @@ namespace Stg
     /** 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 meters_t range,
 			     const ray_test_func_t func,
 			     const void* arg,
 			     const bool ztest )
@@ -2112,12 +2176,12 @@ namespace Stg
 			      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 meters_t range,
+     		   const radians_t fov,
      		   const ray_test_func_t func,
      		   const void* arg,
      		   const bool ztest,
@@ -2129,12 +2193,12 @@ namespace Stg
 			      func,
 			      this,
 			      arg,
-			      ztest,      
+			      ztest,
 			      results );
     }
-      
+
     virtual void UpdateCharge();
-		
+
     static int UpdateWrapper( Model* mod, void* arg ){ mod->Update(); return 0; }
 
     /** Calls CallCallback( CB_UPDATE ) */
@@ -2148,20 +2212,20 @@ namespace Stg
     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 DataVisualize( Camera* cam );
     virtual void DrawSelected(void);
-	
+
     void DrawTrailFootprint();
     void DrawTrailBlocks();
     void DrawTrailArrows();
@@ -2170,45 +2234,45 @@ namespace Stg
     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 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, 
+
+    //void RecordRenderPoint( GSList** head, GSList* link,
     //					unsigned int* c1, unsigned int* c2 );
 
-    void PlaceInFreeSpace( meters_t xmin, meters_t xmax, 
+    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, 
+    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() 
+	 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 );
 
@@ -2224,28 +2288,28 @@ namespace Stg
       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
@@ -2255,15 +2319,15 @@ namespace Stg
     /** 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();
 
@@ -2273,89 +2337,89 @@ namespace Stg
 
     /** 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, 
+    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 );
@@ -2375,74 +2439,74 @@ namespace Stg
     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 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;		
+    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 ); }	 
+    bool HasSubscribers() const { return( subs > 0 ); }
 
-    static std::map< std::string, creator_t> name_map;	 
+    static std::map< std::string, creator_t> name_map;
 
   protected:
     virtual void Startup();
     virtual void Shutdown();
-    virtual void Update();	 						
+    virtual void Update();
   };
 
 
@@ -2461,7 +2525,7 @@ namespace Stg
     };
 
     /** Visualize blobinder data in the GUI. */
-    class Vis : public Visualizer 
+    class Vis : public Visualizer
     {
     public:
       Vis( World* world );
@@ -2488,19 +2552,19 @@ namespace Stg
     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
@@ -2524,17 +2588,17 @@ namespace Stg
 
 
 
-  
+
 
   // Light indicator model
   class ModelLightIndicator : public Model
   {
   public:
-    ModelLightIndicator( World* world, 
+    ModelLightIndicator( World* world,
 			 Model* parent,
 			 const std::string& type );
     ~ModelLightIndicator();
-  
+
     void SetState(bool isOn);
 
   protected:
@@ -2553,49 +2617,49 @@ namespace Stg
 
     enum paddle_state_t {
       PADDLE_OPEN = 0, // default state
-      PADDLE_CLOSED, 
+      PADDLE_CLOSED,
       PADDLE_OPENING,
       PADDLE_CLOSING,
     };
-	 
+
     enum lift_state_t {
       LIFT_DOWN = 0, // default state
-      LIFT_UP, 
+      LIFT_UP,
       LIFT_UPPING, // verbed these to match the paddle state
-      LIFT_DOWNING, 
+      LIFT_DOWNING,
     };
-	 
+
     enum cmd_t {
       CMD_NOOP = 0, // default state
-      CMD_OPEN, 
+      CMD_OPEN,
       CMD_CLOSE,
-      CMD_UP, 
-      CMD_DOWN    
+      CMD_UP,
+      CMD_DOWN
     };
-	 
-	 
-    /** gripper configuration 
+
+
+    /** gripper configuration
      */
     struct config_t
     {
-      Size paddle_size; ///< paddle dimensions 
+      Size paddle_size; ///< paddle dimensions
       paddle_state_t paddles;
-      lift_state_t lift;		
+      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.		
+      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		
+      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();
@@ -2603,13 +2667,13 @@ namespace Stg
 
     config_t cfg;
     cmd_t cmd;
-	 
+
     Block* paddle_left;
     Block* paddle_right;
 
     static Option showData;
 
-  public:	 
+  public:
     static const Size size;
 
     // constructor
@@ -2618,16 +2682,16 @@ namespace Stg
 		  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 ). */
@@ -2661,18 +2725,18 @@ namespace Stg
   /// %ModelFiducial class
   class ModelFiducial : public Model
   {
-  public:  
+  public:
     /** Detected fiducial data */
     class Fiducial
     {
     public:
       meters_t range; ///< range to the target
-      radians_t bearing; ///< bearing 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_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.  
+      int id; ///< the fiducial identifier of the target (i.e. its fiducial_return value), or -1 if none can be detected.
     };
 
   private:
@@ -2684,29 +2748,29 @@ namespace Stg
 
     static Option showData;
     static Option showFov;
-	 
+
     std::vector<Fiducial> fiducials;
-		
-  public:		
-    ModelFiducial( World* world, 
+
+  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 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?	
-		
+    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 )
     {
@@ -2714,10 +2778,10 @@ namespace Stg
       return &fiducials[0];
     }
   };
-	
-	
+
+
   // RANGER MODEL --------------------------------------------------------
-	
+
   /// %ModelRanger class
   class ModelRanger : public Model
   {
@@ -2725,51 +2789,51 @@ namespace Stg
   public:
     ModelRanger( World* world, Model* parent, const std::string& type );
     virtual ~ModelRanger();
-		
+
     virtual void Print( char* prefix ) const;
-		
-    class Vis : public Visualizer 
+
+    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 ){} 
+      static Option showTransducers;
+
+      Vis( World* world );
+      virtual ~Vis( void ){}
       virtual void Visualize( Model* mod, Camera* cam );
     } vis;
-		
+
     class Sensor
-    {		
+    {
     public:
-      Pose pose; 
+      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 ), 
+
+      Sensor() : pose( 0,0,0,0 ),
 		 size( 0.02, 0.02, 0.02 ), // teeny transducer
 		 range( 0.0, 5.0 ),
-		 fov( 0.1 ), 
+		 fov( 0.1 ),
 		 sample_count(1),
 		 color( Color(0,0,1,0.15)),
 		 ranges(),
 		 intensities(),
 		 bearings()
       {}
-			
-      void Update( ModelRanger* rgr );			
+
+      void Update( ModelRanger* rgr );
       void Visualize( Vis* vis, ModelRanger* rgr ) const;
-      std::string String() const;			
+      std::string String() const;
       void Load( Worldfile* wf, int entity );
     };
 
@@ -2778,21 +2842,21 @@ namespace Stg
     { return sensors; }
 
     /** returns a mutable reference to a vector of range and reflectance samples */
-    std::vector<Sensor>& GetSensorsMutable() 
+    std::vector<Sensor>& GetSensorsMutable()
     { return sensors; }
-	 
+
     void LoadSensor( Worldfile* wf, int entity );
-		
+
   private:
-    std::vector<Sensor> sensors;		
-    
+    std::vector<Sensor> sensors;
+
   protected:
-		
+
     virtual void Startup();
     virtual void Shutdown();
-    virtual void Update();		
+    virtual void Update();
   };
-	
+
   // BLINKENLIGHT MODEL ----------------------------------------------------
   class ModelBlinkenlight : public Model
   {
@@ -2809,25 +2873,25 @@ namespace Stg
 		       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 
+    typedef struct
     {
       // GL_V3F
       GLfloat x, y, z;
     } ColoredVertex;
-  
+
   private:
     Canvas* _canvas;
 
@@ -2836,60 +2900,60 @@ namespace Stg
 
     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 ); 
-	  
+		 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; }
   };
@@ -2906,36 +2970,36 @@ namespace Stg
     /** Define a position  control method */
     typedef enum
       { CONTROL_ACCELERATION,
-	CONTROL_VELOCITY, 
-	CONTROL_POSITION 
+	CONTROL_VELOCITY,
+	CONTROL_POSITION
       } ControlMode;
-	 
+
     /** Define a localization method */
     typedef enum
-      { LOCALIZATION_GPS, 
-	LOCALIZATION_ODOM 
+      { LOCALIZATION_GPS,
+	LOCALIZATION_ODOM
       } LocalizationMode;
-	 
+
     /** Define a driving method */
     typedef enum
-      { DRIVE_DIFFERENTIAL, 
-	DRIVE_OMNI, 
-	DRIVE_CAR 
+      { 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;	 
+    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];
 
@@ -2964,11 +3028,11 @@ namespace Stg
       Waypoint( const Pose& pose, Color color ) ;
       Waypoint();
       void Draw() const;
-		
+
       Pose pose;
       Color color;
     };
-	 
+
     std::vector<Waypoint> waypoints;
 
     class WaypointVis : public Visualizer
@@ -2978,7 +3042,7 @@ namespace Stg
       virtual ~WaypointVis( void ){}
       virtual void Visualize( Model* mod, Camera* cam );
     } wpvis;
-	 
+
     class PoseVis : public Visualizer
     {
     public:
@@ -2989,7 +3053,7 @@ namespace Stg
 
     /** 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 );
@@ -3005,7 +3069,7 @@ namespace Stg
 	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) */
@@ -3036,13 +3100,13 @@ namespace Stg
       { 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;
@@ -3055,39 +3119,349 @@ namespace Stg
     ControlMode control_mode;
     ActuatorType actuator_type;
     point3_t axis;
-  
+
     Pose InitialPose;
-  public:  
+  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; }
   };
 
+  // WIFI MODEL --------------------------------------------------------
+  // Forward declare the wifi model so the comm system can make use of it.
+  class ModelWifi;
+  /**
+   * A base class for messages being passed between robots via Wifi.
+   *
+   */
+  class WifiMessageBase
+  {
+    friend class ModelWifi;
+
+    protected:
+      std::string sender_mac;
+      in_addr_t sender_ip_in;
+      in_addr_t sender_netmask_in;
+      uint32_t sender_id;
+
+      in_addr_t recipient_ip_in;
+      in_addr_t intended_recipient_ip_in;
+      in_addr_t recipient_netmask_in;
+      uint32_t  recipient_id;
+
+      std::string message;
+
+    public:
+      WifiMessageBase(){};
+      WifiMessageBase(const WifiMessageBase& toCopy);           ///< Copy constructor
+      WifiMessageBase& operator=(const WifiMessageBase& toCopy);///< Equals operator overload
+      virtual ~WifiMessageBase(){};
+
+      /**
+       * Clone a copy of the wifi message.  Used for making a deep copy of a wifi message and its subclasses
+       * when handling a pointer to the wifi message base class.
+       */
+      virtual WifiMessageBase* Clone() { return new WifiMessageBase(*this); };
+
+      inline void SetMessage(const std::string & value) { message = value; };
+      inline const std::string & GetMessage() { return message; };
+
+      inline void SetSenderMac(const std::string & value) { sender_mac = value; };
+      inline const std::string & GetSenderMac() { return sender_mac; };
+
+      inline void SetSenderIp(in_addr_t value) { sender_ip_in = value; };
+      inline in_addr_t GetSenderIp() { return sender_ip_in; };
+
+      inline void SetSenderNetmask(in_addr_t value) { sender_netmask_in = value; };
+      inline in_addr_t GetSenderNetmask() { return sender_netmask_in; };
+
+      inline void SetSenderId(uint32_t value) { sender_id = value; };
+      inline uint32_t GetSenderId() { return sender_id; };
+
+      void SetBroadcastIp();
+      inline bool IsBroadcastIp() { return (recipient_ip_in & ~recipient_netmask_in) == ~recipient_netmask_in; };
+      inline void SetRecipientIp(in_addr_t value) { recipient_ip_in = value; };
+      inline in_addr_t GetRecipientIp() {return recipient_ip_in; };
+      inline void SetIntendedRecipientIp(in_addr_t value) { intended_recipient_ip_in = value; };
+      inline in_addr_t GetIntendedRecipientIp() {return intended_recipient_ip_in; };
+
+      inline void SetRecipientNetmask(in_addr_t value) { recipient_netmask_in = value; };
+      inline in_addr_t GetRecipientNetmask() { return recipient_netmask_in; };
+
+      inline void SetRecipientId(uint32_t value) { recipient_id = value; };
+      inline uint32_t GetRecipientId() {return recipient_id; };
+
+  };//end class WifiMessageBase
+
+ /**
+  * Interface for inter-robot communication
+  */
+  typedef void (*stg_comm_rx_msg_fn_t)(WifiMessageBase * msg , void * arg);
+
+  /**
+   * Communication class implements call-back based message passing for the Wifi model.
+   */
+  class Communication
+  {
+    private:
+    stg_comm_rx_msg_fn_t ReceiveMsgFn;
+
+    void *arg;
+
+    ModelWifi * wifi_p; ///< Pointer to the Wifi Model that this comm class is associated with
+
+    void SetMessageSender(WifiMessageBase * to_send); /// <<Set the sender props based on the fact we're sending it.
+    void SendMessage(WifiMessageBase * to_send); ///< Send wifi message to message addressee, taking into account robots in range.
+
+    public:
+    Communication( ModelWifi * parent_p );
+    ~Communication();
+
+    void SendUnicastMessage(WifiMessageBase * to_send); ///< Send wifi message to specifc recipient if they're in range.
+    void SendBroadcastMessage(WifiMessageBase * to_send); ///< Send wifi message to all robots in range.
+
+    bool IsReceiveMsgFnSet() { return this->ReceiveMsgFn; }; ///< Determine if receive message is set for robot.
+    void CallReceiveMsgFn(WifiMessageBase * msg) { this->ReceiveMsgFn( msg, this->arg ); } ///< Call message receive Fn.
+    void SetReceiveMsgFn( stg_comm_rx_msg_fn_t rxfn, void * arg) { this->ReceiveMsgFn = rxfn; this->arg = arg; } ///< Set the receive message fn.
+
+  };//end class Communication
+
+
+  /**
+   * Enumerated type defining the different possible wifi models we can use.
+   */
+  enum WifiModelTypeEnum { FRIIS, ITU_INDOOR, LOG_DISTANCE, RAYTRACE, SIMPLE };
+
+  /**
+   * This class encapsulates the various configuration parameters of a robot using the simulated WIFI.
+   */
+  class WifiConfig
+  {
+    protected:
+      static const std::string MODEL_NAME_FRIIS;        ///< String constant used when parsing world file.
+      static const std::string MODEL_NAME_ITU_INDOOR;   ///< String constant used when parsing world file.
+      static const std::string MODEL_NAME_LOG_DISTANCE; ///< String constant used when parsing world file.
+      static const std::string MODEL_NAME_RAYTRACE;     ///< String constant used when parsing world file.
+      static const std::string MODEL_NAME_SIMPLE;       ///< String constant used when parsing world file.
+
+      std::string essid;    ///< Wifi network ESSID
+      std::string mac;      ///< MAC of the current WIFI interface
+      in_addr_t ip_in;      ///< IP address of the wifi interface
+      in_addr_t netmask_in; ///< Netmask of the inet address.
+      double power;         ///< Wifi transmission power
+      double sensitivity;   ///< Wifi transmission sensitivity
+      double freq;          ///< Wifi frequency
+      meters_t range;   ///< Range
+      WifiModelTypeEnum model; ///< Model type
+      double plc;           ///< distance power loss coefficient
+      double ple;           ///< path loss distance exponent
+      double sigma;         ///< standard derivation of gaussian random variable
+      double range_db;      ///< setting the individual ranges to render in render_cfg
+      double wall_factor;   ///< Wall factor for use in raytracing
+      double power_dbm;     ///< Power loss in db/m
+
+      /**
+       * The link success rate detemines the percent of time that
+       * links between two robots will be succesfully established.
+       * It can be used to simulate conditions where WIFI operation
+       * is erratic and prone to random failures.
+       * 100 = completely reliable linkups
+       * 0 = completely unreliable linkups
+       */
+      int link_success_rate;
+    public:
+      WifiConfig();
+
+      // All the setters
+      inline void SetEssid(const std::string & value) { essid = value; };
+      inline void SetMac(const std::string & value) {mac = value; };
+      inline void SetIp(const in_addr_t value) {ip_in = value; };
+      void        SetIp(const std::string & value) { ip_in = inet_addr(value.c_str()); };
+      inline void SetNetmask(const in_addr_t value) {netmask_in = value;};
+      void        SetNetmask(const std::string & value) { netmask_in = inet_addr(value.c_str()); };
+      inline void SetPower(const double value) {power = value; };
+      inline void SetSensitivity(const double value) {sensitivity = value; };
+      inline void SetFreq(const double value) {freq = value; };
+      inline void SetRange(const meters_t value) { range = value; };
+      inline void SetModel(WifiModelTypeEnum value) {model = value; };
+      void        SetModel(const std::string & value);
+      inline void SetPlc(const double value) {plc = value; };
+      inline void SetPle(const double value) {ple = value; };
+      inline void SetSigma(const double value) {sigma=value;};
+      inline void SetRangeDb(const double value) {range_db=value;};
+      inline void SetWallFactor(const double value) {wall_factor=value;};
+      inline void SetPowerDbm(const double value) {power_dbm = value;};
+      inline void SetLinkSuccessRate(const int value) { link_success_rate = value;};
+
+      //All the getters
+      inline std::string & GetEssid() { return essid; };
+      inline std::string & GetMac() { return mac; };
+      inline in_addr_t GetIp() { return ip_in; };
+      std::string  GetIpString() { std::string addr_str( inet_ntoa(*(struct in_addr *)&ip_in)); return addr_str; };
+      inline in_addr_t GetNetmask() { return netmask_in; };
+      std::string  GetNetmaskString() { std::string addr_str( inet_ntoa(*(struct in_addr *)&netmask_in)); return addr_str; };
+      inline double GetPower() { return power; };
+      inline double GetSensitivity() { return sensitivity; };
+      inline double GetFreq() { return freq; };
+      inline meters_t GetRange() { return range; };
+      inline WifiModelTypeEnum GetModel() { return model; };
+      const std::string & GetModelString();
+      inline double GetPlc() { return plc; };
+      inline double GetPle() { return ple; };
+      inline double GetSigma() { return sigma; };
+      inline double GetRangeDb() { return range_db; };
+      inline double GetWallFactor() { return wall_factor;};
+      inline double GetPowerDbm() { return power_dbm;};
+      inline int GetLinkSuccessRate() { return link_success_rate;};
+  };//end WifiConfig
+
+  /**
+   * Information about a robot that is the wifi neighbor of the current robot.
+   */
+  class WifiNeighbor
+  {
+    protected:
+      uint32_t id;            ///< id of neighbor
+      Pose pose;              ///< global pose of corresponding neighbour.
+      std::string essid;      ///< essid of the neighbor
+      std::string mac;        ///< mac of the neighbor
+      in_addr_t ip_in;        ///< ip address of the neighbor
+      in_addr_t netmask_in;   ///< netmask of the neighbor
+      double freq;            ///< Neighbor's frequency
+      double db;              ///< Neighbor's signal-strength
+
+      Communication * comm;   ///< Neighbor's comm interface
+    public:
+      WifiNeighbor() { comm = NULL; };
+      ~WifiNeighbor() { comm = NULL; essid.clear(); mac.clear();};
+
+      //All the setters
+      inline void SetId(const uint32_t value) { id = value; };
+      inline void SetPose(const Pose & value) { pose = value; };
+      inline void SetEssid(const std::string & value) { essid = value; };
+      inline void SetMac(const std::string & value) { mac = value; };
+      inline void SetIp(const in_addr_t value) { ip_in = value; };
+      inline void SetNetmask(const in_addr_t value) { netmask_in = value; };
+      inline void SetFreq(const double value) { freq = value; };
+      inline void SetDb(const double value) {db = value; };
+      inline void SetCommunication(Communication * value) { comm = value; };
+
+      //All the geters
+      inline uint32_t GetId() { return id; };
+      inline Pose & GetPose() { return pose; };
+      inline std::string & GetEssid() { return essid; };
+      inline std::string & GetMac() { return mac; };
+      inline in_addr_t GetIp() { return ip_in; };
+      inline in_addr_t GetNetmask() { return netmask_in; };
+      inline double GetFreq() { return freq; };
+      inline double GetDb() { return db; };
+      inline Communication * GetCommunication() { return comm; };
+  };
+
+  /**
+   * Typedef defining a vector of neighbors
+   */
+  typedef std::vector<WifiNeighbor> WifiNeighborVec;
+
+  /** Wifi link information.
+  */
+  typedef struct
+  {
+    WifiNeighborVec neighbors;   ///< Information about each radio neighbor to the current wifi.
+  } stg_wifi_data_t;
+
+  /**
+   * The Wifi Model class.
+   */
+  class ModelWifi : public Model
+  {
+    friend class Canvas;
+    friend class Communication;
+
+    private:
+    WifiConfig      wifi_config;   /* Wifi configuration parameters for the current WIFI interace. */
+    stg_wifi_data_t link_data;     /* Link information for each neighbor */
+
+    void CompareModels( Model * value, Model * user );
+    bool DoesLinkSucceed();
+    std::string GetUniqueMac( World* world );
+    static Option showWifi;
+
+    unsigned int random_seed; ///< seed for the random number generator
+
+#ifdef HAVE_BOOST_RANDOM
+    // Note: Although boost is not strictly required for random number generation, the quality of the random
+    // numbers available using the boost library is generally going to be higher.
+
+    // We will use a random number generator unique to EACH
+    BaseBoostGenerator random_generator; ///< Boost random number generator
+
+    // Define a uniform random number distribution of integer values between
+    // 1 and 6 inclusive.
+    typedef boost::uniform_int<> uniform_distribution_type;
+    typedef boost::variate_generator<BaseBoostGenerator&, uniform_distribution_type> BoostRandomIntGenerator;
+
+    // Define a normal distribution of doubles for the log distance function.
+    typedef boost::normal_distribution<> normal_distribution_type;
+    typedef boost::variate_generator<BaseBoostGenerator&, normal_distribution_type> BoostNormalGenerator;
+
+    //Define the communication and normal distribution random number generators.
+    BoostRandomIntGenerator * comm_reliability_gen;
+    BoostNormalGenerator * normal_gen;
+#endif
+
+    public:
+    Communication comm;           ///< Inter-robot communication interface.
+
+    // constructor
+    ModelWifi( World* world, Model* parent, const std::string& type );
+    // destructor
+    ~ModelWifi();
+
+    virtual void Startup();
+    virtual void Shutdown();
+    virtual void Update();
+    virtual void Load();
+    virtual void DataVisualize( Camera* cam );
+
+    /** returns the quality of link to each neighbor */
+    stg_wifi_data_t* GetLinkInfo( );
+
+    /** Append link information to the light of neighbors */
+    void AppendLinkInfo( ModelWifi* mod, double db );
+
+    // Get the user-tweakable configuration of the wifi model
+    WifiConfig * GetConfig( ) { return &wifi_config; };
+
+    Communication * GetCommunication( ) { return &comm; };
+
+  };//end class ModelWifi
+
 
 }; // end namespace stg
 
diff --git a/libstage/typetable.cc b/libstage/typetable.cc
index 59cec0f51..0fe1c476f 100644
--- a/libstage/typetable.cc
+++ b/libstage/typetable.cc
@@ -11,7 +11,7 @@ Model* Creator( World* world, Model* parent, const std::string& type )
 
 static void Register( const std::string& type, creator_t func )
 {
-  Model::name_map[type] = func;  
+  Model::name_map[type] = func;
 }
 
 /** Map model names to named constructors for each model type */
@@ -19,7 +19,7 @@ void Stg::RegisterModels()
 {
   // generic model
   Register( "model", Creator<Model> );
-  
+
   Register( "actuator",       Creator<ModelActuator> );
   Register( "blinkenlight",   Creator<ModelBlinkenlight> );
   Register( "blobfinder",     Creator<ModelBlobfinder> );
@@ -29,5 +29,6 @@ void Stg::RegisterModels()
   Register( "lightindicator", Creator<ModelLightIndicator> );
   Register( "position",       Creator<ModelPosition> );
   Register( "ranger",         Creator<ModelRanger> );
-}  
+  Register( "wifi",           Creator<ModelWifi> );
+}
 
diff --git a/libstage/world.cc b/libstage/world.cc
index df1788b09..858175eeb 100644
--- a/libstage/world.cc
+++ b/libstage/world.cc
@@ -1,6 +1,6 @@
 
 /**
-   $Id$ 
+   $Id$
 **/
 
 /** @defgroup world World
@@ -46,7 +46,7 @@
     elapsed. In libstage, World::Update() returns true. In Stage with
     a GUI, the simulation is paused.wo In Stage without a GUI, Stage
     quits.
- 
+
     - resolution <float>\n
     The resolution (in meters) of the underlying bitmap model. Larger
     values speed up raytracing at the expense of fidelity in collision
@@ -70,7 +70,7 @@
     parallel-enabled high-resolution models, e.g. a laser with
     hundreds or thousands of samples, or lots of models. Defaults to
     1. Values of less than 1 will be forced to 1.
-	 
+
     @par More examples
     The Stage source distribution contains several example world files in
     <tt>(stage src)/worlds</tt> along with the worldfile properties
@@ -81,12 +81,12 @@
 #define _GNU_SOURCE
 #endif
 
-//#define DEBUG 
+//#define DEBUG
 
 #include <stdlib.h>
 #include <assert.h>
 #include <string.h> // for strdup(3)
-#include <locale.h> 
+#include <locale.h>
 #include <limits.h>
 #include <libgen.h> // for dirname(3)
 
@@ -101,16 +101,16 @@ using namespace Stg;
 bool World::ltx::operator()(const Model* a, const Model* b) const
 {
   const meters_t ax( a->GetGlobalPose().x );
-  const meters_t bx( b->GetGlobalPose().x );  
+  const meters_t bx( b->GetGlobalPose().x );
   // break ties using the pointer value to give a unique ordering
-  return ( ax == bx ? a < b : ax < bx ); 
+  return ( ax == bx ? a < b : ax < bx );
 }
 bool World::lty::operator()(const Model* a, const Model* b) const
 {
   const meters_t ay( a->GetGlobalPose().y );
-  const meters_t by( b->GetGlobalPose().y );  
+  const meters_t by( b->GetGlobalPose().y );
   // break ties using the pointer value ro give a unique ordering
-  return ( ay == by ? a < b : ay < by ); 
+  return ( ay == by ? a < b : ay < by );
 }
 
 // static data members
@@ -120,9 +120,9 @@ std::set<World*> World::world_set;
 std::string World::ctrlargs;
 std::vector<std::string> World::args;
 
-World::World( const std::string& name, 
+World::World( const std::string& name,
 	      double ppm )
-  : 
+  :
   // private
   destroy( false ),
   dirty( true ),
@@ -131,6 +131,7 @@ World::World( const std::string& name,
   models_with_fiducials(),
   models_with_fiducials_byx(),
   models_with_fiducials_byy(),
+  num_models_with_wifi(0),
   ppm( ppm ), // raytrace resolution
   quit( false ),
   show_clock( false ),
@@ -139,17 +140,17 @@ World::World( const std::string& name,
   threads_working( 0 ),
   threads_start_cond(),
   threads_done_cond(),
-  total_subs( 0 ), 
+  total_subs( 0 ),
   worker_threads( 1 ),
 
   // protected
   cb_list(),
   extent(),
-  graphics( false ), 
+  graphics( false ),
   option_table(),
   powerpack_list(),
   quit_time( 0 ),
-  ray_list(),  
+  ray_list(),
   sim_time( 0 ),
   superregions(),
   updates( 0 ),
@@ -167,13 +168,13 @@ World::World( const std::string& name,
       PRINT_WARN( "Stg::Init() must be called before a World is created." );
       exit(-1);
     }
- 
+
   pthread_mutex_init( &sync_mutex, NULL );
   pthread_cond_init( &threads_start_cond, NULL );
   pthread_cond_init( &threads_done_cond, NULL );
- 
+
   World::world_set.insert( this );
-  
+
   ground = new Model(this, NULL, "model");
   assert(ground);
   ground->SetToken( "_ground_model" ); // allow users to identify this unique model
@@ -215,9 +216,9 @@ void World::Run()
     if(found_gui && (world_set.size() != 1))
     {
         PRINT_WARN( "When using the GUI only a single world can be simulated." );
-        exit(-1);      
+        exit(-1);
     }
-    
+
     if(found_gui)
     {
         Fl::run();
@@ -229,15 +230,15 @@ void World::Run()
 }
 
 bool World::UpdateAll()
-{  
+{
   bool quit( true );
-  
+
   FOR_EACH( world_it, World::world_set )
     {
       if( (*world_it)->Update() == false )
 	quit = false;
     }
-  
+
   return quit;
 }
 
@@ -247,29 +248,29 @@ void* World::update_thread_entry( std::pair<World*,int> *thread_info )
 {
   World* world( thread_info->first );
   const int thread_instance( thread_info->second );
-  
+
   //printf( "thread ID %d waiting for mutex\n", thread_instance );
 
-  pthread_mutex_lock( &world->sync_mutex );  
+  pthread_mutex_lock( &world->sync_mutex );
 
   while( 1 )
     {
       //printf( "thread ID %d waiting for start\n", thread_instance );
       // wait until the main thread signals us
       //puts( "worker waiting for start signal" );
-      
+
       pthread_cond_wait( &world->threads_start_cond, &world->sync_mutex );
       pthread_mutex_unlock( &world->sync_mutex );
-		
+
       //printf( "worker %u thread awakes for task %u\n", thread_instance, task );
       world->ConsumeQueue( thread_instance );
       //printf( "thread %d done\n", thread_instance );
-      
+
       // done working, so increment the counter. If this was the last
       // thread to finish working, signal the main thread, which is
       // blocked waiting for this to happen
-      
-      pthread_mutex_lock( &world->sync_mutex );	  
+
+      pthread_mutex_lock( &world->sync_mutex );
       if( --world->threads_working == 0 )
 	{
 	  //puts( "last worker signalling main thread" );
@@ -277,7 +278,7 @@ void* World::update_thread_entry( std::pair<World*,int> *thread_info )
 	}
       // keep lock going round the loop
     }
-  
+
   return NULL;
 }
 
@@ -301,26 +302,26 @@ void World::RemoveModel( Model* mod )
 }
 
 void World::LoadBlock( Worldfile* wf, int entity )
-{ 
+{
   // lookup the group in which this was defined
   Model* mod( models_by_wfentity[ wf->GetEntityParent( entity )]);
-  
+
   if( ! mod )
     PRINT_ERR( "block has no model for a parent" );
-  
+
   mod->LoadBlock( wf, entity );
 }
 
 void World::LoadSensor( Worldfile* wf, int entity )
-{ 
+{
   // lookup the group in which this was defined
   ModelRanger* rgr( dynamic_cast<ModelRanger*>(models_by_wfentity[ wf->GetEntityParent( entity )]));
-  
+
   //todo verify that the parent is indeed a ranger
-	
+
   if( ! rgr )
     PRINT_ERR( "block has no ranger model for a parent" );
-  
+
   rgr->LoadSensor( wf, entity );
 }
 
@@ -328,16 +329,16 @@ void World::LoadSensor( Worldfile* wf, int entity )
 Model* World::CreateModel( Model* parent, const std::string& typestr )
 {
   Model* mod(NULL); // new model to return
-  
+
   // find the creator function pointer in the map. use the
   // vanilla model if the type is NULL.
   creator_t creator = NULL;
-  
+
   // printf( "creating model of type %s\n", typestr );
-  
-  std::map< std::string, creator_t>::iterator it = 
+
+  std::map< std::string, creator_t>::iterator it =
     Model::name_map.find( typestr );
-  
+
   if( it == Model::name_map.end() )
     {
       puts("");
@@ -354,33 +355,33 @@ Model* World::CreateModel( Model* parent, const std::string& typestr )
     }
   else
     {
-      PRINT_ERR1( "Unknown model type %s in world file.", 
+      PRINT_ERR1( "Unknown model type %s in world file.",
 		  typestr.c_str() );
       exit( 1 );
     }
-  
+
   //printf( "created model %s\n", mod->Token() );
-  
+
   return mod;
 }
 
 void World::LoadModel( Worldfile* wf, int entity )
-{ 
+{
   const int parent_entity( wf->GetEntityParent( entity ));
-  
-  PRINT_DEBUG2( "wf entity %d parent entity %d\n", 
+
+  PRINT_DEBUG2( "wf entity %d parent entity %d\n",
 		entity, parent_entity );
-  
+
   Model* parent( models_by_wfentity[ parent_entity ]);
-    
-  const char *typestr((char*)wf->GetEntityType(entity));      	  
+
+  const char *typestr((char*)wf->GetEntityType(entity));
   assert(typestr);
-  
+
   Model* mod( CreateModel( parent, typestr ));
-  
+
   // configure the model with properties from the world file
   mod->Load(wf, entity );
- 
+
   // record the model we created for this worldfile entry
   models_by_wfentity[entity] = mod;
 }
@@ -399,64 +400,64 @@ void World::Load( const std::string& worldfile_path )
 
   // end the output line of worldfile components
   //puts("");
-  
+
   const int entity(0);
-  
+
   // nothing gets added if the string is empty
   this->SetToken( wf->ReadString( entity, "name", worldfile_path ));
-  
-  this->quit_time = 
+
+  this->quit_time =
     (usec_t)( million * wf->ReadFloat( entity, "quit_time", this->quit_time ) );
-  
-  this->ppm = 
+
+  this->ppm =
     1.0 / wf->ReadFloat( entity, "resolution", 1.0 / this->ppm );
-  
-  this->show_clock = 
+
+  this->show_clock =
     wf->ReadInt( entity, "show_clock", this->show_clock );
-  
-  this->show_clock_interval = 
+
+  this->show_clock_interval =
     wf->ReadInt( entity, "show_clock_interval", this->show_clock_interval );
-  
+
   // read msec instead of usec: easier for user
   this->sim_interval =
     1e3 * wf->ReadFloat( entity, "interval_sim", this->sim_interval / 1e3 );
-  
-  this->worker_threads = wf->ReadInt( entity, "threads",  this->worker_threads );  
+
+  this->worker_threads = wf->ReadInt( entity, "threads",  this->worker_threads );
   if( this->worker_threads < 1 )
     {
       PRINT_WARN( "threads set to <1. Forcing to 1" );
       this->worker_threads = 1;
     }
-  
-  pending_update_callbacks.resize( worker_threads + 1 );      
+
+  pending_update_callbacks.resize( worker_threads + 1 );
   event_queues.resize( worker_threads + 1 );
-  
+
   //printf( "worker threads %d\n", worker_threads );
-  
+
   // kick off the threads
   for( unsigned int t(0); t<worker_threads; ++t )
-    {      
+    {
       //normal posix pthread C function pointer
       typedef void* (*func_ptr) (void*);
-      
+
       // the pair<World*,int> is the configuration for each thread. it can't be a local
       // stack var, since it's accssed in the threads
 
       pthread_t pt;
       pthread_create( &pt,
 		      NULL,
-		      (func_ptr)World::update_thread_entry, 
+		      (func_ptr)World::update_thread_entry,
 		      new std::pair<World*,int>( this, t+1 ) );
     }
-  
-  if( worker_threads > 1 ) 
-    printf( "[threads %u]", worker_threads );	
-  
+
+  if( worker_threads > 1 )
+    printf( "[threads %u]", worker_threads );
+
   // Iterate through entitys and create objects of the appropriate type
   for( int entity(1); entity < wf->GetEntityCount(); ++entity )
     {
-      const char *typestr = (char*)wf->GetEntityType(entity);      	  
-      
+      const char *typestr = (char*)wf->GetEntityType(entity);
+
       // don't load window entries here
       if( strcmp( typestr, "window" ) == 0 )
 	{
@@ -469,17 +470,17 @@ void World::Load( const std::string& worldfile_path )
       else
 	LoadModel( wf, entity );
     }
-  
+
   // call all controller init functions
   FOR_EACH( it, models )
     {
       (*it)->blockgroup.CalcSize();
       (*it)->UnMap(); // clears both layers
       (*it)->Map(); // maps both layers
-      
+
       // to here
     }
-  
+
   // the world is all done - run any init code for user's controllers
   FOR_EACH( it, models )
     (*it)->InitControllers();
@@ -494,20 +495,20 @@ void World::UnLoad()
   FOR_EACH( it, children )
     delete (*it);
   children.clear();
- 
+
   models_by_name.clear();
   models_by_wfentity.clear();
-  
+
   ray_list.clear();
 
   // todo - clean up regions & superregions?
-	
+
   token = "[unloaded]";
 }
 
-bool World::PastQuitTime() 
-{ 
-  return( (quit_time > 0) && (sim_time >= quit_time) ); 
+bool World::PastQuitTime()
+{
+  return( (quit_time > 0) && (sim_time >= quit_time) );
 }
 
 std::string World::ClockString() const
@@ -516,13 +517,13 @@ std::string World::ClockString() const
   const uint32_t usec_per_minute(60000000U);
   const uint32_t usec_per_second(1000000U);
   const uint32_t usec_per_msec(1000U);
-  
+
   const uint32_t hours(sim_time / usec_per_hour);
   const uint32_t minutes((sim_time % usec_per_hour) / usec_per_minute);
   const uint32_t seconds((sim_time % usec_per_minute) / usec_per_second);
   const uint32_t msec((sim_time % usec_per_second) / usec_per_msec);
-	
-  std::string str;  
+
+  std::string str;
   char buf[256];
 
   if( hours > 0 )
@@ -533,11 +534,11 @@ std::string World::ClockString() const
 
   snprintf( buf, 255, " %um %02us %03umsec", minutes, seconds, msec);
   str += buf;
-  
+
   return str;
 }
 
-void World::AddUpdateCallback( world_callback_t cb, 
+void World::AddUpdateCallback( world_callback_t cb,
 			       void* user )
 {
   // add the callback & argument to the list
@@ -545,20 +546,20 @@ void World::AddUpdateCallback( world_callback_t cb,
 
 }
 
-int World::RemoveUpdateCallback( world_callback_t cb, 
+int World::RemoveUpdateCallback( world_callback_t cb,
 				 void* user )
 {
   std::pair<world_callback_t,void*> p( cb, user );
-  
+
   FOR_EACH( it, cb_list )
     {
       if( (*it) == p )
 	{
-	  cb_list.erase( it );		
+	  cb_list.erase( it );
 	  break;
 	}
     }
-  
+
   // return the number of callbacks now in the list. Useful for
   // detecting when the list is empty.
   return cb_list.size();
@@ -569,15 +570,15 @@ void World::CallUpdateCallbacks()
   // call model CB_UPDATE callbacks queued up by worker threads
   size_t threads( pending_update_callbacks.size() );
   int cbcount( 0 );
-	
+
   for( size_t t(0); t<threads; ++t )
     {
       std::queue<Model*>& q( pending_update_callbacks[t] );
-			
-      // 			printf( "pending callbacks for thread %u: %u\n", 
-      // 							(unsigned int)t, 
+
+      // 			printf( "pending callbacks for thread %u: %u\n",
+      // 							(unsigned int)t,
       // 							(unsigned int)q.size() );
-			
+
       cbcount += q.size();
 
       while( ! q.empty() )
@@ -587,38 +588,38 @@ void World::CallUpdateCallbacks()
 	}
     }
   //	printf( "cb total %u (global %d)\n\n", (unsigned int)cbcount,update_cb_count );
-	
+
   assert( update_cb_count >= cbcount );
 
   // world callbacks
   FOR_EACH( it, cb_list )
-    {  
+    {
       if( ((*it).first )( this, (*it).second ) )
 	it = cb_list.erase( it );
-    }      
+    }
 }
 
 void World::ConsumeQueue( unsigned int queue_num )
-{  
+{
   std::priority_queue<Event>& queue( event_queues[queue_num] );
-  
+
   if( queue.empty() )
     return;
-  
+
   //printf( "event queue len %d\n", (int)queue.size() );
-  
+
   // update everything on the event queue that happens at this time or earlier
   do
     {
       Event ev( queue.top() );
       if( ev.time > sim_time ) break;
       queue.pop();
-			
+
       //printf( "Q%d @ %llu next event ptr %p cb %p\n", queue_num, sim_time, ev.mod, ev.cb );
       //std::string modelType = ev.mod->GetModelType();
-      //printf( "@ %llu next event <%s %llu %s>\n",  sim_time, modelType.c_str(), ev.time, ev.mod->Token() ); 
-      
-      ev.cb( ev.mod, ev.arg); // call the event's callback on the model			
+      //printf( "@ %llu next event <%s %llu %s>\n",  sim_time, modelType.c_str(), ev.time, ev.mod->Token() );
+
+      ev.cb( ev.mod, ev.arg); // call the event's callback on the model
     }
   while( !queue.empty() );
 }
@@ -628,27 +629,27 @@ bool World::Update()
   //printf( "cells: %u blocks %u\n", Cell::count, Block::count );
 
   //puts( "World::Update()" );
-	
+
   // if we've run long enough, exit
-  if( PastQuitTime() ) 
-    return true;		
-	
+  if( PastQuitTime() )
+    return true;
+
   if( show_clock && ((this->updates % show_clock_interval) == 0) )
     {
       printf( "\r[Stage: %s]", ClockString().c_str() );
       fflush( stdout );
     }
-	
-  sim_time += sim_interval; 
-	
+
+  sim_time += sim_interval;
+
   // rebuild the sets sorted by position on x,y axis
-  models_with_fiducials_byx.clear(); 
-  models_with_fiducials_byy.clear(); 
-	
+  models_with_fiducials_byx.clear();
+  models_with_fiducials_byy.clear();
+
   FOR_EACH( it, models_with_fiducials )
     {
-      models_with_fiducials_byx.insert( *it ); 
-      models_with_fiducials_byy.insert( *it ); 
+      models_with_fiducials_byx.insert( *it );
+      models_with_fiducials_byy.insert( *it );
     }
 
   //printf( "x %lu y %lu\n", models_with_fiducials_byy.size(),
@@ -656,20 +657,20 @@ bool World::Update()
 
   // handle the zeroth queue synchronously in the main thread
   ConsumeQueue( 0 );
-  
+
   // handle all the remaining queues asynchronously in worker threads
   pthread_mutex_lock( &sync_mutex );
-  threads_working = worker_threads; 
+  threads_working = worker_threads;
   // unblock the workers - they are waiting on this condition var
   //puts( "main thread signalling workers" );
   pthread_cond_broadcast( &threads_start_cond );
-  pthread_mutex_unlock( &sync_mutex );		 
-  
+  pthread_mutex_unlock( &sync_mutex );
+
   // update the position of all position models based on their velocity
   // while sensor models are running in other threads
   FOR_EACH( it, active_velocity )
     (*it)->Move();
-  
+
   pthread_mutex_lock( &sync_mutex );
   // wait for all the last update job to complete - it will
   // signal the worker_threads_done condition var
@@ -678,24 +679,24 @@ bool World::Update()
       //puts( "main thread waiting for workers to finish" );
       pthread_cond_wait( &threads_done_cond, &sync_mutex );
     }
-  pthread_mutex_unlock( &sync_mutex );		 
+  pthread_mutex_unlock( &sync_mutex );
   //puts( "main thread awakes" );
-  
+
   // TODO: allow threadsafe callbacks to be called in worker
-  // threads		
-  
-  dirty = true; // need redraw 
-  
+  // threads
+
+  dirty = true; // need redraw
+
   // this stuff must be done in series here
-  
+
   // world callbacks
   CallUpdateCallbacks();
-  
+
   FOR_EACH( it, active_energy )
     (*it)->UpdateCharge();
-  
-  ++updates;  
-    
+
+  ++updates;
+
   return false;
 }
 
@@ -712,9 +713,9 @@ unsigned int World::GetEventQueue( Model* mod ) const
 Model* World::GetModel( const std::string& name ) const
 {
   PRINT_DEBUG1( "looking up model name %s in models_by_name", name.c_str() );
-	
+
   std::map<std::string,Model*>::const_iterator it( models_by_name.find( name ) );
-	
+
   if( it == models_by_name.end() )
     {
       PRINT_WARN1( "lookup of model name %s: no matching name", name.c_str() );
@@ -725,7 +726,7 @@ Model* World::GetModel( const std::string& name ) const
 }
 
 void World::RecordRay( double x1, double y1, double x2, double y2 )
-{  
+{
   float* drawpts( new float[4] );
   drawpts[0] = x1;
   drawpts[1] = y1;
@@ -739,7 +740,7 @@ void World::ClearRays()
   // destroy the C arrays first
   FOR_EACH( it, ray_list )
     delete [] *it;
-  
+
   ray_list.clear();
 }
 
@@ -749,20 +750,20 @@ void World::Raytrace( const Pose &gpose, // global pose
  		      const meters_t range,
  		      const radians_t fov,
  		      const ray_test_func_t func,
- 		      const Model* mod,			 
+ 		      const Model* mod,
 		      const void* arg,
-		      const bool ztest,		      
+		      const bool ztest,
 		      std::vector<RaytraceResult>& results )
 {
   // find the direction of the first ray
   Pose raypose( gpose );
   const double starta( fov/2.0 - raypose.a );
-  
+
   // set up a ray to trace
   Ray ray( mod, gpose, range, func, arg, ztest );
-  
+
   const size_t sample_count = results.size();
-  
+
   for( size_t s(0); s < sample_count; ++s )
     {
       // aim the ray in the right direction before tracing
@@ -772,13 +773,13 @@ void World::Raytrace( const Pose &gpose, // global pose
 }
 
 
-RaytraceResult World::Raytrace( const Pose &gpose, 
+RaytraceResult World::Raytrace( const Pose &gpose,
 				const meters_t range,
 				const ray_test_func_t func,
-				const Model* mod,		
+				const Model* mod,
 				const void* arg,
 				const bool ztest )
-{    
+{
   return Raytrace( Ray( mod, gpose, range, func, arg, ztest ));
 }
 
@@ -794,11 +795,11 @@ RaytraceResult World::Raytrace( const Ray& r )
   // our global position in (floating point) cell coordinates
   double globx( r.origin.x * ppm );
   double globy( r.origin.y * ppm );
-	
+
   // record our starting position
   const double startx( globx );
   const double starty( globy );
-  
+
   // eliminate a potential divide by zero
   const double angle( r.origin.a == 0.0 ? 1e-12 : r.origin.a );
   const double sina(sin(angle));
@@ -809,18 +810,18 @@ RaytraceResult World::Raytrace( const Ray& r )
   // very weird and rare bug is produced)
   const double dx( ppm * r.range * cosa);
   const double dy( ppm * r.range * sina);
-  
+
   // fast integer line 3d algorithm adapted from Cohen's code from
-  // Graphics Gems IV  
-  const int32_t sx(sgn(dx));  
-  const int32_t sy(sgn(dy));  
-  const int32_t ax(abs(dx)); 
-  const int32_t ay(abs(dy));  
-  const int32_t bx(2*ax);	
-  const int32_t by(2*ay);	
+  // Graphics Gems IV
+  const int32_t sx(sgn(dx));
+  const int32_t sy(sgn(dy));
+  const int32_t ax(abs(dx));
+  const int32_t ay(abs(dy));
+  const int32_t bx(2*ax);
+  const int32_t by(2*ay);
   int32_t exy(ay-ax); // difference between x and y distances
   int32_t n(ax+ay); // the manhattan distance to the goal cell
-    
+
   // the distances between region crossings in X and Y
   const double xjumpx( sx * REGIONWIDTH );
   const double xjumpy( sx * REGIONWIDTH * tana );
@@ -832,7 +833,7 @@ RaytraceResult World::Raytrace( const Ray& r )
   const double yjumpdist( fabs(yjumpx)+fabs(yjumpy) );
 
   const unsigned int layer( (updates+1) % 2 );
-  
+
   // these are updated as we go along the ray
   double xcrossx(0), xcrossy(0);
   double ycrossx(0), ycrossy(0);
@@ -846,20 +847,20 @@ RaytraceResult World::Raytrace( const Ray& r )
   // several useful asserts are commented out so that Stage is not too
   // slow in debug builds. Add them in if chasing a suspected raytrace bug
   while( n > 0  ) // while we are still not at the ray end
-    { 
+    {
       SuperRegion* sr( GetSuperRegion(point_int_t(GETSREG(globx),GETSREG(globy))));
       Region* reg( sr ?	sr->GetRegion(GETREG(globx),GETREG(globy)) : NULL );
-			
+
       if( reg && reg->count ) // if the region contains any objects
 	{
 	  //assert( reg->cells.size() );
-					
+
 	  // invalidate the region crossing points used to jump over
 	  // empty regions
 	  calculatecrossings = true;
-					
+
 	  // convert from global cell to local cell coords
-	  int32_t cx( GETCELL(globx) ); 
+	  int32_t cx( GETCELL(globx) );
 	  int32_t cy( GETCELL(globy) );
 
 	  // since reg->count was non-zero, we expect this pointer to be good
@@ -867,27 +868,27 @@ RaytraceResult World::Raytrace( const Ray& r )
 
 	  // while within the bounds of this region and while some ray remains
 	  // we'll tweak the cell pointer directly to move around quickly
-	  while( (cx>=0) && (cx<REGIONWIDTH) && 
-		 (cy>=0) && (cy<REGIONWIDTH) && 
+	  while( (cx>=0) && (cx<REGIONWIDTH) &&
+		 (cy>=0) && (cy<REGIONWIDTH) &&
 		 n > 0 )
-	    {			 
+	    {
 	      FOR_EACH( it, c->blocks[layer] )
 		{
 		  Block* block( *it );
 		  assert( block );
-		  
+
 		  // skip if not in the right z range
-		  if( r.ztest && 
-		      ( r.origin.z < block->global_z.min || 
+		  if( r.ztest &&
+		      ( r.origin.z < block->global_z.min ||
 			r.origin.z > block->global_z.max ) )
-		    continue; 
-									
+		    continue;
+
 		  // test the predicate we were passed
-		  if( (*r.func)( &block->group->mod, (Model*)r.mod, r.arg )) 
+		  if( (*r.func)( &block->group->mod, (Model*)r.mod, r.arg ))
 		    {
 		      // a hit!
 		      result.pose = r.origin;
-		      result.mod = &block->group->mod;	
+		      result.mod = &block->group->mod;
 		      result.color = result.mod->GetColor();
 
 		      if( ax > ay ) // faster than the equivalent hypot() call
@@ -896,113 +897,369 @@ RaytraceResult World::Raytrace( const Ray& r )
 			result.range = fabs((globy-starty) / sina) / ppm;
 
 		      return result;
-		    }				  
+		    }
 		}
 
 	      // increment our cell in the correct direction
 	      if( exy < 0 ) // we're iterating along X
 		{
 		  globx += sx; // global coordinate
-		  exy += by;						
+		  exy += by;
 		  c += sx; // move the cell left or right
 		  cx += sx; // cell coordinate for bounds checking
 		}
 	      else  // we're iterating along Y
 		{
 		  globy += sy; // global coordinate
-		  exy -= bx;						
+		  exy -= bx;
 		  c += sy * REGIONWIDTH; // move the cell up or down
 		  cy += sy; // cell coordinate for bounds checking
-		}			 
+		}
 	      --n; // decrement the manhattan distance remaining
-													 							
+
 	      //rt_cells.push_back( point_int_t( globx, globy ));
-	    }					
+	    }
 	  //printf( "leaving populated region\n" );
-	}							 
+	}
       else // jump over the empty region
-	{		  		  		  
+	{
 	  // on the first run, and when we've been iterating over
 	  // cells, we need to calculate the next crossing of a region
 	  // boundary along each axis
 	  if( calculatecrossings )
 	    {
 	      calculatecrossings = false;
-							
+
 	      // find the coordinate in cells of the bottom left corner of
 	      // the current region
 	      const int32_t ix( globx );
-	      const int32_t iy( globy );				  
+	      const int32_t iy( globy );
 	      double regionx( ix/REGIONWIDTH*REGIONWIDTH );
 	      double regiony( iy/REGIONWIDTH*REGIONWIDTH );
 	      if( (globx < 0) && (ix % REGIONWIDTH) ) regionx -= REGIONWIDTH;
 	      if( (globy < 0) && (iy % REGIONWIDTH) ) regiony -= REGIONWIDTH;
-							
+
 	      // calculate the distance to the edge of the current region
-	      const double xdx( sx < 0 ? 
+	      const double xdx( sx < 0 ?
 				regionx - globx - 1.0 : // going left
-				regionx + REGIONWIDTH - globx ); // going right			 
+				regionx + REGIONWIDTH - globx ); // going right
 	      const double xdy( xdx*tana );
-					
-	      const double ydy( sy < 0 ? 
+
+	      const double ydy( sy < 0 ?
 				regiony - globy - 1.0 :  // going down
-				regiony + REGIONWIDTH - globy ); // going up		 
+				regiony + REGIONWIDTH - globy ); // going up
 	      const double ydx( ydy/tana );
-					
+
 	      // these stored hit points are updated as we go along
 	      xcrossx = globx+xdx;
 	      xcrossy = globy+xdy;
-							
+
 	      ycrossx = globx+ydx;
 	      ycrossy = globy+ydy;
-							
+
 	      // find the distances to the region crossing points
 	      // manhattan distance is faster than using hypot()
 	      distX = fabs(xdx)+fabs(xdy);
-	      distY = fabs(ydx)+fabs(ydy);		  
+	      distY = fabs(ydx)+fabs(ydy);
 	    }
-					
+
 	  if( distX < distY ) // crossing a region boundary left or right
 	    {
 	      // move to the X crossing
-	      globx = xcrossx; 
-	      globy = xcrossy; 
-							
+	      globx = xcrossx;
+	      globy = xcrossy;
+
 	      n -= distX; // decrement remaining manhattan distance
-							
+
 	      // calculate the next region crossing
-	      xcrossx += xjumpx; 
+	      xcrossx += xjumpx;
 	      xcrossy += xjumpy;
-							
+
 	      distY -= distX;
 	      distX = xjumpdist;
-							
+
 	      //rt_candidate_cells.push_back( point_int_t( xcrossx, xcrossy ));
-	    }			 
+	    }
 	  else // crossing a region boundary up or down
-	    {		  
+	    {
 	      // move to the X crossing
 	      globx = ycrossx;
 	      globy = ycrossy;
-							
-	      n -= distY; // decrement remaining manhattan distance 			
-				
+
+	      n -= distY; // decrement remaining manhattan distance
+
 	      // calculate the next region crossing
 	      ycrossx += yjumpx;
 	      ycrossy += yjumpy;
-							
+
 	      distX -= distY;
 	      distY = yjumpdist;
 
 	      //rt_candidate_cells.push_back( point_int_t( ycrossx, ycrossy ));
-	    }	
-	}			  	
+	    }
+	}
       //rt_cells.push_back( point_int_t( globx, globy ));
-    } 
+    }
 
   return result;
 }
 
+
+// Stage spends 50-99% of its time in this method.
+meters_t World::RaytraceWifi( const Pose &gpose,
+      const meters_t range,
+      const ray_test_func_t func,
+      const Model* mod,
+      const Model* target,
+      const void* arg,
+      const bool ztest )
+{
+  Ray r( mod, gpose, range, func, arg, ztest );
+  return RaytraceWifi( r, target );
+}
+
+meters_t World::RaytraceWifi( const Ray& r,
+                                  const Model* target )
+{
+  //rt_cells.clear();
+  //rt_candidate_cells.clear();
+
+  // initialize the sample
+  //xRaytraceResult sample( r.origin, r.range );
+
+  // Distance (in pixels) along X or Y axis that we travel inside an obstacle
+  point_int_t dist_pixels(0, 0);
+  // Distance in meters
+  meters_t dist_meters = 0.0;
+
+  // our global position in (floating point) cell coordinates
+  double globx( r.origin.x * ppm );
+  double globy( r.origin.y * ppm );
+
+  // record our starting position
+
+  // eliminate a potential divide by zero
+  const double angle( r.origin.a == 0.0 ? 1e-12 : r.origin.a );
+  const double cosa(cos(angle));
+  const double sina(sin(angle));
+  const double tana(sina/cosa); // = tan(angle)
+
+  // the x and y components of the ray (these need to be doubles, or a
+  // very weird and rare bug is produced)
+  const double dx( ppm * r.range * cosa);
+  const double dy( ppm * r.range * sina);
+
+  // fast integer line 3d algorithm adapted from Cohen's code from
+  // Graphics Gems IV
+  const int32_t sx(sgn(dx));   // sgn() is a fast macro
+  const int32_t sy(sgn(dy));
+  const int32_t ax(abs(dx));
+  const int32_t ay(abs(dy));
+  const int32_t bx(2*ax);
+  const int32_t by(2*ay);
+  int32_t exy(ay-ax); // difference between x and y distances
+  int32_t n(ax+ay); // the manhattan distance to the goal cell
+
+  // the distances between region crossings in X and Y
+  const double xjumpx( sx * REGIONWIDTH );
+  const double xjumpy( sx * REGIONWIDTH * tana );
+  const double yjumpx( sy * REGIONWIDTH / tana );
+  const double yjumpy( sy * REGIONWIDTH );
+
+  // manhattan distance between region crossings in X and Y
+  const double xjumpdist( fabs(xjumpx)+fabs(xjumpy) );
+  const double yjumpdist( fabs(yjumpx)+fabs(yjumpy) );
+
+  const unsigned int layer( (updates+1) % 2 );
+
+  // these are updated as we go along the ray
+  double xcrossx(0), xcrossy(0);
+  double ycrossx(0), ycrossy(0);
+  double distX(0), distY(0);
+  bool calculatecrossings( true );
+
+  // Stage spends up to 95% of its time in this loop! It would be
+  // neater with more function calls encapsulating things, but even
+  // inline calls have a noticeable (2-3%) effect on performance.
+  while( n > 0  ) // while we are still not at the ray end
+  {
+    Region* reg( GetSuperRegion( point_int_t(GETSREG(globx),
+                             GETSREG(globy)))
+              ->GetRegion( GETREG(globx), GETREG(globy) ));
+
+    if( reg->count ) // if the region contains any objects
+    {
+      // invalidate the region crossing points used to jump over
+      // empty regions
+      calculatecrossings = true;
+
+      // convert from global cell to local cell coords
+      int32_t cx( GETCELL(globx) );
+      int32_t cy( GETCELL(globy) );
+
+      Cell* c( &reg->cells[ cx + cy * REGIONWIDTH ] );
+      assert(c); // should be good: we know the region contains objects
+
+      // while within the bounds of this region and while some ray remains
+      // we'll tweak the cell pointer directly to move around quickly
+      while( (cx>=0) && (cx<REGIONWIDTH) &&
+             (cy>=0) && (cy<REGIONWIDTH) &&
+              n > 0 )
+      {
+        // Assume cell is not occupied by any block
+        int hit=0;
+
+        //Go through the blocks occupying the cell
+        FOR_EACH( it, c->blocks[layer] )
+        {
+          Block* block( *it );
+          assert( block );
+
+          // skip if not in the right z range
+          if( r.ztest &&
+            ( r.origin.z < block->global_z.min ||
+              r.origin.z > block->global_z.max ) )
+            continue;
+
+          // test the predicate we were passed
+          if( (*r.func)( &block->group->mod, (Model*)r.mod, r.arg ))
+          {
+            // a hit!
+            if (!target->IsRelated( &block->group->mod ))
+            {
+              // We hit something other than the target.
+              hit = 1;
+            }
+            // Break out of the loop after the first block of a
+            // matching device is hit
+            break;
+
+          }
+        }//end FOREACH
+
+        // increment our cell in the correct direction
+        if( exy < 0 ) // we're iterating along X
+        {
+          globx += sx; // global coordinate
+          exy += by;
+          c += sx; // move the cell left or right
+          cx += sx; // cell coordinate for bounds checking
+          if (hit) {
+            // Add the distance that we pass through this
+            // device.
+            dist_pixels.x += 1;
+          }
+        }
+        else  // we're iterating along Y
+				{
+          globy += sy; // global coordinate
+          exy -= bx;
+          c += sy * REGIONWIDTH; // move the cell up or down
+          cy += sy; // cell coordinate for bounds checking
+          if (hit)
+          {
+            // Add the distance that we pass through this
+            // device.
+            dist_pixels.y += 1;
+          }
+        }
+        --n; // decrement the manhattan distance remaining
+
+				  //rt_cells.push_back( point_int_t( globx, globy ));
+      }//end while
+			//printf( "leaving populated region\n" );
+		}
+    else // jump over the empty region
+    {
+      // on the first run, and when we've been iterating over
+      // cells, we need to calculate the next crossing of a region
+      // boundary along each axis
+			if( calculatecrossings )
+			{
+        calculatecrossings = false;
+
+        // find the coordinate in cells of the bottom left corner of
+        // the current region
+        int32_t ix( globx );
+        int32_t iy( globy );
+        double regionx( ix/REGIONWIDTH*REGIONWIDTH );
+        double regiony( iy/REGIONWIDTH*REGIONWIDTH );
+        if( (globx < 0) && (ix % REGIONWIDTH) ) regionx -= REGIONWIDTH;
+        if( (globy < 0) && (iy % REGIONWIDTH) ) regiony -= REGIONWIDTH;
+
+        // calculate the distance to the edge of the current region
+        double xdx( sx < 0 ?
+            regionx - globx - 1.0 : // going left
+            regionx + REGIONWIDTH - globx ); // going right
+        double xdy( xdx*tana );
+
+        double ydy( sy < 0 ?
+            regiony - globy - 1.0 :  // going down
+            regiony + REGIONWIDTH - globy ); // going up
+        double ydx( ydy/tana );
+
+        // these stored hit points are updated as we go along
+        xcrossx = globx+xdx;
+        xcrossy = globy+xdy;
+
+        ycrossx = globx+ydx;
+        ycrossy = globy+ydy;
+
+        // find the distances to the region crossing points
+        // manhattan distance is faster than using hypot()
+        distX = fabs(xdx)+fabs(xdy);
+        distY = fabs(ydx)+fabs(ydy);
+      }//end calculatecrossings
+
+      if( distX < distY ) // crossing a region boundary left or right
+      {
+        // move to the X crossing
+        globx = xcrossx;
+        globy = xcrossy;
+
+        n -= distX; // decrement remaining manhattan distance
+
+        // calculate the next region crossing
+        xcrossx += xjumpx;
+        xcrossy += xjumpy;
+
+        distY -= distX;
+        distX = xjumpdist;
+
+        //rt_candidate_cells.push_back( point_int_t( xcrossx, xcrossy ));
+      }
+      else // crossing a region boundary up or down
+      {
+        // move to the X crossing
+        globx = ycrossx;
+        globy = ycrossy;
+
+        n -= distY; // decrement remaining manhattan distance
+
+        // calculate the next region crossing
+        ycrossx += yjumpx;
+        ycrossy += yjumpy;
+
+        distX -= distY;
+        distY = yjumpdist;
+        //rt_candidate_cells.push_back( point_int_t( ycrossx, ycrossy ));
+      }//end cross region boundary up/down
+    }//end calculatecrossings
+    //rt_cells.push_back( point_int_t( globx, globy ));
+  }
+
+  if (dist_pixels.x > dist_pixels.y) {
+    dist_meters = fabs(dist_pixels.x / cosa) / ppm;
+  } else {
+    dist_meters = fabs(dist_pixels.y / sina) / ppm;
+  }
+
+  return dist_meters;
+
+
+}//end RayTraceWifi
+
+
 static int _save_cb( Model* mod, void* dummy )
 {
   mod->Save();
@@ -1031,76 +1288,76 @@ void World::Reload( void )
 void World::MapPoly( const std::vector<point_int_t>& pts, Block* block, unsigned int layer )
 {
   const size_t pt_count( pts.size() );
-  
+
   for( size_t i(0); i<pt_count; ++i )
     {
       const point_int_t& start(pts[i] );
       const point_int_t& end(pts[(i+1)%pt_count]);
-			
+
       // line rasterization adapted from Cohen's 3D version in
-      // Graphics Gems II. Should be very fast.  
+      // Graphics Gems II. Should be very fast.
       const int32_t dx( end.x - start.x );
       const int32_t dy( end.y - start.y );
-      const int32_t sx(sgn(dx));  
-      const int32_t sy(sgn(dy));  
-      const int32_t ax(abs(dx));  
-      const int32_t ay(abs(dy));  
-      const int32_t bx(2*ax);	
-      const int32_t by(2*ay);	 
-
-      int32_t exy(ay-ax); 
+      const int32_t sx(sgn(dx));
+      const int32_t sy(sgn(dy));
+      const int32_t ax(abs(dx));
+      const int32_t ay(abs(dy));
+      const int32_t bx(2*ax);
+      const int32_t by(2*ay);
+
+      int32_t exy(ay-ax);
       int32_t n(ax+ay);
-			
+
       int32_t globx(start.x);
       int32_t globy(start.y);
-			
-      while( n ) 
-	{				
-	  Region* reg( GetSuperRegionCreate( point_int_t(GETSREG(globx), 
+
+      while( n )
+	{
+	  Region* reg( GetSuperRegionCreate( point_int_t(GETSREG(globx),
 							 GETSREG(globy)))
-		       ->GetRegion( GETREG(globx), 
-				    GETREG(globy)));										
+		       ->GetRegion( GETREG(globx),
+				    GETREG(globy)));
 	  assert(reg);
-					
+
 	  // add all the required cells in this region before looking up
-	  // another region			
-	  int32_t cx( GETCELL(globx) ); 
+	  // another region
+	  int32_t cx( GETCELL(globx) );
 	  int32_t cy( GETCELL(globy) );
-					
+
 	  // need to call Region::GetCell() before using a Cell pointer
 	  // directly, because the region allocates cells lazily, waiting
 	  // for a call of this method
 	  Cell* c( reg->GetCell( cx, cy ) );
-					
+
 	  // while inside the region, manipulate the Cell pointer directly
-	  while( (cx>=0) && (cx<REGIONWIDTH) && 
-	  	 (cy>=0) && (cy<REGIONWIDTH) && 
+	  while( (cx>=0) && (cx<REGIONWIDTH) &&
+	  	 (cy>=0) && (cy<REGIONWIDTH) &&
 	  	 n > 0 )
-	    {					
+	    {
             // if the block is not already rendered in the cell
             //if( find (block->rendered_cells[layer].begin(), block->rendered_cells[layer].end(), c )
-              // == block->rendered_cells[layer].end() )                            
-                c->AddBlock(block, layer ); 
-							
+              // == block->rendered_cells[layer].end() )
+                c->AddBlock(block, layer );
+
 	      // cleverly skip to the next cell (now it's safe to
 	      // manipulate the cell pointer)
-	      if( exy < 0 ) 
+	      if( exy < 0 )
 		{
 		  globx += sx;
 		  exy += by;
 		  c += sx;
 		  cx += sx;
 		}
-	      else 
+	      else
 		{
 		  globy += sy;
-		  exy -= bx; 
+		  exy -= bx;
 		  c += sy * REGIONWIDTH;
 		  cy += sy;
 		}
 	      --n;
 	    }
-	}			
+	}
     }
 }
 
@@ -1113,11 +1370,11 @@ SuperRegion* World::AddSuperRegion( const point_int_t& sup )
   Extend( point3_t( (sup.x << SRBITS) / ppm,
 		    (sup.y << SRBITS) / ppm,
 		    0 ));
-  
+
   // top right corner of the new superregion
   Extend( point3_t( ((sup.x+1) << SRBITS) / ppm,
 		    ((sup.y+1) << SRBITS) / ppm,
-		    0 ));  
+		    0 ));
   return sr;
 }
 
@@ -1125,24 +1382,24 @@ SuperRegion* World::AddSuperRegion( const point_int_t& sup )
 inline SuperRegion* World::GetSuperRegion( const point_int_t& org )
 {
   SuperRegion* sr(NULL);
-  
+
   // I despise some STL syntax sometimes...
   std::map<point_int_t,SuperRegion*>::iterator it( superregions.find(org) );
-  
+
   if( it != superregions.end() )
     sr = it->second;
-  
+
   return sr;
 }
 
 inline SuperRegion* World::GetSuperRegionCreate( const point_int_t& org )
 {
   SuperRegion* sr( GetSuperRegion(org) );
-  
+
   if( sr == NULL ) // no superregion exists! make a new one
     {
-      sr = AddSuperRegion( org );  
-      assert( sr ); 
+      sr = AddSuperRegion( org );
+      assert( sr );
     }
   return sr;
 }
@@ -1161,7 +1418,7 @@ void World::Extend( point3_t pt )
 
 void World::AddPowerPack( PowerPack* pp )
 {
-  powerpack_list.push_back( pp ); 
+  powerpack_list.push_back( pp );
 }
 
 void World::RemovePowerPack( PowerPack* pp )
@@ -1183,7 +1440,7 @@ void World::Log( Model* mod )
   //LogEntry::Print();
 }
 
-bool World::Event::operator<( const Event& other ) const 
+bool World::Event::operator<( const Event& other ) const
 {
   return( time > other.time );
 }
diff --git a/worlds/wifi.world b/worlds/wifi.world
index 9b370aa32..59b1abc68 100644
--- a/worlds/wifi.world
+++ b/worlds/wifi.world
@@ -1,54 +1,125 @@
+include "map.inc"
+include "pioneer.inc"
+include "sick.inc"
 
-# Desc: 1 pioneer robot with laser	
-# CVS: $Id: wifi.world,v 1.1 2006-03-02 23:19:09 gerkey Exp $
+interval_sim 100  # simulation timestep in milliseconds
+interval_real 20  # real-time interval between simulation updates in milliseconds
 
-# defines 'map' object used for floorplans
-include "map.inc"
+quit_time 1800
 
-# size of the world in meters
-size [16 16]
+paused 0
 
-# set the resolution of the underlying raytrace model in meters
 resolution 0.02
 
-# update the screen every 10ms (we need fast update for the stest demo)
-gui_interval 20
-
 # configure the GUI window
 window
-( 
-  size [ 591.000 638.000 ] 
-  center [-0.010 -0.040] 
-  scale 0.028 
-)
+(
+  size [ 556.000 557.000 ] # in pixels
+  scale 29.162
+     # pixels per meter
+  center [ 0.095  -0.359 ]
+  rotate [ 0  0 ]
 
-# load an environment bitmap
-map
-( 
-  bitmap "bitmaps/cave.png"
-  size [16 16]
-  name "cave"
+  show_data 1              # 1=on 0=off
 )
 
-# define a wifi-equipped robot
-define commbot position
+# load an environment bitmap
+floorplan
 (
-  size [0.25 0.25]
-  wifi ()
+  bitmap "bitmaps/cave.png"
+  size [16.000 16.000 0.800]
+  pose [0 0 0 0]
 )
 
-# create a robot
-commbot
+pioneer2dx
 (
-  name "robot1"
+  # can refer to the robot by this name
+  name "r0"
   color "red"
-  pose [-6.5 -6.5 45]
+
+  pose [-6.5 -6.5 0 0]
+  sicklaser()
+
+  ctrl "wander_wifi"
+
+  # report error-free position in world coordinates
+  localization "gps"
+  localization_origin [ 0 0 0 0 ]
+
+  wifi(
+    ip "192.168.0.2"
+    mac "08:00:20:ae:fd:7e"
+    netmask "255.255.255.0"
+    essid "test network"
+    #model "friis"
+    model "raytrace"
+    wall_factor 5
+    power 45
+    sensitivity -75
+    range_db -45
+    # link_success_rate 90
+    # random_seed 10
+  )
 )
 
 # create another robot
-commbot
+pioneer2dx
 (
-  name "robot2"
+  name "r1"
   color "blue"
-  pose [-3.5 -2.5 90]
+
+  pose [-3.5 -2.5 0 0]
+  sicklaser()
+
+  ctrl "wander_wifi"
+
+  # report error-free position in world coordinates
+  localization "gps"
+  localization_origin [ 0 0 0 0 ]
+
+  wifi(
+    ip "192.168.0.3"
+    netmask "255.255.255.0"
+    mac "08:00:20:ae:fe:7e"
+    essid "test network"
+    #model "friis"
+    model "raytrace"
+    wall_factor 5
+    power 45
+    sensitivity -75
+    range_db -45
+    # link_success_rate 90
+    # random_seed 10
+  )
 )
+
+pioneer2dx
+(
+ name "r2"
+  color "green"
+
+  pose [6.5 6.5 0 45]
+  sicklaser()
+
+  ctrl "wander_wifi"
+
+  # report error-free position in world coordinates
+  localization "gps"
+  localization_origin [ 0 0 0 0 ]
+
+  wifi(
+    ip "192.168.0.4"
+    netmask "255.255.255.0"
+    mac "08:00:20:ae:ff:7e"
+    essid "test network"
+    #model "friis"
+    model "raytrace"
+    wall_factor 5
+    power 45
+    sensitivity -75
+    range_db -45
+    # link_success_rate 90
+    # random_seed 10
+  )
+
+)
\ No newline at end of file