Left: | ||
Right: |
LEFT | RIGHT |
---|---|
1 /* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */ | 1 /* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */ |
2 /* | 2 /* |
3 * Copyright (c) 2015 Federico Guerra | 3 * Copyright (c) 2015 Federico Guerra |
4 * | 4 * |
5 * This program is free software; you can redistribute it and/or modify | 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 | 6 * it under the terms of the GNU General Public License version 2 as |
7 * published by the Free Software Foundation; | 7 * published by the Free Software Foundation; |
8 * | 8 * |
9 * This program is distributed in the hope that it will be useful, | 9 * This program is distributed in the hope that it will be useful, |
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of | 10 * but WITHOUT ANY WARRANTY; without even the implied warranty of |
(...skipping 21 matching lines...) Expand all Loading... | |
32 NS_LOG_COMPONENT_DEFINE ("WossWaypointMobilityModel"); | 32 NS_LOG_COMPONENT_DEFINE ("WossWaypointMobilityModel"); |
33 | 33 |
34 NS_OBJECT_ENSURE_REGISTERED (WossWaypointMobilityModel); | 34 NS_OBJECT_ENSURE_REGISTERED (WossWaypointMobilityModel); |
35 | 35 |
36 | 36 |
37 TypeId | 37 TypeId |
38 WossWaypointMobilityModel::GetTypeId (void) | 38 WossWaypointMobilityModel::GetTypeId (void) |
39 { | 39 { |
40 static TypeId tid = TypeId ("ns3::WossWaypointMobilityModel") | 40 static TypeId tid = TypeId ("ns3::WossWaypointMobilityModel") |
41 .SetParent<WaypointMobilityModel> () | 41 .SetParent<WaypointMobilityModel> () |
42 » .SetGroupName ("Woss") | 42 .SetGroupName ("Woss") |
43 .AddConstructor<WossWaypointMobilityModel> () | 43 .AddConstructor<WossWaypointMobilityModel> () |
44 ; | 44 ; |
45 return tid; | 45 return tid; |
46 } | 46 } |
47 | 47 |
48 | 48 |
49 WossWaypointMobilityModel::WossWaypointMobilityModel () | 49 WossWaypointMobilityModel::WossWaypointMobilityModel () |
50 { | 50 { |
51 } | 51 } |
52 | 52 |
53 WossWaypointMobilityModel::~WossWaypointMobilityModel () | 53 WossWaypointMobilityModel::~WossWaypointMobilityModel () |
54 { | 54 { |
55 } | 55 } |
56 | 56 |
57 void | 57 void |
58 WossWaypointMobilityModel::DoDispose (void) | 58 WossWaypointMobilityModel::DoDispose (void) |
59 { | 59 { |
60 NS_LOG_FUNCTION (this); | |
61 | |
60 MobilityModel::DoDispose (); | 62 MobilityModel::DoDispose (); |
61 } | 63 } |
62 | 64 |
63 void | 65 void |
64 WossWaypointMobilityModel::Update (void) const | 66 WossWaypointMobilityModel::Update (void) const |
65 { | 67 { |
68 NS_LOG_FUNCTION (this); | |
69 | |
66 const Time now = Simulator::Now (); | 70 const Time now = Simulator::Now (); |
67 bool newWaypoint = false; | 71 bool newWaypoint = false; |
68 | 72 |
69 if ( now < m_current.time ) | 73 if ( now < m_current.time ) |
70 { | |
71 return; | |
72 } | |
73 | |
74 while ( now >= m_next.time ) | |
75 { | |
76 if ( m_waypoints.empty () ) | |
77 { | 74 { |
78 if ( m_current.time <= m_next.time ) | |
79 { | |
80 /* | |
81 Set m_next.time = -1 to make sure this doesn't happen more than once. | |
82 The comparison here still needs to be '<=' in the case of mobility wit h one waypoint. | |
83 */ | |
Tom Henderson
2016/04/25 17:45:07
it is probably cleaner to implement this as a bool
Fedwar
2016/04/30 21:18:09
This part of the code is a copy paste of the origi
Tom Henderson
2016/05/07 14:57:52
Hmm, maybe it should be fixed there too... I don't
Fedwar
2016/05/08 16:19:48
Update is const because it is called by other cons
| |
84 m_next.time = Seconds (-1.0); | |
85 m_current.position = m_next.position; | |
86 m_current.time = now; | |
87 m_velocity = Vector (0,0,0); | |
88 NotifyCourseChange (); | |
89 } | |
90 else | |
91 { | |
92 m_current.time = now; | |
93 } | |
94 | |
95 return; | 75 return; |
96 } | 76 } |
97 | 77 |
98 m_current = m_next; | 78 while ( now >= m_next.time ) |
99 m_next = m_waypoints.front (); | 79 { |
100 m_waypoints.pop_front (); | 80 if ( m_waypoints.empty () ) |
101 newWaypoint = true; | 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 } | |
102 | 98 |
103 const double tSpan = (m_next.time - m_current.time).GetSeconds (); | 99 return; |
104 NS_ASSERT (tSpan > 0); | 100 } |
105 ···· | 101 |
106 m_velocity.x = (m_next.position.x - m_current.position.x) / tSpan; | 102 m_current = m_next; |
107 m_velocity.y = (m_next.position.y - m_current.position.y) / tSpan; | 103 m_next = m_waypoints.front (); |
108 m_velocity.z = (m_next.position.z - m_current.position.z) / tSpan; | 104 m_waypoints.pop_front (); |
109 } | 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 } | |
110 | 114 |
111 if ( now > m_current.time ) // Won't ever be less, but may be equal | 115 if ( now > m_current.time ) // Won't ever be less, but may be equal |
Tom Henderson
2016/04/25 17:45:07
should you check this with an assert? e.g.
NS_AS
Fedwar
2016/05/08 16:19:48
Done.
| |
112 { | 116 { |
113 const double tSpan = (m_next.time - m_current.time).GetSeconds (); | 117 const double tSpan = (m_next.time - m_current.time).GetSeconds (); |
114 | 118 |
115 const double tDiff = (now - m_current.time).GetSeconds (); | 119 const double tDiff = (now - m_current.time).GetSeconds (); |
116 | 120 |
117 NS_LOG_DEBUG ("tSpan=" << tSpan << "; tDiff=" << tDiff); | 121 NS_LOG_DEBUG ("tSpan=" << tSpan << "; tDiff=" << tDiff); |
118 ·· | 122 |
119 woss::CoordZ cCurrent = woss::CoordZ::getCoordZFromCartesianCoords (m_curren t.position.x, | 123 woss::CoordZ cCurrent = woss::CoordZ::getCoordZFromCartesianCoords (m_curr ent.position.x, |
120 m_curr ent.position.y, | 124 m_curr ent.position.y, |
121 m_curr ent.position.z); | 125 m_curr ent.position.z); |
122 ···· | |
123 woss::CoordZ cNext = woss::CoordZ::getCoordZFromCartesianCoords (m_next.posi tion.x,· | |
124 m_next.pos ition.y, | |
125 m_next.pos ition.z); | |
126 ···· | |
127 NS_LOG_DEBUG ("cCurrent=" << cCurrent << "; cNext=" << cNext); | |
128 | 126 |
129 double circleDist = cCurrent.getGreatCircleDistance (cNext); | 127 woss::CoordZ cNext = woss::CoordZ::getCoordZFromCartesianCoords (m_next.po sition.x, |
130 double depthDiff = cNext.getDepth () - cCurrent.getDepth (); // signed! | 128 m_next.po sition.y, |
131 double ratio = tDiff / tSpan; | 129 m_next.po sition.z); |
132 ···· | |
133 NS_LOG_DEBUG ("circleDist=" << circleDist << "; depthDiff=" << depthDiff << "; ratio=" << ratio); | |
134 ···· | |
135 NS_ASSERT (ratio <= 1.0); | |
136 ···· | |
137 woss::Coord cNew = cCurrent; | |
138 double newDepth = cCurrent.getDepth ();· | |
139 ···· | |
140 if (circleDist > 0.0)· | |
141 cNew = woss::Coord::getCoordAlongGreatCircle (cCurrent, cNext, ratio*circl eDist); | |
142 ······ | |
143 if (depthDiff != 0.0) | |
144 newDepth += ratio*(depthDiff); | |
145 | 130 |
146 NS_LOG_DEBUG ("cNew=" << cNew << "; newDepth=" << newDepth); | 131 NS_LOG_DEBUG ("cCurrent=" << cCurrent << "; cNext=" << cNext); |
147 ···· | |
148 woss::CoordZ czNew = woss::CoordZ (cNew, newDepth); | |
149 | 132 |
150 NS_LOG_DEBUG ("czNew=" << czNew); | 133 double circleDist = cCurrent.getGreatCircleDistance (cNext); |
151 ···· | 134 double depthDiff = cNext.getDepth () - cCurrent.getDepth (); // signed! |
152 m_current.position.x = czNew.getCartX (); | 135 double ratio = tDiff / tSpan; |
153 m_current.position.y = czNew.getCartY (); | |
154 m_current.position.z = czNew.getCartZ (); | |
155 m_current.time = now; | |
156 | 136 |
157 NS_LOG_DEBUG ("m_current=" << m_current);···· | 137 NS_LOG_DEBUG ("circleDist=" << circleDist << "; depthDiff=" << depthDiff < < "; ratio=" << ratio); |
158 } | 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 } | |
159 | 167 |
160 if ( newWaypoint ) | 168 if ( newWaypoint ) |
161 { | 169 { |
162 NotifyCourseChange (); | 170 NotifyCourseChange (); |
163 } | 171 } |
164 } | 172 } |
165 | 173 |
166 | 174 |
167 } // namespace ns3 | 175 } // namespace ns3 |
168 | 176 |
169 #endif /* NS3_WOSS_SUPPORT */ | 177 #endif /* NS3_WOSS_SUPPORT */ |
LEFT | RIGHT |