From 57bda08b9e1371b1cd06eff8ee8ef62bf908b39a Mon Sep 17 00:00:00 2001 From: Ivan Mahonin Date: Nov 27 2019 06:54:37 +0000 Subject: initial commit --- diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..7accb76 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +*.kdev4 +/input +/simu diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..1002176 --- /dev/null +++ b/build.sh @@ -0,0 +1,22 @@ +#!/bin/bash + +set -e + +if [ -z "$CPP" ]; then + CPP=c++ +fi + +if [ -z "$TARGET" ]; then + TARGET="simu" +fi + +LIBS="sdl2" +FLAGS="-Wall -lm -lGL -lGLU" + +FLAGS="`pkg-config --cflags --libs $LIBS` $FLAGS " +if [ "$1" == "debug" ]; then + FLAGS="$FLAGS -O0 -g" +fi + +echo $CPP $FLAGS *.cpp -o "$TARGET" + $CPP $FLAGS *.cpp -o "$TARGET" diff --git a/loader.cpp b/loader.cpp new file mode 100644 index 0000000..f100997 --- /dev/null +++ b/loader.cpp @@ -0,0 +1,159 @@ + +#include +#include +#include + +#include "loader.h" + + +class Loader::Internal { +public: + Track &track; + const std::string &filename; + + std::ifstream f; + int row; + int col; + + bool valid_point; + bool newline; + Point point; + + Internal(Track &track, const std::string &filename): + track(track), + filename(filename), + row(1), + col(), + valid_point(), + newline() + { } + + void error(const std::string &msg) { + std::cerr << "Error while loading track: " << filename + << ":" << row + << ":" << col + << ": " << msg + << std::endl; + } + + int get() { + int c = f.get(); + if (c == '\n') ++row, col = 0, newline = true; else ++col; + return c; + } + + bool read_pair(int &command, Real &number) { + command = 0; number = 0; + + char c = f.get(); ++row; + if (c < 0) return false; + + while(std::isspace(c)) c = get(); + if (c == '(') while(c != ')') c = get(); + while(std::isspace(c)) c = get(); + newline = false; + + if (c < 0) { return false; } + + command = c; + + while(std::isspace(f.peek())) get(); + if (newline) return true; + + Real sign = 1; + Real frac = 1; + int mode = 0; + while(true) { + c = f.peek(); + if (mode == 0 && c == '-') { sign = -1; mode = 1; } + else if (mode == 0 && c == '+') { sign = +1; mode = 1; } + else if (mode <= 1 && c == '.') { mode = 2; } + else if (mode <= 1 && isdigit(c)) { number = number*10 + (c - '0')*sign; sign = 1; mode = 1; } + else if (mode == 2 && isdigit(c)) { number += (c - '0')*(frac *= 0.1); } + else if (mode == 0) { error("expected number"); return c >= 0; } + else { break; } + get(); + } + + while(std::isspace(f.peek())) get(); + return true; + } + + void read_point_data(Real &data, int command, Real number) { + if (!valid_point) + error("arg " + std::string(1, command) + " was set in unknown command"); + data = number; + } + + bool load() { + track.points.clear(); + + std::cout << "loading: " << filename << std::endl; + + f.open(filename); + if (!f) { + error("cannot open file"); + f.close(); + return false; + } + + int command; + Real number; + point.speed = 600/60.0; + while(read_pair(command, number)) { + int inumber = (int)round(number); + bool arg = true; + if (command == 'X') { + read_point_data(point.position.x, command, number); + } else + if (command == 'Y') { + read_point_data(point.position.y, command, number); + } else + if (command == 'Z') { + read_point_data(point.position.z, command, number); + } else + if (command == 'A') { + read_point_data(point.angle, command, number); + } else + if (command == 'F') { + if (number < 1e-3) { + error("feed rate too small"); + number = 1e-3; + } + read_point_data(point.speed, command, number/60.0); + } else { + arg = false; + } + + if (newline || !arg) { + if (valid_point) track.points.push_back(point); + + if (!arg) { + valid_point = false; + if (command == 'G' && (inumber == 0 || inumber == 1)) + valid_point = true; + else + error("Unknown command: " + std::string(1, command)); + } + } + } + if (valid_point) track.points.push_back(point); + + track.split(1, 0, 0); + track.calc_length(); + track.split(0, 1, 0); + + std::cout << "track length: " << ((track.points.empty() ? 0.0 : track.points.back().time)/60.0) << " min" << std::endl; + + f.close(); + + std::cout << "loaded: " << filename << std::endl; + return true; + } +}; + + +bool Loader::load(Track &track, const std::string &filename) { + return Internal(track, filename).load(); +} + diff --git a/loader.h b/loader.h new file mode 100644 index 0000000..a0c42be --- /dev/null +++ b/loader.h @@ -0,0 +1,17 @@ + +#ifndef LOADER_H +#define LOADER_H + + +#include "track.h" + + +class Loader { +private: + class Internal; +public: + static bool load(Track &track, const std::string &filename); +}; + + +#endif diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..d1a5ead --- /dev/null +++ b/main.cpp @@ -0,0 +1,171 @@ + +#include +#include + +#include +#include +#include + +#include "scene.h" +#include "simulator.h" +#include "loader.h" + +class Main { +public: + bool sdl_initialized; + SDL_Window *mainWindow; + SDL_GLContext mainContext; + Scene *scene; + + Main(): + sdl_initialized(), + mainWindow(), + mainContext(), + scene() + { } + + ~Main() { deinit(); } + + bool init() { + if (SDL_Init(SDL_INIT_VIDEO) < 0) { + std::cerr << "SDL_Init failed" << std::endl; + deinit(); + return false; + } + sdl_initialized = true; + + mainWindow = SDL_CreateWindow( + "simu", + SDL_WINDOWPOS_CENTERED, + SDL_WINDOWPOS_CENTERED, + 512, + 512, + SDL_WINDOW_OPENGL | SDL_WINDOW_RESIZABLE ); + + if (!mainWindow) + { + std::cerr << "cannot create window" << std::endl; + std::cerr << SDL_GetError() << std::endl; + SDL_ClearError(); + deinit(); + return false; + } + + mainContext = SDL_GL_CreateContext(mainWindow); + SDL_GL_SetAttribute(SDL_GL_CONTEXT_PROFILE_MASK, SDL_GL_CONTEXT_PROFILE_CORE); + SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 3); + SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 2); + SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1); + + // vsync + SDL_GL_SetSwapInterval(1); + + scene = new Scene(); + + glClearColor(0.0, 0.0, 0.0, 0.0); + glEnable(GL_LINE_SMOOTH); + glEnable(GL_POINT_SMOOTH); + glHint(GL_LINE_SMOOTH_HINT, GL_NICEST); + glHint(GL_POINT_SMOOTH_HINT, GL_NICEST); + + glEnable(GL_LIGHT0); + float light0[] = {1, 1, 1, 0}; + glLightfv(GL_LIGHT0, GL_POSITION, light0); + + glEnable(GL_LIGHT1); + float light1[] = {-1, -1, -1, 0}; + float color1[] = {1, 1, 1, 1}; + glLightfv(GL_LIGHT1, GL_POSITION, light1); + glLightfv(GL_LIGHT1, GL_DIFFUSE, color1); + glLightfv(GL_LIGHT1, GL_SPECULAR, color1); + + glEnable(GL_COLOR_MATERIAL); + + resize(512, 512); + + return true; + } + + void deinit() { + if (scene) { delete scene; scene = 0; } + if (mainContext) { SDL_GL_DeleteContext(mainContext); mainContext = 0; } + if (mainWindow) { SDL_DestroyWindow(mainWindow); mainWindow = 0; } + if (sdl_initialized) { SDL_Quit(); sdl_initialized = false; } + } + + void resize(int width, int height) { + glViewport(0, 0, width, height); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + gluPerspective(90.0, (Real)width/height, 1, 1000); + glMatrixMode(GL_MODELVIEW); + } + + void loop() { + bool quit = false; + Uint32 prev_time = SDL_GetTicks(); + while(!quit) { + SDL_Event event; + while (SDL_PollEvent(&event)) { + if (event.type == SDL_QUIT) + quit = true; + if (event.type == SDL_WINDOWEVENT) { + if (event.window.event == SDL_WINDOWEVENT_RESIZED) { + resize(event.window.data1, event.window.data2); + } + } + if (event.type == SDL_KEYDOWN) { + switch (event.key.keysym.sym) { + case SDLK_ESCAPE: + quit = true; + break; + case SDLK_r: + case SDLK_g: + case SDLK_b: + break; + default: + break; + } + } + if (event.type == SDL_MOUSEMOTION) { + if (event.motion.state & SDL_BUTTON_LMASK) { + scene->angles.z += event.motion.xrel; + scene->angles.x += event.motion.yrel; + } + if (event.motion.state & SDL_BUTTON_RMASK) { + scene->offset.z *= pow(2, event.motion.yrel/100.0); + } + if (event.motion.state & SDL_BUTTON_MMASK) { + scene->offset.x += event.motion.xrel; + scene->offset.y += event.motion.yrel; + } + } + } + + Uint32 curr_time = SDL_GetTicks(); + double dt = (curr_time - prev_time)/1000.0; + prev_time = curr_time; + + scene->update(dt); + + glClear(GL_COLOR_BUFFER_BIT); + scene->draw(); + SDL_GL_SwapWindow(mainWindow); + } + } +}; + + +int main(int argc, char **argv) +{ + Main app; + if (!app.init()) return -1; + SimulatorXYZA simulator_xyza(800, 720); + if (argc > 1) { + Loader::load(app.scene->track, argv[1]); + app.scene->simulator = &simulator_xyza; + app.scene->simulator->simulate(app.scene->track, 1.5); + } + app.loop(); + return 0; +} diff --git a/raster.cpp b/raster.cpp new file mode 100644 index 0000000..dfd4b15 --- /dev/null +++ b/raster.cpp @@ -0,0 +1,28 @@ + +#include "raster.h" + + +void Raster::touch(int index, Real value, Real x, Real y, Real dx, Real dy, TouchPointList &points) { + dx = std::max(0.500001, dx); + dy = std::max(0.500001, dy); + int x0 = std::max(0, (int)ceil(x - dx)); + int y0 = std::max(0, (int)ceil(y - dy)); + int x1 = std::min(width, (int)ceil(x + dx)); + int y1 = std::min(height, (int)ceil(y + dy)); + dx = 1/dx; + dy = 1/dy; + + for(int iy = y0; iy < y1; ++iy) { + for(int ix = x0; ix < x1; ++ix) { + Real ddx = (x-ix)*dx; + Real ddy = (y-iy)*dy; + if (ddx + ddy <= 1.00001) { + Real &pixel = (*this)[iy][ix]; + if (pixel > value) { + points.push_back( TouchPoint(index, iy, ix, pixel, value) ); + pixel = value; + } + } + } + } +} diff --git a/raster.h b/raster.h new file mode 100644 index 0000000..ebf2198 --- /dev/null +++ b/raster.h @@ -0,0 +1,87 @@ + +#ifndef RASTER_H +#define RASTER_H + + +#include + +#include "track.h" + + +class TouchPoint; +typedef std::vector TouchPointList; + + +class Raster { +private: + Real *data; + int width; + int height; + +public: + explicit Raster(int width = 0, int height = 0, Real value = 0): + data(), width(), height() + { resize(width, height, value); } + + ~Raster() { reset(); } + + void fill(Real value) { + if (data) + for(Real *i = data, *e = i + width*height; i < e; ++i) + *i = value; + } + + void resize(int width, int height, Real value = 0) { + if (data) delete[] data; + data = 0; + this->width = this->height = 0; + + if (width > 0 && height > 0) { + this->width = width; + this->height = height; + this->data = new Real[width*height]; + fill(value); + } + } + + void reset() { resize(0, 0); } + + int get_width() const { return width; } + int get_height() const { return height; } + + const Real* get_data() const { return data; } + const Real* operator[](int row) const + { assert(data); return data + row*width; } + + Real* get_data() { return data; } + Real* operator[](int row) + { assert(data); return data + row*width; } + + Real get(int x, int y, Real def = 0) const + { return x >= 0 && y >= 0 && x < width && y < height ? (*this)[y][x] : def; } + void set(int x, int y, Real value) + { if (x >= 0 && y >= 0 && x < width && y < height) (*this)[y][x] = value; } + + void touch(int index, Real value, Real x, Real y, Real dx, Real dy, TouchPointList &points); +}; + + +class TouchPoint { +public: + int index; + int row; + int col; + Real prev; + Real next; + + TouchPoint(int index = 0, int row = 0, int col = 0, Real prev = 0, Real next = 0): + index(index), row(row), col(col), prev(prev), next(next) { } + + void apply_next(Raster &raster) const + { raster.set(col, row, next); } + void apply_prev(Raster &raster) const + { raster.set(col, row, prev); } +}; + + +#endif diff --git a/scene.cpp b/scene.cpp new file mode 100644 index 0000000..98f7ddd --- /dev/null +++ b/scene.cpp @@ -0,0 +1,165 @@ + +#include + +#include + +#include "scene.h" +#include "simulator.h" + + +Scene::Scene(): + angles(45.0, 0.0, 30.0), + offset(0, 0, -100), + stars_seed(rand()), + time(), + time_forward(0.0), + time_backward(20.0), + time_speed(10.0), + simulator() +{ } + +Scene::~Scene() { } + + +Vector4 Scene::color_by_time(Real t) { + return t <= time + ? color_by_time_past(t) + : color_by_time_future(t); +} + +Vector4 Scene::color_by_time_past(Real t) { + Vector4 far_past(0.33, 0.33, 0.33, 0); + Vector4 past(1, 1, 0.33, 1); + + Vector4 color; + if (time_backward <= 0.0001 || t < time - time_backward) + return far_past; + Real l = (time - t)/time_backward; + Real k = 1 - l; + for(int i = 0; i < 4; ++i) + color.c[i] = past.c[i]*k + far_past.c[i]*l; + return color; +} + +Vector4 Scene::color_by_time_future(Real t) { + Vector4 future(1, 1, 0.33, 1); + Vector4 far_future(0.166, 0.166, 0.66, 0); + + Vector4 color; + if (time_forward <= 0.0001 || time + time_forward < t) + return far_future; + Real l = (t - time)/time_forward; + Real k = 1 - l; + for(int i = 0; i < 4; ++i) + color.c[i] = future.c[i]*k + far_future.c[i]*l; + return color; +} + +void Scene::update(Real dt) { + time += time_speed*dt; + if (time < 0 || track.points.empty()) { + time = 0; + if (time_speed < 0) time_speed = 0; + } else + if (time > track.points.back().time) { + time = track.points.back().time; + if (time_speed > 0) time_speed = 0; + } + + if (simulator) { + int index = track.index_by_time(time); + if (index >= 0) simulator->scroll_to(index); + } +} + +Real Scene::random() + { return (rand()/(Real)RAND_MAX - 0.5)*2; } + +void Scene::draw_stars() { + int seed = rand(); + srand(stars_seed); + + glEnable(GL_FOG); + glFogf(GL_FOG_DENSITY, 0.002f); + + glColor4d(1, 1, 1, 1); + glBegin(GL_POINTS); + for(int i = 0; i < 1000; ++i) { + glVertex3d( + random()*1000.0, + random()*1000.0, + random()*1000.0 ); + } + glEnd(); + + glDisable(GL_FOG); + srand(seed); +} + +void Scene::draw_axes() { + glBegin(GL_LINES); + + glColor3d(1, 0, 0); + glVertex3d(0, 0, 0); + glVertex3d(50, 0, 0); + + glColor3d(0, 1, 0); + glVertex3d(0, 0, 0); + glVertex3d(0, 50, 0); + + glColor3d(0, 0, 1); + glVertex3d(0, 0, 0); + glVertex3d(0, 0, 50); + + glEnd(); +} + +void Scene::draw_track() { + int index = track.index_by_time(time); + if (index <= 0) return; + + const Point &p0 = track.points[index-1]; + const Point &p1 = track.points[index]; + Real l = p1.duration > 1e-5 ? (p0.time - time)/p1.duration : 0.5; + Point p = Point::median(l, p0, p1); + + glBegin(GL_LINE_STRIP); + for(int i = 0; i < index; ++i) { + glColor4dv( color_by_time_past(track.points[i].time).c ); + glVertex3dv( track.points[i].real_pos.c ); + } + glColor4dv( color_by_time_past(p.time).c ); + glVertex3dv( p.real_pos.c ); + glEnd(); + + glBegin(GL_LINE_STRIP); + for(int i = (int)track.points.size() - 1; i >= index; --i) { + glColor4dv( color_by_time_future(track.points[i].time).c ); + glVertex3dv( track.points[i].real_pos.c ); + } + glColor4dv( color_by_time_future(p.time).c ); + glVertex3dv( p.real_pos.c ); + glEnd(); +} + +void Scene::draw() { + glPushMatrix(); + + glTranslated(offset.x, offset.y, offset.z); + glRotated(-90, 1, 0, 0); + glRotated(angles.y, 0, 1, 0); + glRotated(angles.x, 1, 0, 0); + glRotated(angles.z, 0, 0, 1); + + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_DST_ALPHA); + + draw_stars(); + draw_axes(); + if (simulator) simulator->draw(); + draw_track(); + + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glDisable(GL_BLEND); + glPopMatrix(); +} diff --git a/scene.h b/scene.h new file mode 100644 index 0000000..1beae7f --- /dev/null +++ b/scene.h @@ -0,0 +1,46 @@ + +#ifndef SCENE_H +#define SCENE_H + + +#include "track.h" + +class Simulator; + +class Scene { +public: + Track track; + Vector3 angles; + Vector3 offset; + int stars_seed; + + Real time; + Real time_forward; + Real time_backward; + Real time_speed; + + Simulator *simulator; + + Scene(); + ~Scene(); + + Vector4 color_by_time(Real t); + Vector4 color_by_time_future(Real t); + Vector4 color_by_time_past(Real t); + + Real random(); + + void update(Real dt); + + void draw_stars(); + void draw_axes(); + void draw_track(); + void draw(); +}; + + +#endif + + + + diff --git a/simulator.cpp b/simulator.cpp new file mode 100644 index 0000000..d03bcdd --- /dev/null +++ b/simulator.cpp @@ -0,0 +1,120 @@ + +#include + +#include + +#include "simulator.h" + + +void SimulatorXYZA::simulate(const Track &track, Real tool_radius) { + points.clear(); + simu_index = 0; + x0 = x1 = 0; + + if (!raster.get_data() || track.points.empty()) return; + + std::cout << "SimulatorXYZA::simulate" << std::endl; + + x0 = x1 = track.points.front().position.x; + Real max_radius = 0; + for(PointList::const_iterator i = track.points.begin(); i != track.points.end(); ++i) { + Real radius = sqrt(i->position.y*i->position.y + i->position.z*i->position.z); + if (max_radius < radius) max_radius = radius; + if (x0 > i->position.x) x0 = i->position.x; + if (x1 < i->position.x) x1 = i->position.x; + } + raster.fill(max_radius); + + if (x1 + 1e-5 < x0) { + std::cout << "SimulatorXYZA::simulate: zero bound" << std::endl; + return; + } + + Real h = raster.get_height(); + Real kx = raster.get_width()/(x1 - x0); + Real ka = 1/360.0; + Real kr = h/(2*M_PI); + + Real dx = fabs(tool_radius*kx); + int index = 0; + for(PointList::const_iterator i = track.points.begin(); i != track.points.end(); ++i, ++index) { + Real radius = sqrt(i->position.y*i->position.y + i->position.z*i->position.z); + if (radius < 1e-5) radius = 1e-5; + Real dy = fabs(tool_radius/radius*kr); + + Real x = (i->position.x - x0)*kx; + Real y = i->angle*ka; + y = (y - floor(y))*h; + + raster.touch(index, radius, x, y, dx, dy, points); + if (y - dy < 0) + raster.touch(index, radius, x, y + h, dx, dy, points); + if (y + dy > h) + raster.touch(index, radius, x, y - h, dx, dy, points); + + if (index % 1000 == 0) + std::cout << "." << std::flush; + if (index % 100000 == 0) + std::cout << std::endl << std::flush; + } + simu_index = (int)points.size(); + + std::cout << "SimulatorXYZA::simulate: done" << std::endl; +} + +void SimulatorXYZA::scroll_to(int index) { + while(simu_index < (int)points.size() && points[simu_index].index <= index) + points[simu_index++].apply_next(raster); + while(simu_index > 0 && points[simu_index-1].index > index) + points[--simu_index].apply_prev(raster); +} + +void SimulatorXYZA::draw() const { + int w = raster.get_width(); + int h = raster.get_height(); + if (w < 2 || h < 2) return; + + Vector3 v, v0, v1; + + Real kx = (x1 - x0)/w; + Real ka = 2*M_PI/h; + + glEnable(GL_LIGHTING); + + glColor4d(0, 0, 1, 0.5); + + glBegin(GL_TRIANGLE_STRIP); + for(int r = 0; r < h; ++r) { + int rr = r ? r-1 : h-1; + Real a0 = rr*ka; + Real a1 = r*ka; + Real s0 = sin(a0), s1 = sin(a1); + Real c0 = cos(a0), c1 = cos(a1); + for(int c = 0; c < w; ++c) { + v0.x = v1.x = x0 + c*kx; + Real r0 = raster[rr][c]; + Real r1 = raster[r][c]; + v0.y = -s0*r0, v0.z = c0*r0; + v1.y = -s1*r1, v1.z = c1*r1; + + Vector3 dv0(v0.x - v.x, v0.y - v.y, v0.z - v.z); + Vector3 dv1(v1.x - v.x, v1.y - v.y, v1.z - v.z); + Vector3 norm( + dv0.y*dv1.z - dv0.z*dv1.y, + dv0.z*dv1.x - dv0.x*dv1.z, + dv0.x*dv1.y - dv0.y*dv1.x ); + Real l = sqrt(norm.x*norm.x + norm.y*norm.y + norm.z*norm.z); + l = l > 1e-5 ? 1/l : 0; + glNormal3d(norm.x*l, norm.y*l, norm.z*l); + + if (!c) glVertex3dv(v0.c); + glVertex3dv(v0.c); + glVertex3dv(v1.c); + v = v1; + } + glVertex3dv(v.c); + } + glEnd(); + + glDisable(GL_LIGHTING); +} diff --git a/simulator.h b/simulator.h new file mode 100644 index 0000000..14d81e0 --- /dev/null +++ b/simulator.h @@ -0,0 +1,53 @@ + +#ifndef SIMULATOR_H +#define SIMULATOR_H + + +#include + +#include "track.h" +#include "raster.h" + + +class Simulator { +public: + virtual ~Simulator() { } + virtual void simulate(const Track &track, Real tool_radius) = 0; + virtual void scroll_to(int index) = 0; + virtual void draw() const = 0; +}; + + +class SimulatorXYZA: public Simulator { +private: + TouchPointList points; + int simu_index; + +public: + Real x0; + Real x1; + Raster raster; + + SimulatorXYZA(): simu_index(), x0(), x1() { } + + SimulatorXYZA(int xpixels, int ypixels): + SimulatorXYZA() + { resize(xpixels, ypixels); } + + const TouchPointList& get_points() const { return points; } + int get_simu_index() const { return simu_index; } + int get_index() const { return simu_index <= 0 ? -1 : points[simu_index-1].index; } + + void resize(int xpixels, int ypixels) { + points.clear(); + simu_index = 0; + raster.resize(xpixels, ypixels); + } + + void simulate(const Track &track, Real tool_radius); + void scroll_to(int index); + void draw() const; +}; + + +#endif diff --git a/track.cpp b/track.cpp new file mode 100644 index 0000000..39406b3 --- /dev/null +++ b/track.cpp @@ -0,0 +1,75 @@ + +#include "track.h" + + +void Track::split(Real da, Real dl, Real dt) { + if (points.size() <= 1u) return; + + PointList new_points; + new_points.push_back(points.front()); + for(PointList::iterator i = points.begin() + 1; i != points.end(); ++i) { + Point &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( Point::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 = Point::calc_real_pos( points.front() ); + Vector3 prev = points.front().real_pos; + Real l = 0, time = 0; + for(PointList::iterator i = points.begin(); i != points.end(); ++i) { + if (i->speed < 1e-3) i->speed = 1e-3; + const Vector3 &p = i->real_pos = Point::calc_real_pos(*i); + Vector3 d(p.x - prev.x, p.y - prev.y, p.y - prev.y); + prev = p; + + i->l = l += i->length = sqrt(d.x*d.x + d.y*d.y + d.z*d.z); + i->time = time += i->duration = i->length/i->speed; + } +} + +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; +} diff --git a/track.h b/track.h new file mode 100644 index 0000000..ce4e9eb --- /dev/null +++ b/track.h @@ -0,0 +1,98 @@ + +#ifndef TRACK_H +#define TRACK_H + + +#include +#include + +#include + + +typedef double Real; + +class Vector3 { +public: + union { + struct { Real x, y, z; }; + struct { Real r, g, b; }; + struct { Real c[3]; }; + }; + + explicit Vector3(Real x = 0, Real y = 0, Real z = 0): + x(x), y(y), z(z) { } +}; + + +class Vector4 { +public: + union { + struct { Real x, y, z, w; }; + struct { Real r, g, b, a; }; + struct { Real c[4]; }; + }; + + explicit Vector4(Real x = 0, Real y = 0, Real z = 0, Real w = 0): + x(x), y(y), z(z), w(w) { } +}; + + + +class Point { +public: + Vector3 position; + Real angle; + Real speed; + + Real l; + Real time; + Real length; + Real duration; + Vector3 real_pos; + + Point(): angle(), speed(), l(), time(), length() { } + + static Vector3 calc_real_pos(const Point &point) { + Real a = point.angle*(M_PI/180); + Real s = sin(a); + Real c = cos(a); + return Vector3( + point.position.x, + point.position.y*c - point.position.z*s, + point.position.y*s + point.position.z*c ); + } + + static Point median(Real l, const Point &p0, const Point &p1) { + Real k = 1 - l; + Point p; + p.position.x = p0.position.x*k + p1.position.x*l; + p.position.y = p0.position.y*k + p1.position.y*l; + p.position.z = p0.position.z*k + p1.position.z*l; + p.angle = p0.angle *k + p1.angle *l; + p.speed = p1.speed ; + p.l = p0.l *k + p1.l *l; + p.time = p0.time *k + p1.time *l; + p.length = p1.length *l; + p.duration = p1.duration *l; + p.real_pos.x = p0.real_pos.x*k + p1.real_pos.x*l; + p.real_pos.y = p0.real_pos.y*k + p1.real_pos.y*l; + p.real_pos.z = p0.real_pos.z*k + p1.real_pos.z*l; + return p; + } +}; + +typedef std::vector PointList; + +class Track { +public: + PointList points; + + void split(Real da, Real dl, Real dt); + void calc_length(); + + int index_by_l(Real l) const; + int index_by_time(Real t) const; +}; + + +#endif