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