From b2ca5994a0cb1e1b67a3a144aa60587dcce946f5 Mon Sep 17 00:00:00 2001 From: Ivan Mahonin Date: Nov 29 2019 15:40:37 +0000 Subject: cone tool --- diff --git a/generator.cpp b/generator.cpp index 78dbcd9..07d3346 100644 --- a/generator.cpp +++ b/generator.cpp @@ -59,7 +59,8 @@ void GeneratorRowsZAX::touch( const Collider &collider, Track &track, Real safe_z, Real z1, Real z0, - Real a, Real x ) const + Real a, Real x, + bool skip_z0 ) const { Vector3 dir(0, sin(a), -cos(a)); Vector3 pos(x, 0, 0); @@ -69,7 +70,7 @@ void GeneratorRowsZAX::touch( tool_up(track, safe_z); } else { z = std::max(-z, z0); - if (z > z1) + if (z > z1 || (skip_z0 && z == z0)) tool_up(track, safe_z); else tool_feed(track, safe_z, z1, z, a, x); @@ -157,20 +158,26 @@ bool GeneratorRowsZAX::generate(const Collider &collider, Track &track) const { int iia = forward_a ? ia : count_a - ia - 1; Real a = iia*sa; - if (ia > 0) for(int iaa = 1; iaa < count_aa; ++iaa) { - Real aa = sa - sa*iaa/count_a; - aa = forward_a ? a - aa : a + aa; - Real x = forward_x ? min_x : max_x; - touch(collider, track, safe_z, z1, z0, aa, x); - if (track.points.size() >= max_count) break; + + if (feed_in_rotation && ia > 0) { + for(int iaa = 1; iaa < count_aa; ++iaa) { + Real aa = sa - sa*iaa/count_a; + aa = forward_a ? a - aa : a + aa; + Real x = forward_x ? min_x : max_x; + touch(collider, track, safe_z, z1, z0, aa, x, skip_middle_layers); + if (track.points.size() >= max_count) break; + } } for(int ix = 0; ix < count_x; ++ix) { int iix = forward_x ? ix : count_x - ix - 1; Real x = min_x + iix*(max_x - min_x)/(count_x - 1); - touch(collider, track, safe_z, z1, z0, a, x); + touch(collider, track, safe_z, z1, z0, a, x, skip_middle_layers); if (track.points.size() >= max_count) break; } + + if (!feed_in_rotation) + tool_up(track, safe_z); forward_x = !forward_x; if (track.points.size() >= max_count) break; diff --git a/generator.h b/generator.h index a696034..ebb091f 100644 --- a/generator.h +++ b/generator.h @@ -28,6 +28,9 @@ public: Real feed_speed; Real move_speed; + bool skip_middle_layers; + bool feed_in_rotation; + explicit GeneratorRowsZAX(): step_z(1), step_a(1), @@ -38,13 +41,15 @@ public: max_z(), safe_z(), feed_speed(600), - move_speed(1500) + move_speed(1500), + skip_middle_layers(false), + feed_in_rotation(false) { } void tool_point(Track &track, const TrackPoint &p) const; void tool_up(Track &track, Real safe_z) const; void tool_feed(Track &track, Real safe_z, Real z1, Real z, Real a, Real x) const; - void touch(const Collider &collider, Track &track, Real safe_z, Real z1, Real z0, Real a, Real x) const; + void touch(const Collider &collider, Track &track, Real safe_z, Real z1, Real z0, Real a, Real x, bool skip_z0) const; bool generate(const Collider &collider, Track &track) const override; }; diff --git a/geometry.h b/geometry.h index 3e687d0..5c4db9b 100644 --- a/geometry.h +++ b/geometry.h @@ -8,7 +8,7 @@ typedef double Real; -const Real precision = 1e-5; +const Real precision = 1e-10; inline int solve(Real* roots, Real k0, Real k1) { if (fabs(k1) <= precision) return 0; @@ -17,9 +17,9 @@ inline int solve(Real* roots, Real k0, Real k1) { } inline int solve(Real* roots, Real k0, Real k1, Real k2) { - if (fabs(k1) <= precision) return solve(roots, k0, k1); + if (fabs(k2) <= precision*precision) return solve(roots, k0, k1); Real D = k1*k1 - 4*k2*k0; - if (fabs(k1) <= precision) { + if (fabs(D) <= precision*precision) { if (roots) roots[0] = -0.5*k1/k2; return 1; } else diff --git a/main.cpp b/main.cpp index 8831df0..c55b970 100644 --- a/main.cpp +++ b/main.cpp @@ -73,6 +73,7 @@ public: glHint(GL_POINT_SMOOTH_HINT, GL_NICEST); glEnable(GL_NORMALIZE); + glDisable(GL_DEPTH_TEST); glEnable(GL_LIGHT0); float light0[] = {1, 1, 1, 0}; @@ -193,7 +194,8 @@ int main(int argc, char **argv) Model model; SimulatorXYZA simulator_xyza(400, 360); - FlatTool tool(1.5); + //FlatTool tool(1.5); + ConeTool tool(1.25, 10, 0.75); Collider collider(model, tool); Vector3 dir(1, 3, -3); @@ -208,7 +210,7 @@ int main(int argc, char **argv) if (argc > 2 && std::string(argv[2]) != ".") { Loader::load(model, argv[2]); - Loader::save(app.scene->track, "output/tmp.tap"); + //Loader::save(app.scene->track, "output/tmp.tap"); app.scene->model = &model; load_model = true; } @@ -216,9 +218,13 @@ int main(int argc, char **argv) if (argc > 3 && std::string(argv[3]) != ".") { if (!load_track && load_model) { GeneratorRowsZAX generator; - generator.step_z = 1.5; - generator.step_a = 2*0.9*tool.radius; + //generator.step_z = 1.5; + generator.step_z = 0; + //generator.step_a = 2*tool.radius*0.5; + generator.step_a = 0.5*1.5; generator.step_x = 0.5; + //generator.skip_middle_layers = true; + generator.feed_speed = 150; generator.generate(collider, app.scene->track); app.scene->simulator = &simulator_xyza; app.scene->simulator->simulate(app.scene->track, tool); diff --git a/scene.cpp b/scene.cpp index e236072..b1b6c67 100644 --- a/scene.cpp +++ b/scene.cpp @@ -188,9 +188,10 @@ void Scene::draw() { if (model) model->draw(); if (simulator) simulator->draw(); if (tool) tool->draw(tool_pos, tool_dir); + glRotated(-model_angle, 1, 0, 0); draw_track(); glPopMatrix(); - + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glDisable(GL_BLEND); glPopMatrix(); diff --git a/shape.cpp b/shape.cpp index d70b006..e8fd22c 100644 --- a/shape.cpp +++ b/shape.cpp @@ -1,8 +1,56 @@ +#include + #include "shape.h" -CilinderShape::CilinderShape(Real radius, const Vector3 &pos, const Vector3 &dir) { +PointShape::PointShape(const Vector3 &pos, const Vector3 &dir) { + Vector3 vx = dir.perp().norm(); + Vector3 vy = vx.cross(dir); + matrix = Matrix4( + vx.x , vx.y , vx.z , 0, + vy.x , vy.y , vy.z , 0, + dir.x, dir.y, dir.z, 0, + pos.x, pos.y, pos.z, 1 ).inv(); +} + +Real PointShape::base_distance_to_triangle(const Vector3 *v) { + Real dist = INFINITY; + + Vector3 d[3] = { + v[1] - v[0], + v[2] - v[1], + v[0] - v[2] }; + + // collision with plane + Vector3 perp = d[1].cross(d[2]); + if (fabs(perp.z) > precision) { + // nearest plane touch point + Vector3 p(0, 0, perp*v[0]/perp.z); + + // is touch point inside tringle + Real s = sign( perp.cross(d[0])*(p - v[0]), 0.1*precision ); + if ( s + && s == sign( perp.cross(d[1])*(p - v[1]), 0.1*precision ) + && s == sign( perp.cross(d[2])*(p - v[2]), 0.1*precision ) ) + dist = std::min(dist, p.z); + } + + return dist; +} + + +Real PointShape::distance_to_triangle(const Triangle &triangle) const { + Vector3 v[3] = { + matrix.transform( triangle.vertices[0] ), + matrix.transform( triangle.vertices[1] ), + matrix.transform( triangle.vertices[2] ) }; + return base_distance_to_triangle(v); +} + + + +CilinderShape::CilinderShape(const Vector3 &pos, const Vector3 &dir, Real radius) { Vector3 vx = dir.perp().norm(); Vector3 vy = vx.cross(dir); matrix = Matrix4( @@ -12,13 +60,12 @@ CilinderShape::CilinderShape(Real radius, const Vector3 &pos, const Vector3 &dir pos.x , pos.y , pos.z , 1 ).inv(); } -Real CilinderShape::distance_to_triangle(const Triangle &triangle) const { +Real CilinderShape::base_distance_to_triangle(const Vector3 *v) { Real dist = INFINITY; - Vector3 v[3], d[3]; + Vector3 d[3]; Real A[3], B[3], C[3]; for(int i = 0; i < 3; ++i) { - v[i] = matrix.transform( triangle.vertices[i] ); C[i] = v[i].x*v[i].x + v[i].y*v[i].y - 1; // collision with vertex @@ -63,3 +110,114 @@ Real CilinderShape::distance_to_triangle(const Triangle &triangle) const { return dist; } + + +Real CilinderShape::distance_to_triangle(const Triangle &triangle) const { + Vector3 v[3] = { + matrix.transform( triangle.vertices[0] ), + matrix.transform( triangle.vertices[1] ), + matrix.transform( triangle.vertices[2] ) }; + return base_distance_to_triangle(v); +} + + +ConeShape::ConeShape(const Vector3 &pos, const Vector3 &dir, Real radius, Real height, Real cut_radius) { + Vector3 vx = dir.perp().norm(); + Vector3 vy = vx.cross(dir); + matrix = Matrix4( + vx.x*radius, vx.y*radius, vx.z*radius, 0, + vy.x*radius, vy.y*radius, vy.z*radius, 0, + dir.x , dir.y , dir.z , 0, + pos.x , pos.y , pos.z , 1 ).inv(); + this->cut_radius = fabs(radius) > precision ? fabs(cut_radius/radius) : 0; + this->height = fabs(height); +} + +Real ConeShape::base_distance_to_triangle(const Vector3 *v, Real height, Real cut_radius) { + Real dist = INFINITY; + + height = fabs(height); + cut_radius = fabs(cut_radius); + + dist = std::min(dist, CilinderShape::base_distance_to_triangle(v) + height); + if (cut_radius <= precision) + dist = std::min(dist, PointShape::base_distance_to_triangle(v)); + if (cut_radius >= 1 - precision || height <= precision) + return dist; + + Real kz = (1 - cut_radius)/height; + Real dkz = 1/kz; + Real d_cut_radius = 1/cut_radius; + + Vector3 v_cut[3] = { + Vector3(v[0].x*d_cut_radius, v[0].y*d_cut_radius, v[0].z), + Vector3(v[1].x*d_cut_radius, v[1].y*d_cut_radius, v[1].z), + Vector3(v[2].x*d_cut_radius, v[2].y*d_cut_radius, v[2].z) }; + dist = std::min(dist, CilinderShape::base_distance_to_triangle(v_cut)); + + + Real xy2[3]; + for(int i = 0; i < 3; ++i) { + xy2[i] = v[i].x*v[i].x + v[i].y*v[i].y; + + // collision with vertex + if (xy2[i] < 1 && xy2[i] >= cut_radius*cut_radius) { + Real dz = (sqrt(xy2[i]) - cut_radius)*dkz; + dist = std::min(dist, v[i].z + dz); + } + } + + // collision with edge + Real kz2 = kz*kz; + Real cut_z = cut_radius*dkz; + for(int i = 0; i < 3; ++i) { + Vector3 d = v[(i+1)%3] - v[i]; + + Real A = d.x*d.x + d.y*d.y - d.z*d.z*kz2; + if (A > precision) { + // B = oB + kB*z + Real oB = 2*(d.x*v[i].x + d.y*v[i].y); + Real kB = -2*d.z*kz2; + + // C = oC + kC*z*z + Real oC = xy2[i]; + Real kC = -kz2; + + // B*B - 4AC = 0 + Real a = kB*kB - 4*A*kC; + Real b = 2*oB*kB; + Real c = oB*oB - 4*A*oC; + + Real roots[2]; + int count = solve(roots, c, b, a); + for(int j = 0; j < count; ++j) { + Real B = oB + kB*roots[j]; + Real l = -0.5*B/A; + + //Vector3 pp = d*l + Vector3(v[i].x, v[i].y, roots[j]); + //assert(fabs(pp.x*pp.x + pp.y*pp.y - kz2*pp.z*pp.z) < precision); + + if (l >= -precision && l <= 1 + precision) { + Real z = d.z*l + roots[j]; + if (z < 0) { + Vector3 p = d*l + v[i]; + Real r2 = p.x*p.x + p.y*p.y; + if (r2 <= 1 + precision && r2 >= cut_radius*cut_radius) + dist = std::min(dist, p.z - cut_z - z); + } + } + } + } + } + + return dist; +} + +Real ConeShape::distance_to_triangle(const Triangle &triangle) const { + Vector3 v[3] = { + matrix.transform( triangle.vertices[0] ), + matrix.transform( triangle.vertices[1] ), + matrix.transform( triangle.vertices[2] ) }; + return base_distance_to_triangle(v, height, cut_radius); +} + diff --git a/shape.h b/shape.h index 8a59027..ce423f8 100644 --- a/shape.h +++ b/shape.h @@ -13,11 +13,31 @@ public: virtual Real distance_to_triangle(const Triangle &triangle) const = 0; }; +class PointShape: public Shape { +public: + Matrix4 matrix; + PointShape(const Vector3 &pos, const Vector3 &dir); + Real distance_to_triangle(const Triangle &triangle) const override; + static Real base_distance_to_triangle(const Vector3 *v); +}; + class CilinderShape: public Shape { public: Matrix4 matrix; - CilinderShape(Real radius, const Vector3 &pos, const Vector3 &dir); + CilinderShape(const Vector3 &pos, const Vector3 &dir, Real radius); + Real distance_to_triangle(const Triangle &triangle) const override; + static Real base_distance_to_triangle(const Vector3 *v); +}; + +class ConeShape: public Shape { +public: + Matrix4 matrix; + Real cut_radius; + Real height; + + ConeShape(const Vector3 &pos, const Vector3 &dir, Real radius, Real height, Real cut_radius = 0); Real distance_to_triangle(const Triangle &triangle) const override; + static Real base_distance_to_triangle(const Vector3 *v, Real height, Real cut_radius = 0); }; diff --git a/tool.cpp b/tool.cpp index 51e2f8d..016b07a 100644 --- a/tool.cpp +++ b/tool.cpp @@ -4,53 +4,80 @@ #include "tool.h" -void FlatTool::draw(const Vector3 &pos, const Vector3 &dir) const { - const int segments = 16; - const Real len = 10*radius; +static void draw_disc(const Vector3 &p, const Vector3 &axis, Real r, bool flip = false, int segments = 16) { + Vector3 n = axis.norm(); + Vector3 vx = n.perp().norm(); + Vector3 vy = vx.cross(n); + if (flip) glNormal3d(-n.x, -n.y, -n.z); else glNormal3dv(n.c); + glBegin(GL_TRIANGLE_FAN); + glVertex3dv(p.c); + for(int i = 0; i <= segments; ++i) { + Real a = 2*M_PI*(flip ? segments - i - 1 : i)/segments; + Vector3 n = vx*cos(a) + vy*sin(a); + glVertex3dv((p + n*r).c); + } + glEnd(); +} - glPushMatrix(); - glEnable(GL_LIGHTING); - glColor4d(1, 1, 0, 0.25); - - glTranslated(pos.x, pos.y, pos.z); - Vector3 vx = dir.cross(Vector3(dir.y, dir.z, dir.x)); - vx = vx*(1/vx.len()); +static void draw_cone(const Vector3 &p0, const Vector3 &p1, Real r0, Real r1, bool flip = false, int segments = 16) { + Vector3 dir = (p1 - p0).norm(); + Vector3 vx = dir.perp().norm(); Vector3 vy = vx.cross(dir); - - Real matrix[16] = { - vx.x, vx.y, vx.z, 0, - vy.x, vy.y, vy.z, 0, - dir.x, dir.y, dir.z, 0, - 0, 0, 0, 1 }; - glMultMatrixd(matrix); - glBegin(GL_TRIANGLE_STRIP); for(int i = 0; i <= segments; ++i) { Real a = 2*M_PI*i/segments; - Real c = cos(a); - Real s = sin(a); - glNormal3b(c, s, 0); - glVertex3d(c*radius, s*radius, 0); - glVertex3d(c*radius, s*radius, -len); + Vector3 n = vx*cos(a) + vy*sin(a); + if (flip) { + glNormal3d(-n.x, -n.y, -n.z); + glVertex3dv((p1 + n*r1).c); + glVertex3dv((p0 + n*r0).c); + } else { + glNormal3dv(n.c); + glVertex3dv((p0 + n*r0).c); + glVertex3dv((p1 + n*r1).c); + } } glEnd(); +} - glBegin(GL_TRIANGLE_FAN); - glNormal3b(0, 0, 1); - for(int i = 0; i < segments; ++i) { - Real a = 2*M_PI*i/segments; - glVertex3d(cos(a)*radius, sin(a)*radius, 0); - } - glEnd(); + +void FlatTool::draw(const Vector3 &pos, const Vector3 &dir) const { + Vector3 p0 = pos - dir.norm()*10*radius; + + glEnable(GL_LIGHTING); + glColor4d(1, 1, 0, 0.25); - glBegin(GL_TRIANGLE_FAN); - glNormal3b(0, 0, -1); - for(int i = 0; i < segments; ++i) { - Real a = 2*M_PI*i/segments; - glVertex3d(cos(a)*radius, sin(a)*radius, -len); + draw_cone(p0, pos, radius, radius); + draw_disc(p0, pos - p0, radius, true); + draw_disc(pos, pos - p0, radius); + + glDisable(GL_LIGHTING); + glPopMatrix(); +} + + +void ConeTool::draw(const Vector3 &pos, const Vector3 &dir) const { + glEnable(GL_LIGHTING); + glColor4d(1, 1, 0, 0.25); + + Real r = fabs(radius); + Real cr = fabs(cut_radius); + Real h = fabs(height); + Real cl = 10*r; + + Vector3 p1 = pos; + if (h > precision && cr < r - precision) { + p1 = pos - dir.norm()*h; + draw_cone(p1, pos, r, cr); + if (cr > precision) + draw_disc(pos, pos - p1, cr); + cl = std::max(2*r, cl - h); } - glEnd(); + Vector3 p0 = p1 - dir.norm()*cl; + draw_cone(p0, p1, r, r); + draw_disc(p0, p1 - p0, r, true); glDisable(GL_LIGHTING); glPopMatrix(); } + diff --git a/tool.h b/tool.h index 19c31f4..a538744 100644 --- a/tool.h +++ b/tool.h @@ -15,12 +15,6 @@ public: virtual Real get_radius() const = 0; // must be >= 0 virtual Real get_height(Real offset) const = 0; // must be >= 0 - virtual void calc_collision( - Real plane_k, - Real &tool_height, - Real &collision_height, - Real &collision_offset ) const = 0; - virtual Real get_height_polar(Real distance, Real offset, Real angle) const { // must be >= distance // not exact solution, only for small angles if (distance < 1e-5) return 1e-5; @@ -46,22 +40,6 @@ public: Real get_height(Real offset) const override { return fabs(offset) <= fabs(radius) ? 0 : INFINITY; } - void calc_collision( - Real plane_k, - Real &tool_height, - Real &collision_height, - Real &collision_offset - ) const override - { - tool_height = 0; - collision_height = 0; - collision_offset = 0; - - if (plane_k <= 1e-5) return; - tool_height = collision_height = radius*plane_k; - collision_offset = radius; - } - Real get_height_polar(Real distance, Real offset, Real angle) const override { if (distance < 1e-5) return 1e-5; if (!(fabs(offset) <= fabs(radius))) return INFINITY; @@ -71,7 +49,78 @@ public: } Shape* create_collision_shape(const Vector3 &pos, const Vector3 &dir) const override { - return new CilinderShape(radius, pos, dir); + return new CilinderShape(pos, dir, radius); + } + + void draw(const Vector3 &pos, const Vector3 &dir) const override; +}; + +class ConeTool: public Tool { +public: + Real radius; + Real height; + Real cut_radius; + + explicit ConeTool(Real radius = 0.5, Real height = 1, Real cut_radius = 0): + radius(radius), height(height), cut_radius(cut_radius) { } + + Real get_radius() const override + { return radius; } + Real get_height(Real offset) const override { + Real r = fabs(radius); + offset = fabs(offset); + if (!(offset < r)) return INFINITY; + Real cr = fabs(cut_radius); + if (offset <= cr) return 0; + Real h = fabs(height); + if (h < precision) return 0; + return height*(offset - cr)/(r - cr); + } + + Real get_height_polar(Real distance, Real offset, Real angle) const override { + if (distance < 1e-5) return 1e-5; + + offset = fabs(offset); + angle = fabs(angle); + Real r = fabs(radius); + Real cr = fabs(cut_radius); + Real h = fabs(height); + if (h <= precision) cr = r; + + if (!(offset <= r)) return INFINITY; + Real t2 = tan(angle)*distance; t2 *= t2; + Real r2 = cr*cr - offset*offset; + if (t2 <= r2) return sqrt(distance*distance + t2); + + if (h <= precision) return INFINITY; + + Real tt2 = tan(angle)*(distance + h); tt2 *= tt2; + Real rr2 = radius*radius - offset*offset; + if (!(t2 <= rr2)) return INFINITY; + + Real c = cos(angle); + Real s = sin(angle); + + Real kz = (1 - cr)/h; + Real kz2 = kz*kz; + Real d = distance - cr/kz; + if (d < precision) return INFINITY; + + Real A = kz2*c*c - s*s; + Real B = -2*kz2*d*c; + Real C = kz2*d*d - offset*offset; + + Real roots[2]; + int count = solve(roots, C, B, A); + for(int i = 0; i < count; ++i) + if (roots[i]*c > distance && roots[i] > distance) + return roots[i]; + + return INFINITY; + } + + Shape* create_collision_shape(const Vector3 &pos, const Vector3 &dir) const override { + return new ConeShape(pos, dir, radius, height, cut_radius); } void draw(const Vector3 &pos, const Vector3 &dir) const override;