| 149 | } |
| 150 | |
| 151 | Math::float3 World::Waynet::interpolatePositionOnPath(const WaynetInstance& waynet, const std::vector<size_t>& path, float p) |
| 152 | { |
| 153 | if (path.size() == 1) |
| 154 | return waynet.waypoints[path[0]].position; |
| 155 | |
| 156 | if (p >= 1.0f) |
| 157 | return waynet.waypoints[path[path.size() - 1]].position; |
| 158 | |
| 159 | float pathLength = getPathLength(waynet, path); |
| 160 | float traveled = pathLength * p; |
| 161 | |
| 162 | float d = 0.0f; |
| 163 | for (size_t i = 0; i < path.size() - 1; i++) |
| 164 | { |
| 165 | // Check if the percentage-value is inside the current region |
| 166 | float len = (waynet.waypoints[path[i]].position - waynet.waypoints[path[i + 1]].position).length(); |
| 167 | |
| 168 | if (d <= traveled && traveled < d + len) |
| 169 | { |
| 170 | //LogInfo() << "Segment: " << i << "/" << path.size() << " at " << d << ", need " << traveled << "(Total: " << pathLength << ")"; |
| 171 | // Found our segment! |
| 172 | // Upscale our percentage to this single segment |
| 173 | p = (traveled - d) / len; |
| 174 | //p = (p - d) / lenNorm; |
| 175 | return Math::float3::lerp(waynet.waypoints[path[i]].position, |
| 176 | waynet.waypoints[path[i + 1]].position, p); |
| 177 | } |
| 178 | |
| 179 | d += len; |
| 180 | } |
| 181 | |
| 182 | // FIXME: Throw an exception or something? |
| 183 | return waynet.waypoints[path[0]].position; |
| 184 | } |
| 185 | |
| 186 | float World::Waynet::getPathLength(const WaynetInstance& waynet, const std::vector<size_t>& path) |
| 187 | { |