#include "track.h"
void Track::split(Real da, Real dl, Real dt) {
if (points.size() <= 1u) return;
TrackPointList new_points;
new_points.push_back(points.front());
for(TrackPointList::iterator i = points.begin() + 1; i != points.end(); ++i) {
TrackPoint &prev = *(i - 1);
int count = 0;
if (da > 1e-5) {
int c = (int)ceil(fabs(i->angle - prev.angle)/da);
if (count < c) count = c;
}
if (dl > 1e-5) {
int c = (int)ceil(fabs(i->length)/dl);
if (count < c) count = c;
}
if (dt > 1e-5) {
int c = (int)ceil(fabs(i->duration)/dt);
if (count < c) count = c;
}
for(int j = 1; j < count-1; ++j)
new_points.push_back( TrackPoint::median(j/(Real)count, prev, *i) );
new_points.push_back(*i);
}
points.swap( new_points );
}
void Track::calc_length() {
if (points.empty()) return;
points.front().real_pos = TrackPoint::calc_real_pos( points.front() );
Vector3 prev = points.front().real_pos;
Real l = 0, time = 0;
for(TrackPointList::iterator i = points.begin(); i != points.end(); ++i) {
if (i->speed < 1e-3) i->speed = 1e-3;
i->real_pos = TrackPoint::calc_real_pos(*i);
Vector3 d = i->real_pos - prev;
prev = i->real_pos;
i->length = d.len();
l += i->length;
i->l = l;
i->duration = i->length/i->speed;
time += i->duration;
i->time = time;
}
}
int Track::index_by_l(Real l) const {
int i0 = 1, i1 = (int)points.size() - 1;
if (i1 <= i0) return -1;
if (l <= points[i0].l) return i0;
if (points[i1].l <= l) return i1;
while(i0 + 1 < i1) {
int i = (i0 + i1)/2;
(points[i].l <= l ? i0 : i1) = i;
}
return i0;
}
int Track::index_by_time(Real t) const {
int i0 = 1, i1 = (int)points.size() - 1;
if (i1 <= i0) return -1;
if (t <= points[i0].time) return i0;
if (points[i1].time <= t) return i1;
while(i0 + 1 < i1) {
int i = (i0 + i1)/2;
(points[i].time <= t ? i0 : i1) = i;
}
return i0;
}