LEFT | RIGHT |
(no file at all) | |
| 1 /* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */ |
| 2 /* |
| 3 * Copyright (c) 2015 Federico Guerra |
| 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: Federico Guerra <federico@guerra-tlc.com> |
| 19 */ |
| 20 |
| 21 #ifdef NS3_WOSS_SUPPORT |
| 22 |
| 23 #include <coordinates-definitions.h> |
| 24 #include "ns3/simulator.h" |
| 25 #include "ns3/log.h" |
| 26 #include "woss-waypoint-mobility-model.h" |
| 27 |
| 28 |
| 29 |
| 30 namespace ns3 { |
| 31 |
| 32 NS_LOG_COMPONENT_DEFINE ("WossWaypointMobilityModel"); |
| 33 |
| 34 NS_OBJECT_ENSURE_REGISTERED (WossWaypointMobilityModel); |
| 35 |
| 36 |
| 37 TypeId |
| 38 WossWaypointMobilityModel::GetTypeId (void) |
| 39 { |
| 40 static TypeId tid = TypeId ("ns3::WossWaypointMobilityModel") |
| 41 .SetParent<WaypointMobilityModel> () |
| 42 .SetGroupName ("Woss") |
| 43 .AddConstructor<WossWaypointMobilityModel> () |
| 44 ; |
| 45 return tid; |
| 46 } |
| 47 |
| 48 |
| 49 WossWaypointMobilityModel::WossWaypointMobilityModel () |
| 50 { |
| 51 } |
| 52 |
| 53 WossWaypointMobilityModel::~WossWaypointMobilityModel () |
| 54 { |
| 55 } |
| 56 |
| 57 void |
| 58 WossWaypointMobilityModel::DoDispose (void) |
| 59 { |
| 60 NS_LOG_FUNCTION (this); |
| 61 |
| 62 MobilityModel::DoDispose (); |
| 63 } |
| 64 |
| 65 void |
| 66 WossWaypointMobilityModel::Update (void) const |
| 67 { |
| 68 NS_LOG_FUNCTION (this); |
| 69 |
| 70 const Time now = Simulator::Now (); |
| 71 bool newWaypoint = false; |
| 72 |
| 73 if ( now < m_current.time ) |
| 74 { |
| 75 return; |
| 76 } |
| 77 |
| 78 while ( now >= m_next.time ) |
| 79 { |
| 80 if ( m_waypoints.empty () ) |
| 81 { |
| 82 if ( m_current.time <= m_next.time ) |
| 83 { |
| 84 /* |
| 85 Set m_next.time = -1 to make sure this doesn't happen more than
once. |
| 86 The comparison here still needs to be '<=' in the case of mobili
ty with one waypoint. |
| 87 */ |
| 88 m_next.time = Seconds (-1.0); |
| 89 m_current.position = m_next.position; |
| 90 m_current.time = now; |
| 91 m_velocity = Vector (0,0,0); |
| 92 NotifyCourseChange (); |
| 93 } |
| 94 else |
| 95 { |
| 96 m_current.time = now; |
| 97 } |
| 98 |
| 99 return; |
| 100 } |
| 101 |
| 102 m_current = m_next; |
| 103 m_next = m_waypoints.front (); |
| 104 m_waypoints.pop_front (); |
| 105 newWaypoint = true; |
| 106 |
| 107 const double tSpan = (m_next.time - m_current.time).GetSeconds (); |
| 108 NS_ASSERT (tSpan > 0); |
| 109 |
| 110 m_velocity.x = (m_next.position.x - m_current.position.x) / tSpan; |
| 111 m_velocity.y = (m_next.position.y - m_current.position.y) / tSpan; |
| 112 m_velocity.z = (m_next.position.z - m_current.position.z) / tSpan; |
| 113 } |
| 114 |
| 115 if ( now > m_current.time ) // Won't ever be less, but may be equal |
| 116 { |
| 117 const double tSpan = (m_next.time - m_current.time).GetSeconds (); |
| 118 |
| 119 const double tDiff = (now - m_current.time).GetSeconds (); |
| 120 |
| 121 NS_LOG_DEBUG ("tSpan=" << tSpan << "; tDiff=" << tDiff); |
| 122 |
| 123 woss::CoordZ cCurrent = woss::CoordZ::getCoordZFromCartesianCoords (m_curr
ent.position.x, |
| 124 m_curr
ent.position.y, |
| 125 m_curr
ent.position.z); |
| 126 |
| 127 woss::CoordZ cNext = woss::CoordZ::getCoordZFromCartesianCoords (m_next.po
sition.x, |
| 128 m_next.po
sition.y, |
| 129 m_next.po
sition.z); |
| 130 |
| 131 NS_LOG_DEBUG ("cCurrent=" << cCurrent << "; cNext=" << cNext); |
| 132 |
| 133 double circleDist = cCurrent.getGreatCircleDistance (cNext); |
| 134 double depthDiff = cNext.getDepth () - cCurrent.getDepth (); // signed! |
| 135 double ratio = tDiff / tSpan; |
| 136 |
| 137 NS_LOG_DEBUG ("circleDist=" << circleDist << "; depthDiff=" << depthDiff <
< "; ratio=" << ratio); |
| 138 |
| 139 NS_ASSERT (ratio <= 1.0); |
| 140 |
| 141 woss::Coord cNew = cCurrent; |
| 142 double newDepth = cCurrent.getDepth (); |
| 143 |
| 144 if (circleDist > 0.0) |
| 145 { |
| 146 cNew = woss::Coord::getCoordAlongGreatCircle (cCurrent, cNext, ratio *
circleDist); |
| 147 } |
| 148 |
| 149 if (depthDiff != 0.0) |
| 150 { |
| 151 newDepth += ratio * (depthDiff); |
| 152 } |
| 153 |
| 154 NS_LOG_DEBUG ("cNew=" << cNew << "; newDepth=" << newDepth); |
| 155 |
| 156 woss::CoordZ czNew = woss::CoordZ (cNew, newDepth); |
| 157 |
| 158 NS_LOG_DEBUG ("czNew=" << czNew); |
| 159 |
| 160 m_current.position.x = czNew.getCartX (); |
| 161 m_current.position.y = czNew.getCartY (); |
| 162 m_current.position.z = czNew.getCartZ (); |
| 163 m_current.time = now; |
| 164 |
| 165 NS_LOG_DEBUG ("m_current=" << m_current); |
| 166 } |
| 167 |
| 168 if ( newWaypoint ) |
| 169 { |
| 170 NotifyCourseChange (); |
| 171 } |
| 172 } |
| 173 |
| 174 |
| 175 } // namespace ns3 |
| 176 |
| 177 #endif /* NS3_WOSS_SUPPORT */ |
LEFT | RIGHT |