|
|
0a0ecd |
|
|
|
0a0ecd |
#include <iostream></iostream>
|
|
|
0a0ecd |
|
|
|
0a0ecd |
#include "generatorradial.h"
|
|
|
0a0ecd |
|
|
|
0a0ecd |
|
|
|
0a0ecd |
void GeneratorRadialXYZ::tool_point(Track &track, const TrackPoint &p) const {
|
|
|
0a0ecd |
if (!track.points.empty()) {
|
|
|
0a0ecd |
TrackPoint &last = track.points.back();
|
|
|
0a0ecd |
if ( (last.position - p.position).len() <= precision
|
|
|
0a0ecd |
&& fabs(last.angle - p.angle) <= precision )
|
|
|
0a0ecd |
{
|
|
|
0a0ecd |
last.speed = p.speed;
|
|
|
0a0ecd |
return;
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
track.points.push_back(p);
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
|
|
|
0a0ecd |
void GeneratorRadialXYZ::tool_up(Track &track, Real safe_z) const {
|
|
|
0a0ecd |
if (track.points.empty()) return;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
const TrackPoint &prev = track.points.back();
|
|
|
0a0ecd |
if (prev.position.z == safe_z) return;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
TrackPoint p;
|
|
|
0a0ecd |
p.position = prev.position;
|
|
|
0a0ecd |
p.position.z = safe_z;
|
|
|
0a0ecd |
p.speed = move_speed/60.0;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
tool_point(track, p);
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
|
|
|
0a0ecd |
void GeneratorRadialXYZ::tool_feed(Track &track, Real safe_z, Real z1, Real z, Real x, Real y) const {
|
|
|
0a0ecd |
TrackPoint p;
|
|
|
0a0ecd |
p.position = Vector3(x, y, safe_z);
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (track.points.empty() || track.points.back().position.z == safe_z) {
|
|
|
0a0ecd |
p.speed = move_speed/60.0;
|
|
|
0a0ecd |
tool_point(track, p);
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (safe_z > z1 && z1 > z) {
|
|
|
0a0ecd |
p.position.z = z1;
|
|
|
0a0ecd |
tool_point(track, p);
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
p.position.z = z;
|
|
|
0a0ecd |
p.speed = feed_speed/60.0;
|
|
|
0a0ecd |
tool_point(track, p);
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
|
|
|
0a0ecd |
void GeneratorRadialXYZ::touch(
|
|
|
0a0ecd |
const Collider &collider,
|
|
|
0a0ecd |
Track &track,
|
|
|
0a0ecd |
Real safe_z, Real z1, Real z0,
|
|
|
0a0ecd |
Real x, Real y,
|
|
|
0a0ecd |
bool skip_z0 ) const
|
|
|
0a0ecd |
{
|
|
|
0a0ecd |
Vector3 dir(0, 0, -1);
|
|
|
0a0ecd |
Vector3 pos(x, y, 0);
|
|
|
0a0ecd |
|
|
|
0a0ecd |
Real z = collider.distance_to_model(pos, dir);
|
|
|
0a0ecd |
if (std::isinf(z))
|
|
|
0a0ecd |
z = -z0;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
z = std::max(-z, z0);
|
|
|
0a0ecd |
if (z > z1 || (skip_z0 && z == z0))
|
|
|
0a0ecd |
tool_up(track, safe_z);
|
|
|
0a0ecd |
else
|
|
|
0a0ecd |
tool_feed(track, safe_z, z1, z, x, y);
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
|
|
|
0a0ecd |
bool GeneratorRadialXYZ::generate(const Collider &collider, Track &track) const {
|
|
|
0a0ecd |
track.points.clear();
|
|
|
0a0ecd |
|
|
|
0a0ecd |
const Real precision = 1e-3;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (collider.model.triangles.empty()) return false;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (step_z != 0 && step_z <= precision) return false;
|
|
|
0a0ecd |
if (step_a <= precision) return false;
|
|
|
0a0ecd |
if (step_r <= precision) return false;
|
|
|
0a0ecd |
if (feed_speed <= precision) return false;
|
|
|
0a0ecd |
if (move_speed <= precision) return false;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
std::cout << "GeneratorRadialXYZ::generate" << std::endl;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
unsigned long long max_count = 100ull*1024*1024/sizeof(track.points.front());
|
|
|
0a0ecd |
|
|
|
0a0ecd |
Real min_r = this->min_r;
|
|
|
0a0ecd |
Real max_r = this->max_r;
|
|
|
0a0ecd |
Real min_z = this->min_z;
|
|
|
0a0ecd |
Real max_z = this->max_z;
|
|
|
0a0ecd |
Real safe_z = this->safe_z;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
const TriangleList &triangles = collider.model.triangles;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
// calc bounds
|
|
|
0a0ecd |
Real br1 = 0, bz0 = INFINITY, bz1 = -INFINITY;
|
|
|
0a0ecd |
for(TriangleList::const_iterator i = triangles.begin(); i != triangles.end(); ++i) {
|
|
|
0a0ecd |
for(int j = 0; j < 3; ++j) {
|
|
|
0a0ecd |
const Vector3 &v = i->vertices[j];
|
|
|
0a0ecd |
const Real r = sqrt(v.x*v.x + v.y*v.y);
|
|
|
0a0ecd |
br1 = std::max(br1, r);
|
|
|
0a0ecd |
bz0 = std::min(bz0, v.z);
|
|
|
0a0ecd |
bz1 = std::max(bz1, v.z);
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (br1 < step_a || bz1 + precision <= bz0) {
|
|
|
0a0ecd |
std::cout << "GeneratorRadialXYZ::generate: zero model size" << std::endl;
|
|
|
0a0ecd |
return false;
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
br1 += collider.tool.get_radius() + 0.5*step_r;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (max_r <= min_r) { min_r = 0; max_r = br1; }
|
|
|
0a0ecd |
if (max_z <= min_z) { min_z = bz0; max_z = bz1; }
|
|
|
0a0ecd |
max_z = std::max(max_z, bz1);
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (max_r - precision <= min_r || max_z - precision <= min_z) {
|
|
|
0a0ecd |
std::cout << "GeneratorRadialXYZ::generate: zero bounds" << std::endl;
|
|
|
0a0ecd |
return false;
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
min_r = std::max(min_r - collider.tool.get_radius(), step_a*0.5);
|
|
|
0a0ecd |
safe_z = std::max(safe_z, max_z + 1);
|
|
|
0a0ecd |
|
|
|
0a0ecd |
int count_z = step_z ? std::max(1, (int)ceil((max_z - min_z)/step_z - precision)) : 1;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
int count_rl = full_radius ? 1 : std::max(1, (int)ceil(log2(max_r/min_r) - precision) + 1);
|
|
|
0a0ecd |
Real kr = pow(max_r/min_r, -1/Real(count_rl));
|
|
|
0a0ecd |
|
|
|
0a0ecd |
Real z1 = max_z + precision;
|
|
|
0a0ecd |
int forward_rl = true;
|
|
|
0a0ecd |
for(int iz = 0; iz < count_z; ++iz) {
|
|
|
0a0ecd |
Real z0 = iz == count_z-1 ? min_z : max_z - (iz+1)*(max_z - min_z)/count_z;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
for(int irl = 0; irl < count_rl; ++irl) {
|
|
|
0a0ecd |
int iirl = forward_rl ? irl : count_rl - irl - 1;
|
|
|
0a0ecd |
Real r1 = max_r*pow(kr, Real(iirl));
|
|
|
0a0ecd |
Real r0 = r1*kr;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
int count_a = std::max(3, (int)ceil(2*M_PI/step_a*r1 - precision));
|
|
|
0a0ecd |
if (count_a%2 == 0) ++count_a;
|
|
|
0a0ecd |
double sa = 2*M_PI/count_a;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
int count_aa0 = std::max(1, (int)ceil(sa*r0/step_r - precision));
|
|
|
0a0ecd |
int count_aa1 = std::max(1, (int)ceil(sa*r1/step_r - precision));
|
|
|
0a0ecd |
|
|
|
0a0ecd |
int count_r = std::max(1, (int)ceil((r1-r0)/step_r - precision));
|
|
|
0a0ecd |
|
|
|
0a0ecd |
int forward_r = !forward_rl;
|
|
|
0a0ecd |
for(int ia = 0; ia < count_a; ++ia) {
|
|
|
0a0ecd |
Real a = ia*sa;
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (ia > 0) {
|
|
|
0a0ecd |
int count_aa = forward_r ? count_aa0 : count_aa1;
|
|
|
0a0ecd |
Real r = forward_r ? r0 : r1;
|
|
|
0a0ecd |
for(int iaa = 1; iaa < count_aa; ++iaa) {
|
|
|
0a0ecd |
Real aa = a - (sa - sa*iaa/count_a);
|
|
|
0a0ecd |
Real x = cos(aa)*r;
|
|
|
0a0ecd |
Real y = sin(aa)*r;
|
|
|
0a0ecd |
touch(collider, track, safe_z, z1, z0, x, y, skip_middle_layers);
|
|
|
0a0ecd |
if (track.points.size() >= max_count) break;
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
Real c = cos(a);
|
|
|
0a0ecd |
Real s = sin(a);
|
|
|
0a0ecd |
for(int ir = 0; ir < count_r; ++ir) {
|
|
|
0a0ecd |
int iir = forward_r ? ir : count_r - ir - 1;
|
|
|
0a0ecd |
Real r = r0 + iir*(r1 - r0)/(count_r - 1);
|
|
|
0a0ecd |
touch(collider, track, safe_z, z1, z0, c*r, s*r, skip_middle_layers);
|
|
|
0a0ecd |
if (track.points.size() >= max_count) break;
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (track.points.size() >= max_count) break;
|
|
|
0a0ecd |
forward_r = !forward_r;
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
assert(forward_r == forward_rl);
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (track.points.size() >= max_count) break;
|
|
|
0a0ecd |
std::cout << "." << std::flush;
|
|
|
0a0ecd |
r1 = r0;
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (track.points.size() >= max_count) break;
|
|
|
0a0ecd |
std::cout << std::endl << std::flush;
|
|
|
0a0ecd |
forward_rl = !forward_rl;
|
|
|
0a0ecd |
z1 = z0;
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
tool_up(track, safe_z);
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (track.points.size() >= max_count) {
|
|
|
0a0ecd |
std::cout << "GeneratorRadialXYZ::generate: reached memory limit" << std::endl;
|
|
|
0a0ecd |
return false;
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
if (track.points.size() <= 1) {
|
|
|
0a0ecd |
std::cout << "GeneratorRadialXYZ::generate: generated track is empty" << std::endl;
|
|
|
0a0ecd |
return false;
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|
|
|
0a0ecd |
track.calc_length();
|
|
|
0a0ecd |
track.split(0, 0.1, 0);
|
|
|
0a0ecd |
|
|
|
0a0ecd |
std::cout << "GeneratorRadialXYZ::generate: track duration " << (track.points.back().time/60) << std::endl;
|
|
|
0a0ecd |
std::cout << "GeneratorRadialXYZ::generate: done" << std::endl;
|
|
|
0a0ecd |
return true;
|
|
|
0a0ecd |
}
|
|
|
0a0ecd |
|