1 /* -*-  Mode: C++; c-file-style: "gnu"; indent-tabs-mode:nil; -*- */

     2 /*

     3  * Copyright (c) 2009 Phillip Sitbon

     4  *

     5  * This program is free software; you can redistribute it and/or modify

     6  * it under the terms of the GNU General Public License version 2 as

     7  * published by the Free Software Foundation;

     8  *

     9  * This program is distributed in the hope that it will be useful,

    10  * but WITHOUT ANY WARRANTY; without even the implied warranty of

    11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    12  * GNU General Public License for more details.

    13  *

    14  * You should have received a copy of the GNU General Public License

    15  * along with this program; if not, write to the Free Software

    16  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

    17  *

    18  * Author: Phillip Sitbon <phillip@sitbon.net>

    19  */

    20 #include <limits>

    21 #include "ns3/abort.h"

    22 #include "ns3/simulator.h"

    23 #include "ns3/uinteger.h"

    24 #include "ns3/log.h"

    25 #include "waypoint-mobility-model.h"

    26 

    27 NS_LOG_COMPONENT_DEFINE ("WaypointMobilityModel");

    28 

    29 namespace ns3 {

    30 

    31 NS_OBJECT_ENSURE_REGISTERED (WaypointMobilityModel);

    32 

    33 

    34 TypeId

    35 WaypointMobilityModel::GetTypeId (void)

    36 {

    37   static TypeId tid = TypeId ("ns3::WaypointMobilityModel")

    38     .SetParent<MobilityModel> ()

    39     .SetGroupName ("Mobility")

    40     .AddConstructor<WaypointMobilityModel> ()

    41     .AddAttribute ("NextWaypoint", "The next waypoint used to determine position.",

    42                    TypeId::ATTR_GET,

    43                    WaypointValue (),

    44                    MakeWaypointAccessor (&WaypointMobilityModel::GetNextWaypoint),

    45                    MakeWaypointChecker ())

    46     .AddAttribute ("WaypointsLeft", "The number of waypoints remaining.",

    47                    TypeId::ATTR_GET,

    48                    UintegerValue (0),

    49                    MakeUintegerAccessor (&WaypointMobilityModel::WaypointsLeft),

    50                    MakeUintegerChecker<uint32_t> ())

    51     ;

    52   return tid;

    53 }

    54 

    55 

    56 WaypointMobilityModel::WaypointMobilityModel ()

    57  : m_first (true)

    58 {

    59 }

    60 WaypointMobilityModel::~WaypointMobilityModel ()

    61 {

    62 }

    63 void 

    64 WaypointMobilityModel::DoDispose (void)

    65 {

    66   MobilityModel::DoDispose ();

    67 }

    68 void

    69 WaypointMobilityModel::AddWaypoint (const Waypoint &waypoint)

    70 {

    71   if ( m_first )

    72     {

    73       m_first = false;

    74       m_current = m_next = waypoint;

    75     }

    76   else

    77     {

    78       NS_ABORT_MSG_IF ( !m_waypoints.empty () && (m_waypoints.back ().GetTime () >= waypoint.GetTime ()),

    79                         "Waypoints must be added in ascending time order");

    80       m_waypoints.push_back (waypoint);

    81     }

    82 }

    83 Waypoint

    84 WaypointMobilityModel::GetNextWaypoint (void) const

    85 {

    86   Update ();

    87   return m_next;

    88 }

    89 uint32_t

    90 WaypointMobilityModel::WaypointsLeft (void) const

    91 {

    92   Update ();

    93   return m_waypoints.size();

    94 }

    95 void

    96 WaypointMobilityModel::Update (void) const

    97 {

    98   const Time now = Simulator::Now ();

    99   bool newWaypoint = false;

   100 

   101   if ( now <= m_current.GetTime () )

   102     {

   103       return;

   104     }

   105 

   106   while ( now >= m_next.GetTime ()  )

   107     {

   108       if ( m_waypoints.empty () )

   109         {

   110           if ( m_current.GetTime () <= m_next.GetTime () )

   111             {

   112               m_current.SetPosition(m_next.GetPosition ());

   113               m_current.SetTime (now);

   114               m_velocity = Vector (0,0,0);

   115               NotifyCourseChange ();

   116             }

   117           else

   118             {

   119               m_current.SetTime (now);

   120             }

   121 

   122           return;

   123         }

   124 

   125       m_current = m_next;

   126       m_next = m_waypoints.front ();

   127       m_waypoints.pop_front ();

   128       newWaypoint = true;

   129 

   130       const double t_span = (m_next.GetTime () - m_current.GetTime ()).GetSeconds ();

   131       NS_ASSERT (t_span > 0);

   132       m_velocity.x = (m_next.GetPosition ().x - m_current.GetPosition ().x) / t_span;

   133       m_velocity.y = (m_next.GetPosition ().y - m_current.GetPosition ().y) / t_span;

   134       m_velocity.z = (m_next.GetPosition ().z - m_current.GetPosition ().z) / t_span;

   135     }

   136 

   137 

   138   const double t_diff = (now - m_current.GetTime ()).GetSeconds();

   139   Vector pos;

   140   pos.x = m_current.GetPosition ().x + m_velocity.x * t_diff;

   141   pos.y = m_current.GetPosition ().y + m_velocity.y * t_diff;

   142   pos.z = m_current.GetPosition ().z + m_velocity.z * t_diff;

   143   m_current.SetPosition (pos);

   144   m_current.SetTime (now);

   145 

   146   if ( newWaypoint )

   147     {

   148       NotifyCourseChange ();

   149     }

   150 }

   151 Vector

   152 WaypointMobilityModel::DoGetPosition (void) const

   153 {

   154   Update ();

   155   return m_current.GetPosition ();

   156 }

   157 void

   158 WaypointMobilityModel::DoSetPosition (const Vector &position)

   159 {

   160   const Time now = Simulator::Now ();

   161   Update ();

   162   m_current.SetTime (std::max (now, m_next.GetTime ()));

   163   m_current.SetPosition (position);

   164   m_velocity = Vector (0,0,0);

   165   NotifyCourseChange ();

   166 }

   167 void

   168 WaypointMobilityModel::EndMobility (void)

   169 {

   170   m_waypoints.clear ();

   171   m_current.SetTime (Seconds (std::numeric_limits<double>::infinity ()));

   172   m_next.SetTime (m_current.GetTime ());

   173   m_first = true;

   174 }

   175 Vector

   176 WaypointMobilityModel::DoGetVelocity (void) const

   177 {

   178   return m_velocity;

   179 }

   180 

   181 } // namespace ns3

   182