Completely replaced the homebrew Pointf3 class with Eigen Vec3d.

Replaced the unscale macro with a template, implemented templates
for unscaling Eigen vectors.
This commit is contained in:
bubnikv 2018-08-21 17:43:05 +02:00
parent c5256bdd2c
commit cb138a20b8
46 changed files with 329 additions and 373 deletions

View file

@ -71,8 +71,8 @@ bool GeometryBuffer::set_from_triangles(const Polygons& triangles, float z, bool
if (generate_tex_coords)
m_tex_coords = std::vector<float>(t_size, 0.0f);
float min_x = (float)unscale(triangles[0].points[0](0));
float min_y = (float)unscale(triangles[0].points[0](1));
float min_x = unscale<float>(triangles[0].points[0](0));
float min_y = unscale<float>(triangles[0].points[0](1));
float max_x = min_x;
float max_y = min_y;
@ -83,8 +83,8 @@ bool GeometryBuffer::set_from_triangles(const Polygons& triangles, float z, bool
for (unsigned int v = 0; v < 3; ++v)
{
const Point& p = t.points[v];
float x = (float)unscale(p(0));
float y = (float)unscale(p(1));
float x = unscale<float>(p(0));
float y = unscale<float>(p(1));
m_vertices[v_coord++] = x;
m_vertices[v_coord++] = y;
@ -137,11 +137,11 @@ bool GeometryBuffer::set_from_lines(const Lines& lines, float z)
unsigned int coord = 0;
for (const Line& l : lines)
{
m_vertices[coord++] = (float)unscale(l.a(0));
m_vertices[coord++] = (float)unscale(l.a(1));
m_vertices[coord++] = unscale<float>(l.a(0));
m_vertices[coord++] = unscale<float>(l.a(1));
m_vertices[coord++] = z;
m_vertices[coord++] = (float)unscale(l.b(0));
m_vertices[coord++] = (float)unscale(l.b(1));
m_vertices[coord++] = unscale<float>(l.b(0));
m_vertices[coord++] = unscale<float>(l.b(1));
m_vertices[coord++] = z;
}
@ -375,7 +375,7 @@ void GLCanvas3D::Bed::_calc_bounding_box()
m_bounding_box = BoundingBoxf3();
for (const Pointf& p : m_shape)
{
m_bounding_box.merge(Pointf3(p(0), p(1), 0.0));
m_bounding_box.merge(Vec3d(p(0), p(1), 0.0));
}
}
@ -591,7 +591,7 @@ bool GLCanvas3D::Bed::_are_equal(const Pointfs& bed_1, const Pointfs& bed_2)
}
GLCanvas3D::Axes::Axes()
: length(0.0f)
: origin(0, 0, 0), length(0.0f)
{
}
@ -1040,7 +1040,7 @@ void GLCanvas3D::LayersEditing::_render_profile(const PrintObject& print_object,
// Make the vertical bar a bit wider so the layer height curve does not touch the edge of the bar region.
layer_height_max *= 1.12;
coordf_t max_z = unscale(print_object.size(2));
coordf_t max_z = unscale<double>(print_object.size(2));
double layer_height = dynamic_cast<const ConfigOptionFloat*>(print_object.config.option("layer_height"))->value;
float l = bar_rect.get_left();
float w = bar_rect.get_right() - l;
@ -1075,11 +1075,12 @@ void GLCanvas3D::LayersEditing::_render_profile(const PrintObject& print_object,
}
const Point GLCanvas3D::Mouse::Drag::Invalid_2D_Point(INT_MAX, INT_MAX);
const Pointf3 GLCanvas3D::Mouse::Drag::Invalid_3D_Point(DBL_MAX, DBL_MAX, DBL_MAX);
const Vec3d GLCanvas3D::Mouse::Drag::Invalid_3D_Point(DBL_MAX, DBL_MAX, DBL_MAX);
GLCanvas3D::Mouse::Drag::Drag()
: start_position_2D(Invalid_2D_Point)
, start_position_3D(Invalid_3D_Point)
, volume_center_offset(0, 0, 0)
, move_with_shift(false)
, move_volume_idx(-1)
, gizmo_volume_idx(-1)
@ -1950,7 +1951,7 @@ void GLCanvas3D::set_bed_shape(const Pointfs& shape)
bool new_shape = m_bed.set_shape(shape);
// Set the origin and size for painting of the coordinate system axes.
m_axes.origin = Pointf3(0.0, 0.0, (coordf_t)GROUND_Z);
m_axes.origin = Vec3d(0.0, 0.0, (coordf_t)GROUND_Z);
set_axes_length(0.3f * (float)m_bed.get_bounding_box().max_size());
if (new_shape)
@ -1970,7 +1971,7 @@ void GLCanvas3D::set_auto_bed_shape()
// draw a default square bed around object center
const BoundingBoxf3& bbox = volumes_bounding_box();
coordf_t max_size = bbox.max_size();
const Pointf3 center = bbox.center();
const Vec3d center = bbox.center();
Pointfs bed_shape;
bed_shape.reserve(4);
@ -1982,7 +1983,7 @@ void GLCanvas3D::set_auto_bed_shape()
set_bed_shape(bed_shape);
// Set the origin for painting of the coordinate system axes.
m_axes.origin = Pointf3(center(0), center(1), (coordf_t)GROUND_Z);
m_axes.origin = Vec3d(center(0), center(1), (coordf_t)GROUND_Z);
}
void GLCanvas3D::set_axes_length(float length)
@ -2799,7 +2800,7 @@ void GLCanvas3D::on_mouse(wxMouseEvent& evt)
{
// The mouse_to_3d gets the Z coordinate from the Z buffer at the screen coordinate pos x, y,
// an converts the screen space coordinate to unscaled object space.
Pointf3 pos3d = (volume_idx == -1) ? Pointf3(DBL_MAX, DBL_MAX, DBL_MAX) : _mouse_to_3d(pos);
Vec3d pos3d = (volume_idx == -1) ? Vec3d(DBL_MAX, DBL_MAX, DBL_MAX) : _mouse_to_3d(pos);
// Only accept the initial position, if it is inside the volume bounding box.
BoundingBoxf3 volume_bbox = m_volumes.volumes[volume_idx]->transformed_bounding_box();
@ -2831,7 +2832,7 @@ void GLCanvas3D::on_mouse(wxMouseEvent& evt)
// Get new position at the same Z of the initial click point.
float z0 = 0.0f;
float z1 = 1.0f;
Pointf3 cur_pos = Linef3(_mouse_to_3d(pos, &z0), _mouse_to_3d(pos, &z1)).intersect_plane(m_mouse.drag.start_position_3D(2));
Vec3d cur_pos = Linef3(_mouse_to_3d(pos, &z0), _mouse_to_3d(pos, &z1)).intersect_plane(m_mouse.drag.start_position_3D(2));
// Clip the new position, so the object center remains close to the bed.
cur_pos += m_mouse.drag.volume_center_offset;
@ -2839,13 +2840,13 @@ void GLCanvas3D::on_mouse(wxMouseEvent& evt)
if (!m_bed.contains(cur_pos2))
{
Point ip = m_bed.point_projection(cur_pos2);
cur_pos(0) = unscale(ip(0));
cur_pos(1) = unscale(ip(1));
cur_pos(0) = unscale<double>(ip(0));
cur_pos(1) = unscale<double>(ip(1));
}
cur_pos -= m_mouse.drag.volume_center_offset;
// Calculate the translation vector.
Vectorf3 vector = cur_pos - m_mouse.drag.start_position_3D;
Vec3d vector = cur_pos - m_mouse.drag.start_position_3D;
// Get the volume being dragged.
GLVolume* volume = m_volumes.volumes[m_mouse.drag.move_volume_idx];
// Get all volumes belonging to the same group, if any.
@ -2867,7 +2868,7 @@ void GLCanvas3D::on_mouse(wxMouseEvent& evt)
// Apply new temporary volume origin and ignore Z.
for (GLVolume* v : volumes)
v->set_origin(v->get_origin() + Vectorf3(vector(0), vector(1), 0.0));
v->set_origin(v->get_origin() + Vec3d(vector(0), vector(1), 0.0));
m_mouse.drag.start_position_3D = cur_pos;
m_gizmos.refresh();
@ -2878,7 +2879,7 @@ void GLCanvas3D::on_mouse(wxMouseEvent& evt)
{
m_mouse.dragging = true;
const Pointf3& cur_pos = _mouse_to_bed_3d(pos);
const Vec3d& cur_pos = _mouse_to_bed_3d(pos);
m_gizmos.update(Pointf(cur_pos(0), cur_pos(1)));
std::vector<GLVolume*> volumes;
@ -2931,7 +2932,7 @@ void GLCanvas3D::on_mouse(wxMouseEvent& evt)
{
bb.merge(volume->transformed_bounding_box());
}
const Pointf3& size = bb.size();
const Vec3d& size = bb.size();
m_on_update_geometry_info_callback.call(size(0), size(1), size(2), m_gizmos.get_scale());
}
@ -2954,7 +2955,7 @@ void GLCanvas3D::on_mouse(wxMouseEvent& evt)
// if dragging over blank area with left button, rotate
if (m_mouse.is_start_position_3D_defined())
{
const Pointf3& orig = m_mouse.drag.start_position_3D;
const Vec3d& orig = m_mouse.drag.start_position_3D;
m_camera.phi += (((float)pos(0) - (float)orig(0)) * TRACKBALLSIZE);
m_camera.set_theta(m_camera.get_theta() - ((float)pos(1) - (float)orig(1)) * TRACKBALLSIZE);
@ -2962,7 +2963,7 @@ void GLCanvas3D::on_mouse(wxMouseEvent& evt)
m_dirty = true;
}
m_mouse.drag.start_position_3D = Pointf3((coordf_t)pos(0), (coordf_t)pos(1), 0.0);
m_mouse.drag.start_position_3D = Vec3d((coordf_t)pos(0), (coordf_t)pos(1), 0.0);
}
else if (evt.MiddleIsDown() || evt.RightIsDown())
{
@ -2971,8 +2972,8 @@ void GLCanvas3D::on_mouse(wxMouseEvent& evt)
{
// get point in model space at Z = 0
float z = 0.0f;
const Pointf3& cur_pos = _mouse_to_3d(pos, &z);
Pointf3 orig = _mouse_to_3d(m_mouse.drag.start_position_2D, &z);
const Vec3d& cur_pos = _mouse_to_3d(pos, &z);
Vec3d orig = _mouse_to_3d(m_mouse.drag.start_position_2D, &z);
m_camera.target += orig - cur_pos;
m_on_viewport_changed_callback.call();
@ -3277,16 +3278,16 @@ float GLCanvas3D::_get_zoom_to_bounding_box_factor(const BoundingBoxf3& bbox) co
::glGetFloatv(GL_MODELVIEW_MATRIX, matrix);
// camera axes
Pointf3 right((coordf_t)matrix[0], (coordf_t)matrix[4], (coordf_t)matrix[8]);
Pointf3 up((coordf_t)matrix[1], (coordf_t)matrix[5], (coordf_t)matrix[9]);
Pointf3 forward((coordf_t)matrix[2], (coordf_t)matrix[6], (coordf_t)matrix[10]);
Vec3d right((coordf_t)matrix[0], (coordf_t)matrix[4], (coordf_t)matrix[8]);
Vec3d up((coordf_t)matrix[1], (coordf_t)matrix[5], (coordf_t)matrix[9]);
Vec3d forward((coordf_t)matrix[2], (coordf_t)matrix[6], (coordf_t)matrix[10]);
Pointf3 bb_min = bbox.min;
Pointf3 bb_max = bbox.max;
Pointf3 bb_center = bbox.center();
Vec3d bb_min = bbox.min;
Vec3d bb_max = bbox.max;
Vec3d bb_center = bbox.center();
// bbox vertices in world space
std::vector<Pointf3> vertices;
std::vector<Vec3d> vertices;
vertices.reserve(8);
vertices.push_back(bb_min);
vertices.emplace_back(bb_max(0), bb_min(1), bb_min(2));
@ -3303,11 +3304,11 @@ float GLCanvas3D::_get_zoom_to_bounding_box_factor(const BoundingBoxf3& bbox) co
// margin factor to give some empty space around the bbox
coordf_t margin_factor = 1.25;
for (const Pointf3 v : vertices)
for (const Vec3d v : vertices)
{
// project vertex on the plane perpendicular to camera forward axis
Pointf3 pos(v(0) - bb_center(0), v(1) - bb_center(1), v(2) - bb_center(2));
Pointf3 proj_on_plane = pos - pos.dot(forward) * forward;
Vec3d pos(v(0) - bb_center(0), v(1) - bb_center(1), v(2) - bb_center(2));
Vec3d proj_on_plane = pos - pos.dot(forward) * forward;
// calculates vertex coordinate along camera xy axes
coordf_t x_on_plane = proj_on_plane.dot(right);
@ -3389,7 +3390,7 @@ void GLCanvas3D::_camera_tranform() const
::glRotatef(-m_camera.get_theta(), 1.0f, 0.0f, 0.0f); // pitch
::glRotatef(m_camera.phi, 0.0f, 0.0f, 1.0f); // yaw
Pointf3 neg_target = - m_camera.target;
Vec3d neg_target = - m_camera.target;
::glTranslatef((GLfloat)neg_target(0), (GLfloat)neg_target(1), (GLfloat)neg_target(2));
}
@ -3730,7 +3731,7 @@ void GLCanvas3D::_perform_layer_editing_action(wxMouseEvent* evt)
{
const Rect& rect = LayersEditing::get_bar_rect_screen(*this);
float b = rect.get_bottom();
m_layers_editing.last_z = unscale(selected_obj->size(2)) * (b - evt->GetY() - 1.0f) / (b - rect.get_top());
m_layers_editing.last_z = unscale<double>(selected_obj->size(2)) * (b - evt->GetY() - 1.0f) / (b - rect.get_top());
m_layers_editing.last_action = evt->ShiftDown() ? (evt->RightIsDown() ? 3 : 2) : (evt->RightIsDown() ? 0 : 1);
}
@ -3760,10 +3761,10 @@ void GLCanvas3D::_perform_layer_editing_action(wxMouseEvent* evt)
_start_timer();
}
Pointf3 GLCanvas3D::_mouse_to_3d(const Point& mouse_pos, float* z)
Vec3d GLCanvas3D::_mouse_to_3d(const Point& mouse_pos, float* z)
{
if (m_canvas == nullptr)
return Pointf3(DBL_MAX, DBL_MAX, DBL_MAX);
return Vec3d(DBL_MAX, DBL_MAX, DBL_MAX);
_camera_tranform();
@ -3783,10 +3784,10 @@ Pointf3 GLCanvas3D::_mouse_to_3d(const Point& mouse_pos, float* z)
GLdouble out_x, out_y, out_z;
::gluUnProject((GLdouble)mouse_pos(0), (GLdouble)y, (GLdouble)mouse_z, modelview_matrix, projection_matrix, viewport, &out_x, &out_y, &out_z);
return Pointf3((coordf_t)out_x, (coordf_t)out_y, (coordf_t)out_z);
return Vec3d((coordf_t)out_x, (coordf_t)out_y, (coordf_t)out_z);
}
Pointf3 GLCanvas3D::_mouse_to_bed_3d(const Point& mouse_pos)
Vec3d GLCanvas3D::_mouse_to_bed_3d(const Point& mouse_pos)
{
float z0 = 0.0f;
float z1 = 1.0f;
@ -4501,7 +4502,7 @@ bool GLCanvas3D::_travel_paths_by_type(const GCodePreviewData& preview_data)
TypesList::iterator type = std::find(types.begin(), types.end(), Type(polyline.type));
if (type != types.end())
{
type->volume->print_zs.push_back(unscale(polyline.polyline.bounding_box().min(2)));
type->volume->print_zs.push_back(unscale<double>(polyline.polyline.bounding_box().min(2)));
type->volume->offsets.push_back(type->volume->indexed_vertex_array.quad_indices.size());
type->volume->offsets.push_back(type->volume->indexed_vertex_array.triangle_indices.size());
@ -4567,7 +4568,7 @@ bool GLCanvas3D::_travel_paths_by_feedrate(const GCodePreviewData& preview_data)
FeedratesList::iterator feedrate = std::find(feedrates.begin(), feedrates.end(), Feedrate(polyline.feedrate));
if (feedrate != feedrates.end())
{
feedrate->volume->print_zs.push_back(unscale(polyline.polyline.bounding_box().min(2)));
feedrate->volume->print_zs.push_back(unscale<double>(polyline.polyline.bounding_box().min(2)));
feedrate->volume->offsets.push_back(feedrate->volume->indexed_vertex_array.quad_indices.size());
feedrate->volume->offsets.push_back(feedrate->volume->indexed_vertex_array.triangle_indices.size());
@ -4633,7 +4634,7 @@ bool GLCanvas3D::_travel_paths_by_tool(const GCodePreviewData& preview_data, con
ToolsList::iterator tool = std::find(tools.begin(), tools.end(), Tool(polyline.extruder_id));
if (tool != tools.end())
{
tool->volume->print_zs.push_back(unscale(polyline.polyline.bounding_box().min(2)));
tool->volume->print_zs.push_back(unscale<double>(polyline.polyline.bounding_box().min(2)));
tool->volume->offsets.push_back(tool->volume->indexed_vertex_array.quad_indices.size());
tool->volume->offsets.push_back(tool->volume->indexed_vertex_array.triangle_indices.size());
@ -4662,7 +4663,7 @@ void GLCanvas3D::_load_gcode_retractions(const GCodePreviewData& preview_data)
for (const GCodePreviewData::Retraction::Position& position : copy)
{
volume->print_zs.push_back(unscale(position.position(2)));
volume->print_zs.push_back(unscale<double>(position.position(2)));
volume->offsets.push_back(volume->indexed_vertex_array.quad_indices.size());
volume->offsets.push_back(volume->indexed_vertex_array.triangle_indices.size());
@ -4693,7 +4694,7 @@ void GLCanvas3D::_load_gcode_unretractions(const GCodePreviewData& preview_data)
for (const GCodePreviewData::Retraction::Position& position : copy)
{
volume->print_zs.push_back(unscale(position.position(2)));
volume->print_zs.push_back(unscale<double>(position.position(2)));
volume->offsets.push_back(volume->indexed_vertex_array.quad_indices.size());
volume->offsets.push_back(volume->indexed_vertex_array.triangle_indices.size());
@ -4816,7 +4817,7 @@ void GLCanvas3D::_update_toolpath_volumes_outside_state()
if (opt != nullptr)
{
BoundingBox bed_box_2D = get_extents(Polygon::new_scale(opt->values));
print_volume = BoundingBoxf3(Pointf3(unscale(bed_box_2D.min(0)) - tolerance_x, unscale(bed_box_2D.min(1)) - tolerance_y, 0.0), Pointf3(unscale(bed_box_2D.max(0)) + tolerance_x, unscale(bed_box_2D.max(1)) + tolerance_y, m_config->opt_float("max_print_height")));
print_volume = BoundingBoxf3(Vec3d(unscale<double>(bed_box_2D.min(0)) - tolerance_x, unscale<double>(bed_box_2D.min(1)) - tolerance_y, 0.0), Vec3d(unscale<double>(bed_box_2D.max(0)) + tolerance_x, unscale<double>(bed_box_2D.max(1)) + tolerance_y, m_config->opt_float("max_print_height")));
// Allow the objects to protrude below the print bed
print_volume.min(2) = -1e10;
}
@ -4849,7 +4850,7 @@ void GLCanvas3D::_on_move(const std::vector<int>& volume_idxs)
std::set<std::string> done; // prevent moving instances twice
bool object_moved = false;
Pointf3 wipe_tower_origin(0.0, 0.0, 0.0);
Vec3d wipe_tower_origin(0.0, 0.0, 0.0);
for (int volume_idx : volume_idxs)
{
GLVolume* volume = m_volumes.volumes[volume_idx];
@ -4868,7 +4869,7 @@ void GLCanvas3D::_on_move(const std::vector<int>& volume_idxs)
{
// Move a regular object.
ModelObject* model_object = m_model->objects[obj_idx];
const Pointf3& origin = volume->get_origin();
const Vec3d& origin = volume->get_origin();
model_object->instances[instance_idx]->offset = Pointf(origin(0), origin(1));
model_object->invalidate_bounding_box();
object_moved = true;
@ -4881,7 +4882,7 @@ void GLCanvas3D::_on_move(const std::vector<int>& volume_idxs)
if (object_moved)
m_on_instance_moved_callback.call();
if (wipe_tower_origin != Pointf3(0.0, 0.0, 0.0))
if (wipe_tower_origin != Vec3d(0.0, 0.0, 0.0))
m_on_wipe_tower_moved_callback.call(wipe_tower_origin(0), wipe_tower_origin(1));
}