From e595dbc88ed2eddcbf759f84d09502b82802a5b8 Mon Sep 17 00:00:00 2001 From: kai Date: Tue, 9 Oct 2007 07:27:45 +0000 Subject: [PATCH] pathfinding change: the engine uses Triangulation and A* on triangles now. dcdt package added. premake.lua changed to include the dcdt code. it needs to run update-workspaces.bat (flag -showOverlay will draw the triangulation and a single unit paths) This was SVN commit r5393. --- build/premake/premake.lua | 9 +- source/dcdt/se/Abstract.cpp | 722 ++++++ source/dcdt/se/Abstraction.h | 157 ++ source/dcdt/se/Experiments.h | 100 + source/dcdt/se/FunnelDeque.cpp | 557 +++++ source/dcdt/se/FunnelDeque.h | 107 + source/dcdt/se/Location.cpp | 159 ++ source/dcdt/se/Search.cpp | 2328 +++++++++++++++++++ source/dcdt/se/Search.cpp.bak | 1545 ++++++++++++ source/dcdt/se/SearchNode.h | 135 ++ source/dcdt/se/Utility.cpp | 248 ++ source/dcdt/se/Width.cpp | 149 ++ source/dcdt/se/data/ecol.se | 163 ++ source/dcdt/se/linux/makefile | 24 + source/dcdt/se/linux/makefile.se | 14 + source/dcdt/se/linux/makefile.semesh | 14 + source/dcdt/se/linux/makefile.setut | 14 + source/dcdt/se/readme.txt | 70 + source/dcdt/se/se.cpp | 31 + source/dcdt/se/se.h | 199 ++ source/dcdt/se/se_dcdt.cpp | 709 ++++++ source/dcdt/se/se_dcdt.h | 493 ++++ source/dcdt/se/se_dcdt_mesh.cpp | 148 ++ source/dcdt/se/se_mesh.cpp | 237 ++ source/dcdt/se/se_mesh.h | 419 ++++ source/dcdt/se/se_mesh_import.cpp | 535 +++++ source/dcdt/se/se_mesh_import.h | 81 + source/dcdt/se/se_mesh_io.cpp | 219 ++ source/dcdt/se/se_mesh_operators.cpp | 571 +++++ source/dcdt/se/se_triangulator.cpp | 1446 ++++++++++++ source/dcdt/se/se_triangulator.h | 366 +++ source/dcdt/se/sr.cpp | 219 ++ source/dcdt/se/sr.h | 367 +++ source/dcdt/se/sr_alg.cpp | 224 ++ source/dcdt/se/sr_alg.h | 34 + source/dcdt/se/sr_array.cpp | 189 ++ source/dcdt/se/sr_array.h | 341 +++ source/dcdt/se/sr_array_pt.cpp | 202 ++ source/dcdt/se/sr_array_pt.h | 169 ++ source/dcdt/se/sr_box.cpp | 193 ++ source/dcdt/se/sr_box.h | 132 ++ source/dcdt/se/sr_buffer.cpp | 63 + source/dcdt/se/sr_buffer.h | 170 ++ source/dcdt/se/sr_bv.cpp | 1176 ++++++++++ source/dcdt/se/sr_bv_coldet.cpp | 230 ++ source/dcdt/se/sr_bv_coldet.h | 95 + source/dcdt/se/sr_bv_id_pairs.cpp | 216 ++ source/dcdt/se/sr_bv_id_pairs.h | 55 + source/dcdt/se/sr_bv_math.cpp | 902 +++++++ source/dcdt/se/sr_bv_math.h | 72 + source/dcdt/se/sr_bv_nbody.cpp | 471 ++++ source/dcdt/se/sr_bv_nbody.h | 65 + source/dcdt/se/sr_bv_tree.cpp | 331 +++ source/dcdt/se/sr_bv_tree.h | 87 + source/dcdt/se/sr_bv_tree_query.cpp | 839 +++++++ source/dcdt/se/sr_bv_tree_query.h | 173 ++ source/dcdt/se/sr_camera.cpp | 202 ++ source/dcdt/se/sr_camera.h | 108 + source/dcdt/se/sr_cfg_manager.cpp | 34 + source/dcdt/se/sr_cfg_manager.h | 148 ++ source/dcdt/se/sr_cfg_path.cpp | 442 ++++ source/dcdt/se/sr_cfg_path.h | 148 ++ source/dcdt/se/sr_cfg_planner.cpp | 128 + source/dcdt/se/sr_cfg_planner.h | 88 + source/dcdt/se/sr_cfg_planner_lazy.cpp | 137 ++ source/dcdt/se/sr_cfg_tree.cpp | 558 +++++ source/dcdt/se/sr_cfg_tree.h | 169 ++ source/dcdt/se/sr_class_manager.h | 59 + source/dcdt/se/sr_color.cpp | 108 + source/dcdt/se/sr_color.h | 99 + source/dcdt/se/sr_cylinder.cpp | 55 + source/dcdt/se/sr_cylinder.h | 46 + source/dcdt/se/sr_deque.h | 55 + source/dcdt/se/sr_euler.cpp | 188 ++ source/dcdt/se/sr_euler.h | 41 + source/dcdt/se/sr_event.cpp | 62 + source/dcdt/se/sr_event.h | 95 + source/dcdt/se/sr_exp_table.cpp | 511 ++++ source/dcdt/se/sr_exp_table.h | 134 ++ source/dcdt/se/sr_fl.h | 54 + source/dcdt/se/sr_geo2.cpp | 262 +++ source/dcdt/se/sr_geo2.h | 90 + source/dcdt/se/sr_gl.h | 73 + source/dcdt/se/sr_gl_render_funcs.h | 48 + source/dcdt/se/sr_graph.cpp | 663 ++++++ source/dcdt/se/sr_graph.h | 320 +++ source/dcdt/se/sr_grid.cpp | 249 ++ source/dcdt/se/sr_grid.h | 167 ++ source/dcdt/se/sr_hash_table.cpp | 242 ++ source/dcdt/se/sr_hash_table.h | 155 ++ source/dcdt/se/sr_heap.h | 119 + source/dcdt/se/sr_image.cpp | 116 + source/dcdt/se/sr_image.h | 67 + source/dcdt/se/sr_input.cpp | 606 +++++ source/dcdt/se/sr_input.h | 369 +++ source/dcdt/se/sr_light.cpp | 29 + source/dcdt/se/sr_light.h | 82 + source/dcdt/se/sr_line.cpp | 177 ++ source/dcdt/se/sr_line.h | 95 + source/dcdt/se/sr_lines.cpp | 265 +++ source/dcdt/se/sr_lines.h | 122 + source/dcdt/se/sr_list.cpp | 292 +++ source/dcdt/se/sr_list.h | 329 +++ source/dcdt/se/sr_list_node.cpp | 85 + source/dcdt/se/sr_list_node.h | 82 + source/dcdt/se/sr_mat.cpp | 591 +++++ source/dcdt/se/sr_mat.h | 316 +++ source/dcdt/se/sr_material.cpp | 61 + source/dcdt/se/sr_material.h | 49 + source/dcdt/se/sr_matn.cpp | 606 +++++ source/dcdt/se/sr_matn.h | 148 ++ source/dcdt/se/sr_matrix_window.h | 54 + source/dcdt/se/sr_mem_control.cpp | 131 ++ source/dcdt/se/sr_mem_control.h | 139 ++ source/dcdt/se/sr_model.cpp | 1039 +++++++++ source/dcdt/se/sr_model.h | 192 ++ source/dcdt/se/sr_model_export_iv.cpp | 85 + source/dcdt/se/sr_model_import_obj.cpp | 214 ++ source/dcdt/se/sr_output.cpp | 344 +++ source/dcdt/se/sr_output.h | 206 ++ source/dcdt/se/sr_output_window.h | 90 + source/dcdt/se/sr_path_array.cpp | 28 + source/dcdt/se/sr_path_array.h | 57 + source/dcdt/se/sr_plane.cpp | 106 + source/dcdt/se/sr_plane.h | 45 + source/dcdt/se/sr_points.cpp | 104 + source/dcdt/se/sr_points.h | 69 + source/dcdt/se/sr_polygon.cpp | 628 +++++ source/dcdt/se/sr_polygon.h | 156 ++ source/dcdt/se/sr_polygons.cpp | 208 ++ source/dcdt/se/sr_polygons.h | 134 ++ source/dcdt/se/sr_quat.cpp | 335 +++ source/dcdt/se/sr_quat.h | 148 ++ source/dcdt/se/sr_queue.h | 97 + source/dcdt/se/sr_random.cpp | 174 ++ source/dcdt/se/sr_random.h | 77 + source/dcdt/se/sr_sa.cpp | 151 ++ source/dcdt/se/sr_sa.h | 88 + source/dcdt/se/sr_sa_bbox.cpp | 32 + source/dcdt/se/sr_sa_bbox.h | 31 + source/dcdt/se/sr_sa_eps_export.cpp | 379 +++ source/dcdt/se/sr_sa_eps_export.h | 71 + source/dcdt/se/sr_sa_event.cpp | 41 + source/dcdt/se/sr_sa_event.h | 33 + source/dcdt/se/sr_sa_gl_render.h | 67 + source/dcdt/se/sr_sa_model_export.cpp | 65 + source/dcdt/se/sr_sa_model_export.h | 42 + source/dcdt/se/sr_sa_render_mode.cpp | 23 + source/dcdt/se/sr_sa_render_mode.h | 46 + source/dcdt/se/sr_set.cpp | 210 ++ source/dcdt/se/sr_set.h | 186 ++ source/dcdt/se/sr_shared_class.h | 37 + source/dcdt/se/sr_sn.cpp | 37 + source/dcdt/se/sr_sn.h | 80 + source/dcdt/se/sr_sn_editor.cpp | 45 + source/dcdt/se/sr_sn_editor.h | 62 + source/dcdt/se/sr_sn_group.cpp | 108 + source/dcdt/se/sr_sn_group.h | 97 + source/dcdt/se/sr_sn_manipulator.cpp | 233 ++ source/dcdt/se/sr_sn_manipulator.h | 109 + source/dcdt/se/sr_sn_matrix.cpp | 31 + source/dcdt/se/sr_sn_matrix.h | 48 + source/dcdt/se/sr_sn_polygon_editor.cpp | 469 ++++ source/dcdt/se/sr_sn_polygon_editor.h | 102 + source/dcdt/se/sr_sn_shape.cpp | 93 + source/dcdt/se/sr_sn_shape.h | 211 ++ source/dcdt/se/sr_sphere.cpp | 35 + source/dcdt/se/sr_sphere.h | 46 + source/dcdt/se/sr_spline.cpp | 23 + source/dcdt/se/sr_spline.h | 67 + source/dcdt/se/sr_string.cpp | 663 ++++++ source/dcdt/se/sr_string.h | 350 +++ source/dcdt/se/sr_string_array.cpp | 245 ++ source/dcdt/se/sr_string_array.h | 133 ++ source/dcdt/se/sr_swing_control.h | 60 + source/dcdt/se/sr_time.cpp | 98 + source/dcdt/se/sr_time.h | 73 + source/dcdt/se/sr_trace.h | 95 + source/dcdt/se/sr_trackball.cpp | 86 + source/dcdt/se/sr_trackball.h | 59 + source/dcdt/se/sr_tree.cpp | 459 ++++ source/dcdt/se/sr_tree.h | 289 +++ source/dcdt/se/sr_triangle.cpp | 93 + source/dcdt/se/sr_triangle.h | 53 + source/dcdt/se/sr_triangulation.cpp | 379 +++ source/dcdt/se/sr_triangulation.h | 170 ++ source/dcdt/se/sr_var.cpp | 505 ++++ source/dcdt/se/sr_var.h | 145 ++ source/dcdt/se/sr_var_table.cpp | 182 ++ source/dcdt/se/sr_var_table.h | 116 + source/dcdt/se/sr_var_table_window.h | 52 + source/dcdt/se/sr_vec.cpp | 203 ++ source/dcdt/se/sr_vec.h | 259 +++ source/dcdt/se/sr_vec2.cpp | 359 +++ source/dcdt/se/sr_vec2.h | 260 +++ source/dcdt/se/sr_viewer.h | 197 ++ source/dcdt/se/terms.html | 31 + source/pch/se/precompiled.cpp | 1 + source/pch/se/precompiled.h | 7 + source/ps/GameSetup/Config.cpp | 7 + source/ps/GameSetup/Config.h | 2 + source/renderer/TerrainOverlay.cpp | 133 +- source/renderer/TerrainOverlay.h | 260 +++ source/simulation/AStarEngine.cpp | 87 +- source/simulation/AStarEngine.h | 16 + source/simulation/EntityStateProcessing.cpp | 5 +- source/simulation/PathfindEngine.cpp | 307 ++- source/simulation/PathfindEngine.h | 25 +- source/simulation/TRAStarEngine.cpp | 19 + source/simulation/TRAStarEngine.h | 19 + 210 files changed, 44266 insertions(+), 22 deletions(-) create mode 100644 source/dcdt/se/Abstract.cpp create mode 100644 source/dcdt/se/Abstraction.h create mode 100644 source/dcdt/se/Experiments.h create mode 100644 source/dcdt/se/FunnelDeque.cpp create mode 100644 source/dcdt/se/FunnelDeque.h create mode 100644 source/dcdt/se/Location.cpp create mode 100644 source/dcdt/se/Search.cpp create mode 100644 source/dcdt/se/Search.cpp.bak create mode 100644 source/dcdt/se/SearchNode.h create mode 100644 source/dcdt/se/Utility.cpp create mode 100644 source/dcdt/se/Width.cpp create mode 100644 source/dcdt/se/data/ecol.se create mode 100644 source/dcdt/se/linux/makefile create mode 100644 source/dcdt/se/linux/makefile.se create mode 100644 source/dcdt/se/linux/makefile.semesh create mode 100644 source/dcdt/se/linux/makefile.setut create mode 100644 source/dcdt/se/readme.txt create mode 100644 source/dcdt/se/se.cpp create mode 100644 source/dcdt/se/se.h create mode 100644 source/dcdt/se/se_dcdt.cpp create mode 100644 source/dcdt/se/se_dcdt.h create mode 100644 source/dcdt/se/se_dcdt_mesh.cpp create mode 100644 source/dcdt/se/se_mesh.cpp create mode 100644 source/dcdt/se/se_mesh.h create mode 100644 source/dcdt/se/se_mesh_import.cpp create mode 100644 source/dcdt/se/se_mesh_import.h create mode 100644 source/dcdt/se/se_mesh_io.cpp create mode 100644 source/dcdt/se/se_mesh_operators.cpp create mode 100644 source/dcdt/se/se_triangulator.cpp create mode 100644 source/dcdt/se/se_triangulator.h create mode 100644 source/dcdt/se/sr.cpp create mode 100644 source/dcdt/se/sr.h create mode 100644 source/dcdt/se/sr_alg.cpp create mode 100644 source/dcdt/se/sr_alg.h create mode 100644 source/dcdt/se/sr_array.cpp create mode 100644 source/dcdt/se/sr_array.h create mode 100644 source/dcdt/se/sr_array_pt.cpp create mode 100644 source/dcdt/se/sr_array_pt.h create mode 100644 source/dcdt/se/sr_box.cpp create mode 100644 source/dcdt/se/sr_box.h create mode 100644 source/dcdt/se/sr_buffer.cpp create mode 100644 source/dcdt/se/sr_buffer.h create mode 100644 source/dcdt/se/sr_bv.cpp create mode 100644 source/dcdt/se/sr_bv_coldet.cpp create mode 100644 source/dcdt/se/sr_bv_coldet.h create mode 100644 source/dcdt/se/sr_bv_id_pairs.cpp create mode 100644 source/dcdt/se/sr_bv_id_pairs.h create mode 100644 source/dcdt/se/sr_bv_math.cpp create mode 100644 source/dcdt/se/sr_bv_math.h create mode 100644 source/dcdt/se/sr_bv_nbody.cpp create mode 100644 source/dcdt/se/sr_bv_nbody.h create mode 100644 source/dcdt/se/sr_bv_tree.cpp create mode 100644 source/dcdt/se/sr_bv_tree.h create mode 100644 source/dcdt/se/sr_bv_tree_query.cpp create mode 100644 source/dcdt/se/sr_bv_tree_query.h create mode 100644 source/dcdt/se/sr_camera.cpp create mode 100644 source/dcdt/se/sr_camera.h create mode 100644 source/dcdt/se/sr_cfg_manager.cpp create mode 100644 source/dcdt/se/sr_cfg_manager.h create mode 100644 source/dcdt/se/sr_cfg_path.cpp create mode 100644 source/dcdt/se/sr_cfg_path.h create mode 100644 source/dcdt/se/sr_cfg_planner.cpp create mode 100644 source/dcdt/se/sr_cfg_planner.h create mode 100644 source/dcdt/se/sr_cfg_planner_lazy.cpp create mode 100644 source/dcdt/se/sr_cfg_tree.cpp create mode 100644 source/dcdt/se/sr_cfg_tree.h create mode 100644 source/dcdt/se/sr_class_manager.h create mode 100644 source/dcdt/se/sr_color.cpp create mode 100644 source/dcdt/se/sr_color.h create mode 100644 source/dcdt/se/sr_cylinder.cpp create mode 100644 source/dcdt/se/sr_cylinder.h create mode 100644 source/dcdt/se/sr_deque.h create mode 100644 source/dcdt/se/sr_euler.cpp create mode 100644 source/dcdt/se/sr_euler.h create mode 100644 source/dcdt/se/sr_event.cpp create mode 100644 source/dcdt/se/sr_event.h create mode 100644 source/dcdt/se/sr_exp_table.cpp create mode 100644 source/dcdt/se/sr_exp_table.h create mode 100644 source/dcdt/se/sr_fl.h create mode 100644 source/dcdt/se/sr_geo2.cpp create mode 100644 source/dcdt/se/sr_geo2.h create mode 100644 source/dcdt/se/sr_gl.h create mode 100644 source/dcdt/se/sr_gl_render_funcs.h create mode 100644 source/dcdt/se/sr_graph.cpp create mode 100644 source/dcdt/se/sr_graph.h create mode 100644 source/dcdt/se/sr_grid.cpp create mode 100644 source/dcdt/se/sr_grid.h create mode 100644 source/dcdt/se/sr_hash_table.cpp create mode 100644 source/dcdt/se/sr_hash_table.h create mode 100644 source/dcdt/se/sr_heap.h create mode 100644 source/dcdt/se/sr_image.cpp create mode 100644 source/dcdt/se/sr_image.h create mode 100644 source/dcdt/se/sr_input.cpp create mode 100644 source/dcdt/se/sr_input.h create mode 100644 source/dcdt/se/sr_light.cpp create mode 100644 source/dcdt/se/sr_light.h create mode 100644 source/dcdt/se/sr_line.cpp create mode 100644 source/dcdt/se/sr_line.h create mode 100644 source/dcdt/se/sr_lines.cpp create mode 100644 source/dcdt/se/sr_lines.h create mode 100644 source/dcdt/se/sr_list.cpp create mode 100644 source/dcdt/se/sr_list.h create mode 100644 source/dcdt/se/sr_list_node.cpp create mode 100644 source/dcdt/se/sr_list_node.h create mode 100644 source/dcdt/se/sr_mat.cpp create mode 100644 source/dcdt/se/sr_mat.h create mode 100644 source/dcdt/se/sr_material.cpp create mode 100644 source/dcdt/se/sr_material.h create mode 100644 source/dcdt/se/sr_matn.cpp create mode 100644 source/dcdt/se/sr_matn.h create mode 100644 source/dcdt/se/sr_matrix_window.h create mode 100644 source/dcdt/se/sr_mem_control.cpp create mode 100644 source/dcdt/se/sr_mem_control.h create mode 100644 source/dcdt/se/sr_model.cpp create mode 100644 source/dcdt/se/sr_model.h create mode 100644 source/dcdt/se/sr_model_export_iv.cpp create mode 100644 source/dcdt/se/sr_model_import_obj.cpp create mode 100644 source/dcdt/se/sr_output.cpp create mode 100644 source/dcdt/se/sr_output.h create mode 100644 source/dcdt/se/sr_output_window.h create mode 100644 source/dcdt/se/sr_path_array.cpp create mode 100644 source/dcdt/se/sr_path_array.h create mode 100644 source/dcdt/se/sr_plane.cpp create mode 100644 source/dcdt/se/sr_plane.h create mode 100644 source/dcdt/se/sr_points.cpp create mode 100644 source/dcdt/se/sr_points.h create mode 100644 source/dcdt/se/sr_polygon.cpp create mode 100644 source/dcdt/se/sr_polygon.h create mode 100644 source/dcdt/se/sr_polygons.cpp create mode 100644 source/dcdt/se/sr_polygons.h create mode 100644 source/dcdt/se/sr_quat.cpp create mode 100644 source/dcdt/se/sr_quat.h create mode 100644 source/dcdt/se/sr_queue.h create mode 100644 source/dcdt/se/sr_random.cpp create mode 100644 source/dcdt/se/sr_random.h create mode 100644 source/dcdt/se/sr_sa.cpp create mode 100644 source/dcdt/se/sr_sa.h create mode 100644 source/dcdt/se/sr_sa_bbox.cpp create mode 100644 source/dcdt/se/sr_sa_bbox.h create mode 100644 source/dcdt/se/sr_sa_eps_export.cpp create mode 100644 source/dcdt/se/sr_sa_eps_export.h create mode 100644 source/dcdt/se/sr_sa_event.cpp create mode 100644 source/dcdt/se/sr_sa_event.h create mode 100644 source/dcdt/se/sr_sa_gl_render.h create mode 100644 source/dcdt/se/sr_sa_model_export.cpp create mode 100644 source/dcdt/se/sr_sa_model_export.h create mode 100644 source/dcdt/se/sr_sa_render_mode.cpp create mode 100644 source/dcdt/se/sr_sa_render_mode.h create mode 100644 source/dcdt/se/sr_set.cpp create mode 100644 source/dcdt/se/sr_set.h create mode 100644 source/dcdt/se/sr_shared_class.h create mode 100644 source/dcdt/se/sr_sn.cpp create mode 100644 source/dcdt/se/sr_sn.h create mode 100644 source/dcdt/se/sr_sn_editor.cpp create mode 100644 source/dcdt/se/sr_sn_editor.h create mode 100644 source/dcdt/se/sr_sn_group.cpp create mode 100644 source/dcdt/se/sr_sn_group.h create mode 100644 source/dcdt/se/sr_sn_manipulator.cpp create mode 100644 source/dcdt/se/sr_sn_manipulator.h create mode 100644 source/dcdt/se/sr_sn_matrix.cpp create mode 100644 source/dcdt/se/sr_sn_matrix.h create mode 100644 source/dcdt/se/sr_sn_polygon_editor.cpp create mode 100644 source/dcdt/se/sr_sn_polygon_editor.h create mode 100644 source/dcdt/se/sr_sn_shape.cpp create mode 100644 source/dcdt/se/sr_sn_shape.h create mode 100644 source/dcdt/se/sr_sphere.cpp create mode 100644 source/dcdt/se/sr_sphere.h create mode 100644 source/dcdt/se/sr_spline.cpp create mode 100644 source/dcdt/se/sr_spline.h create mode 100644 source/dcdt/se/sr_string.cpp create mode 100644 source/dcdt/se/sr_string.h create mode 100644 source/dcdt/se/sr_string_array.cpp create mode 100644 source/dcdt/se/sr_string_array.h create mode 100644 source/dcdt/se/sr_swing_control.h create mode 100644 source/dcdt/se/sr_time.cpp create mode 100644 source/dcdt/se/sr_time.h create mode 100644 source/dcdt/se/sr_trace.h create mode 100644 source/dcdt/se/sr_trackball.cpp create mode 100644 source/dcdt/se/sr_trackball.h create mode 100644 source/dcdt/se/sr_tree.cpp create mode 100644 source/dcdt/se/sr_tree.h create mode 100644 source/dcdt/se/sr_triangle.cpp create mode 100644 source/dcdt/se/sr_triangle.h create mode 100644 source/dcdt/se/sr_triangulation.cpp create mode 100644 source/dcdt/se/sr_triangulation.h create mode 100644 source/dcdt/se/sr_var.cpp create mode 100644 source/dcdt/se/sr_var.h create mode 100644 source/dcdt/se/sr_var_table.cpp create mode 100644 source/dcdt/se/sr_var_table.h create mode 100644 source/dcdt/se/sr_var_table_window.h create mode 100644 source/dcdt/se/sr_vec.cpp create mode 100644 source/dcdt/se/sr_vec.h create mode 100644 source/dcdt/se/sr_vec2.cpp create mode 100644 source/dcdt/se/sr_vec2.h create mode 100644 source/dcdt/se/sr_viewer.h create mode 100644 source/dcdt/se/terms.html create mode 100644 source/pch/se/precompiled.cpp create mode 100644 source/pch/se/precompiled.h create mode 100644 source/simulation/TRAStarEngine.cpp create mode 100644 source/simulation/TRAStarEngine.h diff --git a/build/premake/premake.lua b/build/premake/premake.lua index 1028583fa0..109eb7302e 100755 --- a/build/premake/premake.lua +++ b/build/premake/premake.lua @@ -304,7 +304,7 @@ function setup_all_libs () -- names of external libraries used (see libraries_dir comment) local extern_libs = {} - + source_dirs = { "network", } @@ -326,7 +326,8 @@ function setup_all_libs () "sound", "scripting", "maths", - "maths/scripting" + "maths/scripting", + "dcdt/se" } extern_libs = { "spidermonkey", @@ -343,12 +344,14 @@ function setup_all_libs () "graphics", "graphics/scripting", "renderer" + } extern_libs = { "opengl", "sdl", -- key definitions "spidermonkey", -- for graphics/scripting "boost" + } setup_static_lib_package("graphics", source_dirs, extern_libs, {}) @@ -369,12 +372,14 @@ function setup_all_libs () source_dirs = { "tools/atlas/GameInterface", "tools/atlas/GameInterface/Handlers" + } extern_libs = { "boost", "sdl", -- key definitions "opengl", "spidermonkey" + } setup_static_lib_package("atlas", source_dirs, extern_libs, {}) diff --git a/source/dcdt/se/Abstract.cpp b/source/dcdt/se/Abstract.cpp new file mode 100644 index 0000000000..953e92720a --- /dev/null +++ b/source/dcdt/se/Abstract.cpp @@ -0,0 +1,722 @@ +//Abstract.cpp + +//DJD: Abstract function definitions { +#include "precompiled.h" + +#include +#include + +#include "se_dcdt.h" + +//the degree for an unabstracted triangle +#define UNABSTRACTED -1 + +template +SrArray *> SeLinkFace::processing; + +void SeDcdt::Abstract() +{ +#if defined EXPERIMENT + //count the number of degree-3 nodes in the abstract graph + int num = 0; +#endif +//Degree-1 and Degree-0 faces + //keep track of the connected component + int component = 0; + //possible degree-1 triangles + SrArray degree1; + //possible degree-2 or 3 triangles + SrArray processing; + //get first face and record it + SeDcdtFace *currentFace = _mesh->first()->fac(); + SeDcdtFace *firstFace = currentFace; + if (outside == NULL) + { + //first determine the "outside" face +// for (int n = 0; n < SeDcdtFace::Faces(); n++) + do + { +// SeDcdtFace *currentFace = SeDcdtFace::Face(n); + if (currentFace == NULL) + { + continue; + } + float x1, y1, x2, y2, x3, y3; + TriangleVertices(currentFace, x1, y1, x2, y2, x3, y3); + //outside face is any whose vertices aren't in counterclockwise order + if (Orientation(x1, y1, x2, y2, x3, y3) <= 0.0f) + { + //record this face, reset, and break + outside = currentFace; + currentFace = firstFace; + break; + } + //if it wasn't found, try the next face + currentFace = currentFace->nxt(); + } + while (currentFace != firstFace); + } + //go through all faces in the triangulation +// for (int n = 0; n < SeDcdtFace::Faces(); n++) + do + { +// SeDcdtFace *currentFace = SeDcdtFace::Face(n); + //don't process the outside face + if ((currentFace == NULL) || (currentFace == outside)) + { + currentFace = currentFace->nxt(); + continue; + } + //keep track of how many edges are constrained + int numConstrained = 0; + //this is the adjacent face to a degree-1 triangle + SeDcdtFace *tempFace = NULL; + //access the parts of the face + SeBase *s = currentFace->se(); + //count the number of constrained edges around this face + for (int i = 0; i < 3; i++) + { + //consider a constrained edge or one bordering the outside face to be blocked + if (Blocked(s)) + { + numConstrained++; + } + //if there's an unconstrained edge, record the face across it + else + { + tempFace = (SeDcdtFace *)(s->sym()->fac()); + } + //go to the next element of the triangle + s = s->nxt(); + } + //if there are two constrained edges, this is a degree-1 triangle + if (numConstrained == 2) + { + //abstract the face and calculate its widths + currentFace->link = new Abstraction(1); + CalculateWidths(currentFace); + //if the face across the unconstrained edge is already degree-1, + if (Degree(tempFace) == 1) + { + //this is a tree component + TreeAbstract(tempFace, component++); + } + //if it's not abstracted, it might now be a degree-1 node + else if (Degree(tempFace) == UNABSTRACTED) + { + degree1.push() = tempFace; + } + } + //if all edges are constrained, this is a degree-0 triangle + else if (numConstrained == 3) + { + //abstract it and set its component + currentFace->link = new Abstraction(0); + currentFace->link->Component(component++); + } + //if it's not degree-0 or 1, it might be degree-2 or 3 + else + { + //enqueue it for later processing + processing.push() = currentFace; + } + //move to the next face in the mesh + currentFace = currentFace->nxt(); + } + //continue until reaching the first face again + while (currentFace != firstFace); + //keep track of which faces we have considered for degree-1 nodes + _mesh->begin_marking(); + //go through the possible degree-1 nodes + while (!degree1.empty()) + { + //get the next possible degree-1 node + SeDcdtFace *currentFace = degree1.pop(); + //if this face has already been dealt with, skip it + if ((_mesh->marked(currentFace)) || (Degree(currentFace) != UNABSTRACTED)) + { + continue; + } + //keep track of the number of adjacent triangles + int numAdjacent = 0; + //the face across an unconstrained edge + SeDcdtFace *tempFace = NULL; + //access the elements of the triangle + SeBase *s = currentFace->se(); + //calculate the number of unabstracted faces across unconstrained edges + for (int i = 0; i < 3; i++) + { + //only consider adjacent, unabstracted edges + if (!Blocked(s) && (Degree(s->sym()) == UNABSTRACTED)) + { + numAdjacent++; + tempFace = (SeDcdtFace *)(s->sym()->fac()); + } + //move to the next edge of the triangle + s = s->nxt(); + } + //if there is only one, this is a degree-1 node + if (numAdjacent == 1) + { + //abstract this node and calculate its widths + currentFace->link = new Abstraction(1); + CalculateWidths(currentFace); + //the adjacent triangle is now possibly degree-1 + degree1.push() = tempFace; + //mark this face as dealt with + _mesh->mark(currentFace); + } + //if there aren't any, this is a tree component + else if (numAdjacent == 0) + { + TreeAbstract(currentFace, component++); + //mark this face as dealt with + _mesh->mark(currentFace); + } + } + //done with degree-1 nodes + _mesh->end_marking(); + +//Degree-2 and Degree-3 faces + //go through all possible degree-3 triangles + for (int n = 0; n < processing.size(); n++) + { + //gets the current face to check + SeDcdtFace *currentFace = processing[n]; + //we only care about unabstracted nodes at this point + if (Degree(currentFace) != UNABSTRACTED) + { + continue; + } + //the number of non-degree-1 triangles across unconstrained edges + int numAdjacent = 0; + //access the elements of the triangle + SeBase *s = currentFace->se(); + //go through the edges of the triangle + for (int i = 0; i < 3; i++) + { + //get the degree of the triangle across that edge + int degree = Degree(s->sym()); + //keep track of non-degree-1 edges across unconstrained edges + if (!Blocked(s) && (degree != 1)) + { + numAdjacent++; + } + //move to the next edge + s = s->nxt(); + } + //if this is not a degree-3 triangle, skip it + if (numAdjacent < 3) + { + continue; + } + //stack of degree-3 nodes in this connected component + SrArray degree3; + //put the current triangle on the stack + degree3.push() = currentFace; + //continue through all degree-3 nodes in this component + while (!degree3.empty()) + { + //get one of the degree-3 nodes from the stack + SeDcdtFace *stackFace = degree3.pop(); + //abstract it and calculate its widths and set its connected component + stackFace->link = new Abstraction(3); +#if defined EXPERIMENT + //keep track of the extra degree-3 node + num++; +#endif + CalculateWidths(stackFace); + stackFace->link->Component(component); + //access the elements of the triangle + s = stackFace->se(); + //go in all directions from this node + for (int i = 0; i < 3; i++) + { + //get the face across this particular edge + SeDcdtFace *tempFace = (SeDcdtFace *)(s->sym()->fac()); + //mark the first face and the previous face + SeDcdtFace *firstFace = stackFace; + SeDcdtFace *lastFace = stackFace; + //keep track of the cumulative angle since the last degree-3 node + float angle = 0; + //follow this chain until another degree-3 node is encountered + while (true) + { + //the number of adjacent faces across unconstrained edges not abstracted as degree-1 + numAdjacent = 0; + //access the elements of this triangle + SeBase *s1 = tempFace->se(); + //go through the edges of this triangle + for (int j = 0; j < 3; j++) + { + //get the degree of the face across this edge + int degree = Degree(s1->sym()); + //if it's not blocked or degree-1, it's adjacent + if (!Blocked(s1) && (degree != 1)) + { + numAdjacent++; + } + //move to the next edge + s1 = s1->nxt(); + } + //if the current triangle is degree-3, + if (numAdjacent == 3) + { + //if it hasn't been abstracted yet, put it on the stack to deal with + if (Degree(tempFace) == UNABSTRACTED) + { + degree3.push() = tempFace; + } + //sets the original degree-3 node adjacent to this one + stackFace->link->Adjacent(i, tempFace); + //and sets the total angle between them + stackFace->link->Angle(i, angle); + //if there were no degree-2 nodes in between these, + if (lastFace == stackFace) + { + //get the elements of this triangle + SeBase *s1 = tempFace->se(); + //go through the edges + for (int j = 0; j < 3; j++) + { + //find the edge between this triangle and the last one + if (s1->sym()->fac() == lastFace) + { + //the choke point is the length of the edge between these faces + float x1, y1, x2, y2, x3, y3; + TriangleVertices(s1, x1, y1, x2, y2, x3, y3); + stackFace->link->Choke(i, Length(x1, y1, x2, y2)); + break; + } + //move to the next edge + s1 = s1->nxt(); + } + } + //if there were degree-2 nodes between them, + else + { + //access the elements of the previous triangle + SeBase *s1 = lastFace->se(); + float choke, width; + //go through its edges + for (int j = 0; j < 3; j++) + { + //find the direction of the original degree-3 node + if (lastFace->link->Adjacent(j) == stackFace) + { + //get the choke point to this node + choke = lastFace->link->Choke(j); + //get the width through this triangle between the two degree-3 nodes + width = lastFace->link->Width((s1->nxt()->sym()->fac() == tempFace) ? j : (j + 2) % 3); + //the choke point of the original degree-3 node is the lesser of the two + stackFace->link->Choke(i, Minimum(choke, width)); + break; + } + //move to the next edge + s1 = s1->nxt(); + } + } + //we've reached the other degree-3 node - stop following this chain + break; + } + //if the triangle is degree-2 + else if (numAdjacent == 2) + { + //if the triangle wasn't abstracted before, + if (Degree(tempFace) == UNABSTRACTED) + { + //abstract it and set its widths and connected component + tempFace->link = new Abstraction(2); + CalculateWidths(tempFace); + tempFace->link->Component(component); + } + //access the elements of this triangle + SeBase *s1 = tempFace->se(); + //go through the edges of the triangle + for (int j = 0; j < 3; j++) + { + //find the edge across which is the last triangle + if (s1->sym()->fac() == lastFace) + { + //set the angle back towards the original degree-3 node + tempFace->link->Angle(j, angle); + //and set the adjacent degree-3 node in that direction + tempFace->link->Adjacent(j, firstFace); + //if there are no nodes between this one and the degree-3 node, + if (lastFace == firstFace) + { + //the choke point is simply the length of the edge between them + float x1, y1, x2, y2, x3, y3; + TriangleVertices(s1, x1, y1, x2, y2, x3, y3); + tempFace->link->Choke(j, Length(x1, y1, x2, y2)); + } + else + { + float choke, width; + //otherwise, access the elements of the last triangle + SeBase *s2 = lastFace->se(); + //and go through the edges + for (int k = 0; k < 3; k++) + { + //we want the edge which leads back to the original degree-3 node + if ((s2->sym()->fac() == tempFace) || (lastFace->link->Adjacent(k) == NULL)) + { + s2 = s2->nxt(); + continue; + } + //get the associated choke point width + choke = lastFace->link->Choke(k); + //get the width between the two degree-3 nodes + if (s2->nxt()->sym()->fac() == tempFace) + { + width = lastFace->link->Width(k); + } + else + { + width = lastFace->link->Width((k + 2) % 3); + } + break; + } + //the choke point value is the lesser of these + tempFace->link->Choke(j, Minimum(choke, width)); + } + break; + } + //move to the next edge + s1 = s1->nxt(); + } + //calculate the angle between edges leading to degree-3 nodes + float currentAngle = 0; + //access the elements of this triangle again + s1 = tempFace->se(); + //record the next triangle to move to + SeDcdtFace *nextFace = NULL; + //go through the edges of the triangle + for (int j = 0; j < 3; j++) + { + //get the degree of the adjacent triangle across this edge + int degree = Degree(s1->sym()); + //if there is a tree component off this node, collapse it + if (!Blocked(s1) && (degree == 1) + && (((SeDcdtFace *)s1->sym()->fac())->link->Component() == INVALID)) + { + TreeCollapse(tempFace, (SeDcdtFace *)(s1->sym()->fac()), component); + } + //when finding the next face, record it + if (!Blocked(s1) && (s1->sym()->fac() != lastFace) && (degree != 1)) + { + nextFace = (SeDcdtFace *)s1->sym()->fac(); + } + //determine the degree of the triangle across the next edge + int degree2 = Degree(s1->nxt()->sym()); + //if this edge and the next are the connecting edges, record the angle between them + if (!((degree == 0) || (degree == 1) || (degree2 == 0) || (degree2 == 1)) + && !Blocked(s1) && !Blocked(s1->nxt())) + { + float x1, y1, x2, y2, x3, y3; + TriangleVertices(s1, x1, y1, x2, y2, x3, y3); + currentAngle = abs(AngleBetween(x1, y1, x2, y2, x3, y3)); + } + //move to the next edge + s1 = s1->nxt(); + } + //increment the cumulative angle so far + angle += currentAngle; + //move to the next edge + lastFace = tempFace; + tempFace = nextFace; + } + //in other cases, report an error + else + { + sr_out.warning("ERROR: should only encounter degree-2 and degree-3 faces this way\n"); + } + } + //follow another chain + s = s->nxt(); + } + } + //get the next component + component++; + } + +//Degree-2 faces (in a ring) + //go through the processing queue again + for (int i = 0; i < processing.size(); i++) + { + //get the current triangle + SeDcdtFace *currentFace = processing[i]; + //only continue if it hasn't been abstracted yet + if (Degree(currentFace) != UNABSTRACTED) + { + continue; + } + //mark it as the first triangle in the ring + SeDcdtFace *firstFace = currentFace; + //the next triangle to visit + SeDcdtFace *nextFace = NULL; + //follow the ring of unabstracted faces + while (true) + { + //access the elements of the current triangle + SeBase *s = currentFace->se(); + //go through its edges + for (int i = 0; i < 3; i++) + { + //record the degree of the triangle across this edge + int degree = Degree(s->sym()); + //if there's a tree component off this face, collapse it + if (!Blocked(s) && (degree == 1)) + { + TreeCollapse(currentFace, (SeDcdtFace *)s->sym()->fac(), component); + } + //when the next face is found in the ring, record it + if (!Blocked(s) && (degree == UNABSTRACTED)) + { + nextFace = (SeDcdtFace *)s->sym()->fac(); + } + //move to the next edge + s = s->nxt(); + } + //abstract the current face, calculate its widths, and set its connected component + currentFace->link = new Abstraction(2); + CalculateWidths(currentFace); + currentFace->link->Component(component); + //access the triangle's elements again + s = currentFace->se(); + //go through the edges again + for (int i = 0; i < 3; i++) + { + //if the adjacent triangle is part of the ring + if (!Blocked(s) && Degree(s->sym()) != 1) + { + //set the angle and choke values accordingly + currentFace->link->Angle(i, INFINITY); + currentFace->link->Choke(i, INFINITY); + } + //move to the next edge + s = s->nxt(); + } + //if the start of the ring has been reencountered, stop following it + if (currentFace == nextFace) + { + break; + } + //move to the next face in the ring + currentFace = nextFace; + } + //move to the next ring (connected component) + component++; + } +#if defined EXPERIMENT + //output the number of degree-3 nodes to the output file + std::ofstream outFile("Data.txt", std::ios_base::app); + outFile << "Degree3: " << num << "\t"; + outFile.close(); +#endif +} + +//Abstract into a tree +void SeDcdt::TreeAbstract(SeDcdtFace *first, int component) +{ + //create a stack of the nodes in the tree + SrArray tree; + //put the initial node on the tree + tree.push() = first; + //continue until the tree can't be expanded anymore + while (!tree.empty()) + { + //get the next face off of the stack + SeDcdtFace *currentFace = tree.pop(); + //get the elements of this triangle + SeBase *s = currentFace->se(); + //go through its edges + for (int i = 0; i < 3; i++) + { + //ignore triangles across contrained edges + if (Blocked(s)) + { + s = s->nxt(); + continue; + } + //gets the face across that edge + SeDcdtFace *tempFace = (SeDcdtFace *)s->sym()->fac(); + //if the adjacent triangle hasn't been dealt with, put it on the stack + if ((tempFace->link->Angle(0) == INVALID) && (tempFace->link->Angle(1) == INVALID) + && (tempFace->link->Angle(2) == INVALID)) + { + tree.push() = tempFace; + } + //move to the next edge + s = s->nxt(); + } + //abstract the triangle if it hasn't already been + if (Degree(currentFace) == UNABSTRACTED) + { + //if the triangle hasn't been abstracted yet, abstract it and calculate its widths + currentFace->link = new Abstraction(1); + CalculateWidths(currentFace); + } + //access the triangle's elements again + s = currentFace->se(); + //move through its edges + for (int i = 0; i < 3; i++) + { + //for each unconstrained edge, + if (!Blocked(s)) + { + //set the angle and choke values accordingly + currentFace->link->Angle(i, INFINITY); + currentFace->link->Choke(i, INFINITY); + } + //move to the next edge + s = s->nxt(); + } + //set the triangle's connected component + currentFace->link->Component(component); + } +} + +//Collapse a tree onto a degree-2 node +void SeDcdt::TreeCollapse(SeDcdtFace *root, SeDcdtFace *currentFace, int component) +{ + //mark edges we have dealt with + _mesh->begin_marking(); + //starting with the root of the tree + _mesh->mark(root); + //create a stack of triangles to be processed + SrArray tree; + //and a parallel one for angles of those triangles + SrArray angles; + //put the current triangle on the stack to be processed + tree.push() = currentFace; + //and record that it is at angle 0 from the root + angles.push() = 0.0f; + //continue until all faces in the tree have been dealt with + while (!tree.empty()) + { + //get the top face off the stack + currentFace = tree.pop(); + //set its connected component + currentFace->link->Component(component); + //mark this face as dealt with + _mesh->mark(currentFace); + //access this face's elements + SeBase *s = currentFace->se(); + //the angles to the other adjacent triangles + float angle[2]; + + int i; + //go through the edges + for (i = 0; i < 3; i++) + { + //the face across that edge + SeDcdtFace *tempFace = (SeDcdtFace *)s->sym()->fac(); + //if this is the entry edge, + if (!Blocked(s) && _mesh->marked(tempFace)) + { + //set the angle + currentFace->link->Angle(i, angles.pop()); + //calculate the angle to the other adjacent triangles + float x1, y1, x2, y2, x3, y3; + TriangleVertices(s, x1, y1, x2, y2, x3, y3); + angle[0] = abs(AngleBetween(x1, y1, x2, y2, x3, y3)) + currentFace->link->Angle(i); + angle[1] = abs(AngleBetween(x2, y2, x3, y3, x1, y1)) + currentFace->link->Angle(i); + //set the adjacent triangle in that direction to the root + currentFace->link->Adjacent(i, root); + //if this is the triangle next to the root, + if (tempFace == root) + { + //the choke value is the length of the edge between them + currentFace->link->Choke(i, Length(x1, y1, x2, y2)); + } + //if there are triangles between this and the root, + else + { + //get the elements of the current triangle + SeBase *s1 = tempFace->se(); + float width, choke; + //go through its edges + for (int j = 0; j < 3; j++) + { + //get the choke value in that direction + float temp = tempFace->link->Choke(j); + //if it's a valid value, + if ((temp != INVALID) && (temp != INFINITY)) + { + //record it + choke = temp; + //gets the width through that triangle between the root and current triangles + if (s1->nxt()->sym()->fac() == currentFace) + { + width = tempFace->link->Width(j); + } + else + { + width = tempFace->link->Width((j + 2) % 3); + } + break; + } + //move to the next edge + s1 = s1->nxt(); + } + //the choke value of this triangle is the lesser of these values + currentFace->link->Choke(i, Minimum(width, choke)); + } + break; + } + //move to the next edge + s = s->nxt(); + } + //record where we left off + int base = i + 1; + //visit the other two adjacent triangles + for (int i = 0; i < 2; i++) + { + //move to the next edge + s = s->nxt(); + //only deal with ones not across a constrained edge + if (Blocked(s)) + { + continue; + } + //set the angle and choke values acoordingly + currentFace->link->Angle((i + base) % 3, INFINITY); + currentFace->link->Choke((i + base) % 3, INFINITY); + //put them on the tree for processing + tree.push() = (SeDcdtFace *)s->sym()->fac(); + //also put the appropriate angle on the other tree + angles.push() = angle[i]; + } + } + //finished collapsing the tree + _mesh->end_marking(); +} + +//deletes the information from the abstraction +void SeDcdt::DeleteAbstraction() +{ + //get the first face in the mesh + SeDcdtFace *currentFace = _mesh->first()->fac(); + //and record it + SeDcdtFace *firstFace = currentFace; + //go through the faces + do + { + //if the triangle is abstracted, + if (currentFace->link != NULL) + { + //free the abstraction information + delete currentFace->link; + currentFace->link = NULL; + } + //move to the next triangle + currentFace = currentFace->nxt(); + } + //until reaching the first face again + while (currentFace != firstFace); + //mark the outside face as being invalid + outside = NULL; +} + +//Abstract function definitions } diff --git a/source/dcdt/se/Abstraction.h b/source/dcdt/se/Abstraction.h new file mode 100644 index 0000000000..2e46740a8f --- /dev/null +++ b/source/dcdt/se/Abstraction.h @@ -0,0 +1,157 @@ +//Abstraction.h + +//DJD: definition of Abstraction class { + +#ifndef ABSTRACTION_H +#define ABSTRACTION_H + +#include +#include "FunnelDeque.h" + +//defines an invalid entry, and "infinity", for our purposes +#define INVALID -1 +#define INFINITY FLT_MAX + +//prototypes for classes defined later +template class SeLinkFace; +class Abstraction; +typedef SeLinkFace SeDcdtFace; + +//class which contains information about our topological abstraction +class Abstraction +{ +protected: + //list of adjacent nodes + //if this node is degree-1 in a tree, all will be NULL + //if it is degree-1 otherwise, one will be the root of the tree, the others will be NULL + //if it is degree-2 in a ring, all will be NULL + //if it is degree-2 otherwise, two will be degree-3 nodes on the ends of the corridor, the other will be NULL + //if it is degree-3, all will be degree-3 nodes on the end of the corridors + SeDcdtFace *m_CAdjacent[3]; + //sum of interior angles of triangles between this one and the corresponding adjacent one + //INVALID if the corresponding adjacent node is NULL + float m_dAngle[3]; + //width between the corresponding edge and the one counterclockwise to it, through the triangle + float m_dWidth[3]; + //smallest width through the triangles between this one and the corresponding adjacent one + //INVALID if the corresponding adjacent node is NULL + float m_dChoke[3]; + //degree of this node + int m_nDegree; + //connected component to which this triangle belongs + int m_nComponent; + + //makes sure the index is in the range [0, 2] + bool CheckBounds(int n) + { + return ((n >= 0) && (n < 3)); + } +public: + //creates the abstraction with given degree + Abstraction(int nDegree) + { + //starts all adjacents as NULL and other information as INVALID + for (int i = 0; i < 3; i++) + { + m_CAdjacent[i] = NULL; + m_dAngle[i] = INVALID; + m_dWidth[i] = INVALID; + m_dChoke[i] = INVALID; + } + m_nDegree = nDegree; + m_nComponent = INVALID; + } + + //default destructor + ~Abstraction() + { + } + + //accessor for the degree of the node + int Degree() + { + return m_nDegree; + } + + //mutator for the degree of the node + void Degree(int nDegree) + { + m_nDegree = nDegree; + } + + //accessor for the connected component of the node + int Component() + { + return m_nComponent; + } + + //mutator for the connected component of the node + void Component(int nComponent) + { + m_nComponent = nComponent; + } + + //accessor for the adjacent node with index given + SeDcdtFace *Adjacent(int n) + { + return (CheckBounds(n)) ? m_CAdjacent[n] : NULL; + } + + //mutator for the adjacent node with index given + void Adjacent(int n, SeDcdtFace *CAdjacent) + { + if (CheckBounds(n)) + { + m_CAdjacent[n] = CAdjacent; + } + } + + //accessor for the angle with index given + float Angle(int n) + { + return (CheckBounds(n)) ? m_dAngle[n] : INVALID; + } + + //mutator for the angle with index given + void Angle(int n, float dAngle) + { + if (CheckBounds(n)) + { + m_dAngle[n] = dAngle; + } + } + + //accessor for the width with index given + float Width(int n) + { + return (CheckBounds(n)) ? m_dWidth[n] : INVALID; + } + + //mutator for the width with index given + void Width(int n, float dWidth) + { + if (CheckBounds(n)) + { + m_dWidth[n] = dWidth; + } + } + + //accessor for the choke point width with index given + float Choke(int n) + { + return (CheckBounds(n)) ? m_dChoke[n] : INVALID; + } + + //mutator for the choke point width with index given + void Choke(int n, float dChoke) + { + if (CheckBounds(n)) + { + m_dChoke[n] = dChoke; + } + } +}; + +#endif + +//definition of Abstraction class } diff --git a/source/dcdt/se/Experiments.h b/source/dcdt/se/Experiments.h new file mode 100644 index 0000000000..5ef5c2e333 --- /dev/null +++ b/source/dcdt/se/Experiments.h @@ -0,0 +1,100 @@ +//Experiments.h + +//DJD: definitions used in running experiments { + +#ifndef EXPERIMENTS_H +#define EXPERIMENTS_H + +//windows-dependent timing mechanism +#include + +//for accurate timing of paths +class Timer +{ +protected: + //time at which the timer was started + LARGE_INTEGER StartTime; +// LARGE_INTEGER StopTime; + //frequency of the counter + LONGLONG Frequency; + //calibration component - time between lines + LONGLONG Correction; +public: + //constructor + Timer() + { + //retrieves the frequency of the counter + LARGE_INTEGER freq; + QueryPerformanceFrequency(&freq); + //gets the relevant portion of that data + Frequency = freq.QuadPart; + LARGE_INTEGER StopTime; + //starts the timer + Start(); +// Stop(); + //and measures the time between these lines + QueryPerformanceCounter(&StopTime); + //stores the relevant part of that figure + Correction = StopTime.QuadPart - StartTime.QuadPart; + } + + //start timing + void Start() + { +// Sleep(0); + //record the start time + QueryPerformanceCounter(&StartTime); + } + +// void Stop() +// { +// QueryPerformanceCounter(&StopTime); +// } + + //return the time since the start time in milliseconds + float GetDuration() + { + //get the current time from the timer + LARGE_INTEGER StopTime; + QueryPerformanceCounter(&StopTime); + //return the calculated duration since the timer was started + return (float)(StopTime.QuadPart - StartTime.QuadPart - Correction) * 1000.0f / Frequency; + } +}; + +//structure for holding data from the experiments +struct Data +{ + //the total time the algorithm ran + float TotalTime; + //the time taken to construct channels, run funnel algorithms, etc. + float ConstructionTime; + //the length of the best path found to this point + float Length; + //the number of nodes searched at this point + int SearchNodes; + //the number of triangles used in funnel algorithms, etc. + int ConstructionNodes; + //the number of paths found so far + int Paths; + //constructor + Data() + { + //initializes all variables + Reset(); + } + //initializes all variables to default values + void Reset() + { + TotalTime = 0.0f; + ConstructionTime = 0.0f; + Length = 0.0f; + SearchNodes = 0; + ConstructionNodes = 0; + Paths = 1; + } +}; + +#endif + +//definitions used in running experiments } diff --git a/source/dcdt/se/FunnelDeque.cpp b/source/dcdt/se/FunnelDeque.cpp new file mode 100644 index 0000000000..c8df227f2a --- /dev/null +++ b/source/dcdt/se/FunnelDeque.cpp @@ -0,0 +1,557 @@ +//FunnelDeque.cpp + +//DJD: definition of FunnelNode functions { + +#include "precompiled.h" + +#include "FunnelDeque.h" +#include +#include + +//default constructor - initializes all +//member variables to default values +FunnelNode::FunnelNode() +{ + m_CLeft = NULL; + m_CRight = NULL; + m_CPoint.set(FLT_MIN, FLT_MIN); +} + +//constructor - initializes the point member +//variable to the coordinates passed +FunnelNode::FunnelNode(float x, float y) +{ + m_CLeft = NULL; + m_CRight = NULL; + m_CPoint.set(x, y); +} + +//constructor - initializes the point member +//variable to the one passed +FunnelNode::FunnelNode(const SrPnt2& point) +{ + m_CLeft = NULL; + m_CRight = NULL; + m_CPoint.set(point.x, point.y); +} + +//mutator for the left pointer +void FunnelNode::Left(FunnelNode *left) +{ + m_CLeft = left; +} + +//mutator for the right pointer +void FunnelNode::Right(FunnelNode *right) +{ + m_CRight = right; +} + +//accessor for the left pointer +FunnelNode *FunnelNode::Left() +{ + return m_CLeft; +} + +//accessor for the right pointer +FunnelNode *FunnelNode::Right() +{ + return m_CRight; +} + +//mutator for the point +void FunnelNode::Point(const SrPnt2& point) +{ + m_CPoint.set(point.x, point.y); +} + +//accessor for the point +SrPnt2 FunnelNode::Point() +{ + SrPnt2 copy; + copy.set(m_CPoint.x, m_CPoint.y); + return copy; +} + +//mutator for the point's coordinates +void FunnelNode::Point(float x, float y) +{ + m_CPoint.set(x, y); +} + +//accessor for the point's X coordinate +float FunnelNode::X() +{ + return m_CPoint.x; +} + +//accessor for the point's Y coordinate +float FunnelNode::Y() +{ + return m_CPoint.y; +} + +//definition of FunnelNode functions } + +//DJD: definition of FunnelDeque functions { + +//constructor - initializes the apex of the funnel +//to the point passed, and the unit radius +FunnelDeque::FunnelDeque(float x, float y, float r) +{ + m_CApex = new FunnelNode(x, y); + m_CLeft = m_CApex; + m_CRight = m_CApex; + m_tApexType = Point; + m_dRadius = r; +} + +//constructor - initializes the apex of the funnel +//to the coordinates passed, and the unit radius +FunnelDeque::FunnelDeque(const SrPnt2& point, float r) +{ + m_CApex = new FunnelNode(point); + m_CLeft = m_CApex; + m_CRight = m_CApex; + m_tApexType = Point; + m_dRadius = r; +} + +//destructor - deletes any funnel nodes in the deque +FunnelDeque::~FunnelDeque() +{ + FunnelNode *current = m_CLeft; + while (current != NULL) + { + FunnelNode *temp = current; + current = current->Right(); + delete temp; + } +} + +//Adds a new point to the funnel, adds to the existing path if the apex moves +//type = RightTangent => point is being added on left side +//type = LeftTangent => point is being added on right side +//type = Point => point is the end of the channel +void FunnelDeque::Add(const SrPnt2& p, CornerType type, SrPolygon& path) +{ + //the new funnel node containing the new point + FunnelNode *next = new FunnelNode(p); + //if it's being added on the left side + //(operations are just reversed for the other side) + if (type == RightTangent) + { + //loop until the point is added to the funnel + while (true) + { + //if the apex is the only point in the funnel, + //simply add the point to the appropriate side + if (m_CLeft == m_CRight) + { + next->Right(m_CApex); + m_CApex->Left(next); + m_CLeft = next; + break; + } + float x_1, y_1, x_2, y_2; + CornerType type_1, type_2; + //if there are no points on the left side of the funnel, + //get the wedge going to the opposite side of the apex + if (m_CLeft == m_CApex) + { + x_1 = m_CLeft->X(); + y_1 = m_CLeft->Y(); + x_2 = m_CLeft->Right()->X(); + y_2 = m_CLeft->Right()->Y(); + type_1 = m_tApexType; + type_2 = LeftTangent; + } + //otherwise get the wedge on this side of the apex + else + { + x_2 = m_CLeft->X(); + y_2 = m_CLeft->Y(); + x_1 = m_CLeft->Right()->X(); + y_1 = m_CLeft->Right()->Y(); + type_2 = RightTangent; + if (m_CLeft->Right() == m_CApex) + { + type_1 = m_tApexType; + } + else + { + type_1 = RightTangent; + } + } + //calculate the angle associated with this wedge + float wedge = Angle(x_1, y_1, type_1, x_2, y_2, type_2); + //also calculate the distance between these two points + float length1 = Distance(x_1, y_1, x_2, y_2); + //and that to the new point (special case for the modofied funnel algorithm) + float length2 = Distance(x_1, y_1, p.x, p.y); + //now get the wedge leading to the new point + x_1 = m_CLeft->X(); + y_1 = m_CLeft->Y(); + if (m_CApex == m_CLeft) + { + type_1 = m_tApexType; + } + else + { + type_1 = RightTangent; + } + x_2 = p.x; + y_2 = p.y; + type_2 = type; + //and the angle associated with it + float toPoint = Angle(x_1, y_1, type_1, x_2, y_2, type_2); + //compare the two angles + float diff = wedge - toPoint; + diff += (diff <= -PI) ? 2.0f * PI : (diff > PI) ? -2.0f * PI : 0.0f; + //if the existing wedge is counterclockwise of that to the point + if (diff < 0.0f) + { + //add the new point on this end of the funnel + m_CLeft->Left(next); + next->Right(m_CLeft); + m_CLeft = next; + break; + } + //if it is clockwise, + else + { + //check if we are popping the apex + if (m_CLeft == m_CApex) + { + //if the point being added is closer than the one being popped, + if (length2 < length1) + { + //make the new point the apex instead of the one on the opposite side + //(special case for the modified funnel algorithm) + float angle = Angle(m_CApex->X(), m_CApex->Y(), m_tApexType, p.x, p.y, type); + AddPoint(m_CApex->X(), m_CApex->Y(), m_tApexType, angle, path); + AddPoint(p.x, p.y, type, angle, path); + next->Right(m_CApex->Right()); + m_CApex->Right()->Left(next); + delete m_CApex; + m_CApex = next; + m_CLeft = next; + m_tApexType = type; + break; + } + //otherwise, + else + { + //move the apex to the right and add the segment between + //the old and new apexes to the path so far + float angle = Angle(m_CApex->X(), m_CApex->Y(), m_tApexType, + m_CApex->Right()->X(), m_CApex->Right()->Y(), CornerType::LeftTangent); + AddPoint(m_CApex->X(), m_CApex->Y(), m_tApexType, angle, path); + AddPoint(m_CApex->Right()->X(), m_CApex->Right()->Y(), CornerType::LeftTangent, angle, path); + m_CApex = m_CApex->Right(); + m_tApexType = LeftTangent; + } + } + //pop the leftmost point off of the funnel + FunnelNode *temp = m_CLeft; + m_CLeft = m_CLeft->Right(); + m_CLeft->Left(NULL); + delete temp; + } + } + } + //otherwise, if adding a point on the right side, + //continue as before, with sides reversed + //adding a point also ends up here - + //we do this to pop off all the necessary points off of + //the right side of the funnel, so at the end we can just + //add to the path the points between the apex and the + //right side of the funnel (done at the end) + else + { + while (true) + { + if (m_CLeft == m_CRight) + { + next->Left(m_CApex); + m_CApex->Right(next); + m_CRight = next; + break; + } + float x_1, y_1, x_2, y_2; + CornerType type_1, type_2; + if (m_CRight == m_CApex) + { + x_1 = m_CRight->X(); + y_1 = m_CRight->Y(); + x_2 = m_CRight->Left()->X(); + y_2 = m_CRight->Left()->Y(); + type_1 = m_tApexType; + type_2 = RightTangent; + } + else + { + x_2 = m_CRight->X(); + y_2 = m_CRight->Y(); + x_1 = m_CRight->Left()->X(); + y_1 = m_CRight->Left()->Y(); + type_2 = LeftTangent; + if (m_CRight->Left() == m_CApex) + { + type_1 = m_tApexType; + } + else + { + type_1 = LeftTangent; + } + } + float wedge = Angle(x_1, y_1, type_1, x_2, y_2, type_2); + float length1 = Distance(x_1, y_1, x_2, y_2); + float length2 = Distance(x_1, y_1, p.x, p.y); + x_1 = m_CRight->X(); + y_1 = m_CRight->Y(); + if (m_CApex == m_CRight) + { + type_1 = m_tApexType; + } + else + { + type_1 = LeftTangent; + } + x_2 = p.x; + y_2 = p.y; + type_2 = type; + float toPoint = Angle(x_1, y_1, type_1, x_2, y_2, type_2); + float diff = wedge - toPoint; + diff += (diff <= -PI) ? 2.0f * PI : (diff > PI) ? -2.0f * PI : 0.0f; + if (diff < 0.0f) + { + if (m_CRight == m_CApex) + { + if (length2 < length1) + { + float angle = Angle(m_CApex->X(), m_CApex->Y(), m_tApexType, p.x, p.y, type); + AddPoint(m_CApex->X(), m_CApex->Y(), m_tApexType, angle, path); + AddPoint(p.x, p.y, type, angle, path); + next->Left(m_CApex->Left()); + m_CApex->Left()->Right(next); + delete m_CApex; + m_CApex = next; + m_CRight = next; + m_tApexType = type; + break; + } + else + { + float angle = Angle(m_CApex->X(), m_CApex->Y(), m_tApexType, + m_CApex->Left()->X(), m_CApex->Left()->Y(), CornerType::RightTangent); + AddPoint(m_CApex->X(), m_CApex->Y(), m_tApexType, angle, path); + AddPoint(m_CApex->Left()->X(), m_CApex->Left()->Y(), CornerType::RightTangent, angle, path); + m_CApex = m_CApex->Left(); + m_tApexType = RightTangent; + } + } + FunnelNode *temp = m_CRight; + m_CRight = m_CRight->Left(); + m_CRight->Right(NULL); + delete temp; + } + else + { + m_CRight->Right(next); + next->Left(m_CRight); + m_CRight = next; + break; + } + } + } + //this is where the points between the apex + //and the right side o fthe funnel are added to the path + //when adding the final point to the channel + if (type == Point) + { + FunnelNode *Current = m_CApex; + while (Current != m_CRight) + { + float x_1, y_1, x_2, y_2, toPoint; + CornerType type_1, type_2; + type_1 = (Current == m_CApex) ? m_tApexType : CornerType::LeftTangent; + type_2 = (Current->Right() == m_CRight) ? CornerType::Point : CornerType::LeftTangent; + x_1 = Current->X(); + y_1 = Current->Y(); + Current = Current->Right(); + x_2 = Current->X(); + y_2 = Current->Y(); + toPoint = Angle(x_1, y_1, type_1, x_2, y_2, type_2); + AddPoint(x_1, y_1, type_1, toPoint, path); + AddPoint(x_2, y_2, type_2, toPoint, path); + } + } +} + +//returns the length of a path found by this funnel algorithm with the current radius +float FunnelDeque::Length(SrPolygon path) +{ + //only paths with an even number of points are valid + if ((path.size() % 2) != 0) + { + return 0.0f; + } + float length = 0.0f; + //go through the straight segments and total their lengths + for (int i = 0; i < path.size(); i += 2) + { + float xDiff = path[i + 1].x - path[i].x; + float yDiff = path[i + 1].y - path[i].y; + length += sqrt(xDiff * xDiff + yDiff * yDiff); + } + //go through the curved segments and add their length + //(the difference of the angles of the + // surrounding segments, times the unit radius) + for (int i = 1; i < path.size() - 3; i += 2) + { + float angle1 = atan2(path[i + 1].y - path[i].y, path[i + 1].x - path[i].x); + float angle2 = atan2(path[i + 3].y - path[i + 2].y, path[i + 3].x - path[i + 2].x); + float diff = angle1 - angle2; + diff += (diff <= -PI) ? 2.0f * PI : (diff > PI) ? -2.0f * PI : 0.0f; + length += m_dRadius * abs(diff); + } + return length; +} + +//returns the angle from x_1, y_1 to x_2, y_2, given the types +float FunnelDeque::Angle(float x_1, float y_1, CornerType type_1, float x_2, float y_2, CornerType type_2) +{ + //simply calls the appropriate function based + //on the types of the two points + if (type_1 == Point) + { + return PointToTangent(x_1, y_1, x_2, y_2, (type_2 == RightTangent)); + } + else if (type_2 == Point) + { + float angle = PointToTangent(x_2, y_2, x_1, y_1, (type_1 == LeftTangent)); + return AngleRange(angle + PI); + } + else if (type_1 != type_2) + { + return ToTangentAlt(x_1, y_1, x_2, y_2, (type_1 == LeftTangent)); + } + else + { + return ToTangent(x_1, y_1, x_2, y_2); + } +} + +//adds a point to the path given a coordinate, its type, +//and the angle of the segment attached +void FunnelDeque::AddPoint(float x, float y, CornerType type, float angle, SrPolygon &path) +{ + //if it's a point type, just add it unchanged + if (type == Point) + { + path.push().set(x, y); + } + //otherwise, + else + { + //move the unit radius from the point to one side or the other + float theta = AngleRange(angle + PI / ((type == LeftTangent) ? 2.0f : -2.0f)); + path.push().set(x + cos(theta) * m_dRadius, y + sin(theta) * m_dRadius); + } +} + +//takes an angle value and restricts it to the range (-PI, PI] +float FunnelDeque::AngleRange(float theta) +{ + return ((theta > PI) ? (theta - 2.0f * PI) : + (theta <= -PI) ? (theta + 2.0f * PI) : theta); +} + +//gets the distance between the points (x_1, y_1) and (x_2, y_2) +float FunnelDeque::Distance(float x_1, float y_1, float x_2, float y_2) +{ + float x = x_2 - x_1; + float y = y_2 - y_1; + return sqrt(x * x + y * y); +} + +//gets the angle of the line tangent to two points of the same type +float FunnelDeque::ToTangent(float x_1, float y_1, float x_2, float y_2) +{ + return atan2(y_2 - y_1, x_2 - x_1); +} + +//gets the angle of the line tangent to a point +//of type LeftTangent and one of type RightTangent +float FunnelDeque::ToTangentAlt(float x_1, float y_1, float x_2, float y_2, bool LeftRight) +{ + float h = Distance(x_1, y_1, x_2, y_2) / 2.0f; + float l = sqrt(h * h - m_dRadius * m_dRadius); + float interior = atan(m_dRadius / l); + float absolute = atan2(y_2 - y_1, x_2 - x_1); + if (LeftRight) + { + interior = -interior; + } + return AngleRange(absolute + interior); +} + +//gets the angle of the line going through one point +//and running tangent to another +float FunnelDeque::PointToTangent(float x_1, float y_1, float x_2, float y_2, bool Right) +{ + float h = Distance(x_1, y_1, x_2, y_2); + float l = sqrt(h * h - m_dRadius * m_dRadius); + float interior = atan(m_dRadius / l); + float absolute = atan2(y_2 - y_1, x_2 - x_1); + if (Right) + { + interior = -interior; + } + return AngleRange(absolute + interior); +} + +//prints the funnel (used for debugging) +void FunnelDeque::Print() +{ + FunnelNode *Current = m_CLeft; + while (Current != NULL) + { + sr_out << "\t(" << Current->X() << ", " << Current->Y() << ")"; + if (Current == m_CLeft) + { + sr_out << " <-- Left"; + } + if (Current == m_CApex) + { + sr_out << " <-- Apex"; + } + if (Current == m_CRight) + { + sr_out << " <-- Right"; + } + sr_out << srnl; + Current = Current->Right(); + } +} + +//prints the path and then the funnel (used for debugging) +void FunnelDeque::Print(SrPolygon path) +{ + sr_out << "Path = {"; + for (int i = 0; i < path.size() - 1; i++) + { + SrPnt2 p = path[i]; + sr_out << "(" << p.x << ", " << p.y << "), "; + } + if (path.size() > 0) + { + SrPnt2 p = path[path.size() - 1]; + sr_out << "(" << p.x << ", " << p.y << ")"; + } + sr_out << "}\n"; + Print(); +} + +//definition of FunnelDeque functions } diff --git a/source/dcdt/se/FunnelDeque.h b/source/dcdt/se/FunnelDeque.h new file mode 100644 index 0000000000..cac0e1ca95 --- /dev/null +++ b/source/dcdt/se/FunnelDeque.h @@ -0,0 +1,107 @@ +//FunnelDeque.h + +//DJD: definition of FunnelNode class { + +#ifndef FUNNELDEQUE_H +#define FUNNELDEQUE_H + +#include "sr_vec2.h" +#include "sr_polygon.h" + +//define a floating point value for pi +#define PI 3.1415926535897932384626433832795f + +//a node in the funnel deque +class FunnelNode +{ +protected: + //left and right pointers (doubly-connected) + FunnelNode *m_CLeft; + FunnelNode *m_CRight; + //the vertex represented by this funnel node + SrPnt2 m_CPoint; +public: + //default constructor + FunnelNode(); + //constructor - initializes the vertex coordinates + FunnelNode(float x, float y); + //constructor - initializes the vertex + FunnelNode(const SrPnt2& point); + //mutator for the left pointer + void Left(FunnelNode *left); + //mutator for the right pointer + void Right(FunnelNode *right); + //accessor for the left pointer + FunnelNode *Left(); + //accessor for the right pointer + FunnelNode *Right(); + //mutator for the vertex + void Point(const SrPnt2& point); + //accessor for the vertex + SrPnt2 Point(); + //mutator for the vertex coordinates + void Point(float x, float y); + //accessor for the vertex x coordinate + float X(); + //accessor for the vertex y coordinate + float Y(); +}; + +//definition of FunnelNode class } + +//DJD: definition of FunnelDeque class { + +//the funnel deque used in the funnel algorithm +class FunnelDeque +{ +public: + //different kinds of corners - point, left and right tangent + //(tangent to a circle around the corner, running along the side specified) + enum CornerType {Point, LeftTangent, RightTangent}; +protected: + //left end, right end, and apex pointers for the deque + FunnelNode *m_CLeft; + FunnelNode *m_CRight; + FunnelNode *m_CApex; + //the corner type of the node at the apex + //(ones on the left side are all right tangent, ones on the right are all left tangent) + CornerType m_tApexType; + //radius of the unit for which we are finding the path + float m_dRadius; + //angle between the first and second points given their corner types + float Angle(float x_1, float y_1, CornerType type_1, float x_2, float y_2, CornerType type_2); + //constrains an angfle to the range (-PI, PI] + float AngleRange(float theta); + //returns the (Euclidean) distance between two points + float Distance(float x_1, float y_1, float x_2, float y_2); + //returns the angle between the given points + //(same as the angle to their tangents if they're the same type) + float ToTangent(float x_1, float y_1, float x_2, float y_2); + //returns the angle between tangents of the given points + //given whether the first is left tangent type, assuming the second is the opposite + float ToTangentAlt(float x_1, float y_1, float x_2, float y_2, bool LeftRight); + //returns the angle between the first point and a tangent to the second + //(the type of the second is specified) + float PointToTangent(float x_1, float y_1, float x_2, float y_2, bool Right); + //adds a given point to the path + void AddPoint(float x, float y, CornerType type, float angle, SrPolygon &path); +public: + //creates the funnel deque based on a starting point and radius + FunnelDeque(float x, float y, float r); + //copy constructor + FunnelDeque(const SrPnt2& point, float r); + //destructor + ~FunnelDeque(); + //adds a given point of a given type to the funnel deque, adding to the path if necessary + void Add(const SrPnt2& p, CornerType type, SrPolygon& path); + //prints the contents of the funnel deque + void Print(); + //prints the contents of the path + void Print(SrPolygon path); + //calculates the length of the path for a unit of given radius + float Length(SrPolygon path); +}; + +#endif + +//definition of FunnelDeque class } diff --git a/source/dcdt/se/Location.cpp b/source/dcdt/se/Location.cpp new file mode 100644 index 0000000000..42b0eecf36 --- /dev/null +++ b/source/dcdt/se/Location.cpp @@ -0,0 +1,159 @@ +//Location.cpp + +#include "precompiled.h" + +#include +#include +#include "se_dcdt.h" +#include + +//the list of unprocessed triangles +template +SrArray *> SeLinkFace::processing; + +//DJD: Point location function definitions { + +//initializes each sector midpoint to point to the triangle in which it is contained +void SeDcdt::InitializeSectors() +{ +#if defined EXPERIMENT + //count the number of triangles + int num = 0; +#endif + //calculate the width and height of the triangulation + float width = _xmax - _xmin; + float height = _ymax - _ymin; + //calculate the width and height of each sector + sectorWidth = width / (float)xSectors; + sectorHeight = height / (float)ySectors; + //initialize all sectors to null + for (int i = 0; i < ySectors; i++) + { + for (int j = 0; j < xSectors; j++) + { + sectors[i][j] = NULL; + } + } + float x, y; + float x1, y1, x2, y2, x3, y3; + //go through the unprocessed triangles + for (int k = 0; k < SeDcdtFace::Faces(); k++) + { + //retrieve the next valid triangle + SeDcdtFace *currentFace = SeDcdtFace::Face(k); + if (currentFace == NULL) + { + continue; + } +#if defined EXPERIMENT + //keep track of the number of triangles + num++; +#endif + //get the minimum and maximum x and y values + TriangleVertices(currentFace, x1, y1, x2, y2, x3, y3); + float left = Min(x1, x2, x3); + float right = Max(x1, x2, x3); + float top = Min(y1, y2, y3); + float bottom = Max(y1, y2, y3); + //calculate the minimum and maximum x and y sector indices covered + int xIndexMin = (int)((left - _xmin + 0.5f * sectorWidth) / sectorWidth); + int xIndexMax = (int)((right - _xmin - 0.5f * sectorWidth) / sectorWidth); + int yIndexMin = (int)((top - _ymin + 0.5f * sectorHeight) / sectorHeight); + int yIndexMax = (int)((bottom - _ymin - 0.5f * sectorHeight) / sectorHeight); + //go through the covered sector midpoints + for (int i = yIndexMin; i <= yIndexMax; i++) + { + for (int j = xIndexMin; j <= xIndexMax; j++) + { + //calculate the location of the midpoint + x = ((float)j + 0.5f) * sectorWidth + _xmin; + y = ((float)i + 0.5f) * sectorHeight + _ymin; + //if the midpoint is in the current triangle, + if (InTriangle(currentFace, x, y)) + { + //point the sector to that triangle + sectors[i][j] = currentFace; + } + } + } + } +#if defined EXPERIMENT + //if we are running an experiment, output the number of triangles to a data file + std::ofstream outFile("Data.txt", std::ios_base::app); + outFile << "Triangles: " << num << "\t"; + outFile.close(); +#endif +} + +//check if the given point is within a given triangle +bool SeDcdt::InTriangle(SeDcdtFace *face, float x, float y) +{ + //get the triangle's vertices + float x1, y1, x2, y2, x3, y3; + TriangleVertices(face, x1, y1, x2, y2, x3, y3); + //check that the point is counterclockwise of all edges + return ((Orientation(x1, y1, x2, y2, x, y) >= 0) + && (Orientation(x2, y2, x3, y3, x, y) >= 0) + && (Orientation(x3, y3, x1, y1, x, y) >= 0)); +} + +//uses the sectors to locate in which triangle the given point lies +SeTriangulator::LocateResult SeDcdt::LocatePoint(float x, float y, SeBase* &result) +{ + //calculates the indices of the sector in which the point lies + int xIndex = (int)((x - _xmin) / sectorWidth); + int yIndex = (int)((y - _ymin) / sectorHeight); + //makes sure the indices are within the proper bounds + xIndex = (xIndex < 0) ? 0 : (xIndex >= xSectors) ? xSectors - 1 : xIndex; + yIndex = (yIndex < 0) ? 0 : (yIndex >= ySectors) ? ySectors - 1 : yIndex; + //retrieve the triangle in which that sector midpoint is located + SeFace *iniface = sectors[yIndex][xIndex]; + //if it was invalid, start at the usual place + if (iniface == NULL) + { + iniface = _search_face(); + } + //use this starting triangle to start point location + return _triangulator->locate_point(iniface, x, y, result); +} + +//locate the given point starting at the usual place +SeTriangulator::LocateResult SeDcdt::LocatePointOld(float x, float y, SeBase* &result) +{ + return _triangulator->locate_point(_search_face(), x, y, result); +} + +//returns the maximum of the 3 values passed +float SeDcdt::Max(float a, float b, float c) +{ + return ((a >= b) && (a >= c)) ? a : + ((b >= a) && (b >= c)) ? b : c; +} + +//returns the minimum of the 3 values passed +float SeDcdt::Min(float a, float b, float c) +{ + return ((a <= b) && (a <= c)) ? a : + ((b <= a) && (b <= c)) ? b : c; +} + +//return a random value in the range of x values (used for testing) +float SeDcdt::RandomX() +{ + return RandomBetween(_xmin, _xmax); +} + +//return a random value in the range of y values (used for testing) +float SeDcdt::RandomY() +{ + return RandomBetween(_ymin, _ymax); +} + +//returns a random number between the values given +float SeDcdt::RandomBetween(float min, float max) +{ + float base = (float)rand() / (float)RAND_MAX; + return (base * (max - min) + min); +} + +//Point location function definitions } diff --git a/source/dcdt/se/Search.cpp b/source/dcdt/se/Search.cpp new file mode 100644 index 0000000000..257344ddbe --- /dev/null +++ b/source/dcdt/se/Search.cpp @@ -0,0 +1,2328 @@ +//Search.cpp + +//DJD: Abstract space searching function definitions { +#include "precompiled.h" + +#include "se_dcdt.h" + +#include +#include + +//function for comparing SearchNodes +struct check : public std::binary_function +{ + bool operator()(SearchNode *&sn1, SearchNode *&sn2) const + { + return ((sn1->f() > sn2->f()) || ((sn1->f() == sn2->f()) && (sn1->h() > sn2->h())));; + } +}; + +//gets the closest point on an unconstrained edge of the triangle to the point given +//(that is at least r from a vertex) . . . this function can be sped up +SrPnt2 SeDcdt::ClosestPointTo(SeDcdtFace *face, float x, float y, float r) +{ + //if the point is in the triangle + if (InTriangle(face, x, y)) + { + //the closest point is the point itself + SrPnt2 p; + p.set(x, y); + return p; + } + SrPnt2 closestPoint, point; + float closestDistance = INFINITY, distance; + //get the edges of the triangle and go through them + SeBase *s = face->se(); + for (int i = 0; i < 3; i++) + { + //gets the vertices of the triangle + float x1, y1, x2, y2, x3, y3; + TriangleVertices(s, x1, y1, x2, y2, x3, y3); + //doesn't consider blocked edges + if (Blocked(s)) + { + s = s->nxt(); + continue; + } + //checks if the angles along this edge and to the point are accute + bool accute1 = IsAccute(AngleBetween(x1, y1, x2, y2, x, y)); + bool accute2 = IsAccute(AngleBetween(x2, y2, x1, y1, x, y)); + if (accute1 && accute2) + { + //if both are, get closest point on this line + distance = PointLineDistance(x, y, x1, y1, x2, y2); + float theta = atan2(y2 - y1, x2 - x1); + theta += (Orientation(x, y, x1, y1, x2, y2) > 0.0f) ? PI / -2.0f : PI / 2.0f; + theta += (theta <= -PI) ? 2.0f * PI : (theta > PI) ? -2.0f * PI : 0.0f; + point.set(x + cos(theta) * distance, y + sin(theta) * distance); + //if it's < r from either end, move it out to distance r + if (Length(point.x, point.y, x1, y1) < r) + { + accute2 = false; + } + else if (Length(point.x, point.y, x2, y2) < r) + { + accute1 = false; + } + } + if (!accute1 && accute2) + { + //get point distance r from (x2, y2) in the direction of (x1, y1) + float theta = atan2(y1 - y2, x1 - x2); + point.set(x2 + cos(theta) * r, y2 + sin(theta) * r); + } + else if (accute1 && !accute2) + { + //get point distance r from (x1, y1) in the direction of (x2, y2) + float theta = atan2(y2 - y1, x2 - x1); + point.set(x1 + cos(theta) * r, y1 + sin(theta) * r); + } + //measure the distance between this point and the one given + distance = Length(point.x, point.y, x, y); + //if this point is closer than others, + if (distance < closestDistance) + { + //set it as the closest point + closestPoint.set(point.x, point.y); + closestDistance = distance; + } + //move to the next edge + s = s->nxt(); + } + //return the closest point found + return closestPoint; +} + +//walks from the source triangle to the destination triangle +//provided the destination triangle is listed as adjacent in the source triangle +//direction is to remove ambiguity when both endpoints of a corridor are the same +bool SeDcdt::WalkBetween(SrArray& path, SeDcdtFace *sourceFace, SeDcdtFace *destinationFace, float r, int direction) +{ + SeDcdtFace *last = NULL; + //go through all faces between the source and destination faces + while (true) + { + //if we have reached the destination face, + if (sourceFace == destinationFace) + { + //add it to the path and exit successfully + path.push() = destinationFace->se(); + return true; + } + //get the elements of the current triangle + SeBase *s = sourceFace->se(); + //go through the edges of the triangle + for (int i = 0; i < 3; i++) + { + //if the destination face is across the current edge (or the direction tells us which way to start), + if ((i == direction) || ((direction == INVALID) + && (sourceFace->link->Adjacent(i) == destinationFace) && (s->sym()->fac() != last))) + { + last = sourceFace; + direction = INVALID; + //make sure the path is wide enough + if (sourceFace->link->Choke(i) < 2.0f * r) + { + //if not, empty the path and return failure + path.size(0); + return false; + } + else + { + //if so, add the current face to the path + path.push() = sourceFace->se(); + //and move to the next one + sourceFace = (SeDcdtFace *)s->sym()->fac(); + break; + } + } + //move to the next edge in the current triangle + s = s->nxt(); + } + } +} + +//checks if a unit of radius r can go through the second triangle between the first and the third +bool SeDcdt::CanCross(SeDcdtFace *middle, SeBase *end1, SeBase *end2, float r) +{ + SeBase *s = middle->se(); + //and goes through its edges + for (int i = 0; i < 3; i++) + { + //if it is neither the first nor the third + if ((s->sym()->fac() != end1->fac()) && (s->sym()->fac() != end2->fac())) + { + //return if the width between the first and the third is large enough for radius r + return (middle->link->Width((i + 1) % 3) >= 2.0f * r); + } + //move to the next edge + s = s->nxt(); + } + sr_out.warning("ERROR: could not find desired face (should never happen)\n"); + return false; +} + +//checks if a unit of radius r can go through the second triangle between the first and the third +bool SeDcdt::CanCross(SeBase *first, SeBase *second, SeBase *third, float r) +{ + //and goes through its edges + for (int i = 0; i < 3; i++) + { + //gets the face across the current edge + SeFace *current = second->sym()->fac(); + //if it is neither the first nor the third + if ((current != first->fac()) && (current != third->fac())) + { + //return if the width between the first and the third is large enough for radius r + return (((SeDcdtFace *)second)->link->Width((i + 1) % 3) >= 2.0f * r); + } + //move to the next edge + second = second->nxt(); + } + sr_out.warning("ERROR: could not find desired face (should never happen)\n"); + return false; +} + +//finds a path in the degree-1 tree with both the start and goal in it +#if defined EXPERIMENT +bool SeDcdt::Degree1Path(SrArray& path, float x1, float y1, SeDcdtFace *startFace, + float x2, float y2, SeDcdtFace *goalFace, float r, int &nodes) +#else +bool SeDcdt::Degree1Path(SrArray& path, float x1, float y1, SeDcdtFace *startFace, + float x2, float y2, SeDcdtFace *goalFace, float r) +#endif +{ + //starts marking the mesh + _mesh->begin_marking(); + //create a priority queue for searching the tree + std::priority_queue, check> pq; + float x, y; + //each face is represented by its midpoint (since only one path must exist) + TriangleMidpoint(startFace, x, y); + SrPnt2 p; + p.set(x, y); + //creates the first search node + SearchNode *current = new SearchNode(Length(x, y, x1, y1), Length(x, y, x2, y2), p, startFace, NULL); + //put it in the queue to be searched + pq.push(current); + //continues until the tree has been exhausted or a path has been found + while (!pq.empty()) + { +#if defined EXPERIMENT + nodes++; +#endif + //gets the first node to search + current = pq.top(); + pq.pop(); + //checks if that node is the goal + if (InTriangle(current->Triangle(), x2, y2)) + { + while (!pq.empty()) + { + pq.top()->Close(); + delete pq.top(); + pq.pop(); + } + SearchNode *temp = current; + //if so, visits all the triangles between the start and the goal + while (current != NULL) + { + //and adds each to the path + path.push() = current->Triangle()->se(); + //moves to the previous triangle + current = current->Back(); + } + temp->Close(); + delete temp; + //since they were visited in reverse, flip the path around + path.revert(); + path.pop(); + //stops marking the mesh + _mesh->end_marking(); + //makes sure the channel is wide enough for the unit + if (ValidPath(path, x1, y1, x2, y2, r)) + { + return true; + } + else + { + //if not, empties the paths and returns failure + path.size(0); + return false; + } + } + //gets the triangle associated with the current node + SeDcdtFace *currentFace = current->Triangle(); + //marks it as visited + _mesh->mark(currentFace); + //get the elements of the triangle + SeBase *s = currentFace->se(); + //go through the edges + for (int i = 0; i < 3; i++) + { + //gets the face on the opposite side of this edge + SeDcdtFace *tempFace = (SeDcdtFace *)s->sym()->fac(); + //only consider it if it's degree-1 and hasn't been visited before + if ((!_mesh->marked(tempFace)) && (tempFace->link->Degree() == 1) && (!Blocked(s))) + { + //get the new triangle's midpoint + TriangleMidpoint(tempFace, x, y); + p.set(x, y); + //create a search node corresponding to this triangle + SearchNode *temp = new SearchNode(current->g() + Length(current->Point().x, current->Point().y, x, y), + Length(x, y, x2, y2), p, tempFace, current); + //and add it to the search queue + pq.push(temp); + current->OpenChild(); + } + //move to the next edge + s = s->nxt(); + } + //if no children were created, close and delete this node + if (current->OpenChildren() <= 0) + { + current->Close(); + delete current; + } + } + //stop marking the mesh + _mesh->end_marking(); + //the tree was exhausted and a path not found - return failure + return false; +} + +//follows a degree-2 ring from one node to another, making sure that each is wide enough for the given radius +bool SeDcdt::Degree2Path(SrArray& path, SeDcdtFace *startFace, SeDcdtFace *nextFace, + SeDcdtFace *goalFace, float r) +{ + //empties the path first + path.size(0); + //and adds the first face to it + path.push() = startFace->se(); + //goes until it has reached the goal triangle + while (nextFace != goalFace) + { + //gets the elements of the current triangle + SeBase *s = nextFace->se(); + //goes through its edges + for (int i = 0; i < 3; i++) + { + //when the face across the current edge is the last triangle, + if (s->sym()->fac() == startFace) + { + //check if the triangle across the next edge is the next triangle in the ring + if (nextFace->link->Angle((i + 1) % 3) == INFINITY) + { + //makes sure this triangle is wide enough for radius r + if (nextFace->link->Width(i) < 2.0f * r) + { + //if not, empties the path and returns failure + path.size(0); + return false; + } + //otherwise, adds the current triangle to the path + path.push() = nextFace->se(); + //and moves to the next triangle in the ring + startFace = nextFace; + nextFace = (SeDcdtFace *)s->nxt()->sym()->fac(); + } + else + { + //similar to above, checks the width + if (nextFace->link->Width((i + 2) % 3) < 2.0f * r) + { + path.size(0); + return false; + } + //and continues along the ring + path.push() = nextFace->se(); + startFace = nextFace; + nextFace = (SeDcdtFace *)s->nxt()->nxt()->sym()->fac(); + } + //process the next triangle in the ring + break; + } + //move to the next edge + s = s->nxt(); + } + } + //return that a path was found successfully + return true; +} + +//follows a degree-2 ring from one node to another, making sure that each is wide enough for the given radius +bool SeDcdt::FollowLoop(SrArray& path, SeDcdtFace *startFace, SeDcdtFace *nextFace, + SeDcdtFace *goalFace, SeDcdtFace *degree3, float r) +{ + //empties the path first + path.size(0); + //and adds the first face to it + path.push() = startFace->se(); + //goes until it has reached the goal triangle + while (nextFace != goalFace) + { + //gets the elements of the current triangle + SeBase *s = nextFace->se(); + //goes through its edges + for (int i = 0; i < 3; i++) + { + //when the face across the current edge is the last triangle, + if (s->sym()->fac() == startFace) + { + //check if the triangle across the next edge is the next triangle in the ring + if (nextFace->link->Adjacent((i + 1) % 3) == degree3) + { + //makes sure this triangle is wide enough for radius r + if (nextFace->link->Width(i) < 2.0f * r) + { + //if not, empties the path and returns failure + path.size(0); + return false; + } + //otherwise, adds the current triangle to the path + path.push() = nextFace->se(); + //and moves to the next triangle in the ring + startFace = nextFace; + nextFace = (SeDcdtFace *)s->nxt()->sym()->fac(); + } + else + { + //similar to above, checks the width + if (nextFace->link->Width((i + 2) % 3) < 2.0f * r) + { + path.size(0); + return false; + } + //and continues along the ring + path.push() = nextFace->se(); + startFace = nextFace; + nextFace = (SeDcdtFace *)s->nxt()->nxt()->sym()->fac(); + } + //process the next triangle in the ring + break; + } + //move to the next edge + s = s->nxt(); + } + } + //return that a path was found successfully + return true; +} + +//determines if the given endpoint has to be checked against the current triangle +bool SeDcdt::ValidEndpoint(SeBase *s, float x, float y, bool left) +{ + //gets the vertices of the current triangle + float x1, y1, x2, y2, x3, y3, x4, y4; + TriangleVertices(s, x1, y1, x2, y2, x3, y3); + //checks if a base angle is obtuse + if ((abs(AngleBetween(x1, y1, x2, y2, x3, y3)) >= PI / 2.0f) + || (abs(AngleBetween(x1, y1, x3, y3, x2, y2)) >= PI / 2.0f)) + { + //if so, we don't have to worry + return true; + } + //gets the angle of the opposite edge + float theta = atan2(y3 - y2, x3 - x2); + //turn 90 degrees clockwise + theta += (theta <= PI / -2.0f) ? 3.0f * PI / 2.0f : PI / -2.0f; + //create a point this angle from the "top" vertex + x4 = x1 + cos(theta); + y4 = y1 + sin(theta); + //returns whether the unit doesn't have to pass through the narrowest point in this triangle + return ((Orientation(x1, y1, x4, y4, x, y) > 0) == (left)); +} + +//checks whether there is a valid path from the start or goal through their corresponding triangles +bool SeDcdt::ValidEndpoint(SeDcdtFace *first, SeDcdtFace *second, float x, float y, float r) +{ + //goes through the edges of the first triangle + SeBase *s = first->se(); + for (int i = 0; i < 3; i++) + { + //when it finds the one between the first and second triangles, + if (s->sym()->fac() == second) + { + //if it has to cross the line extending from either end vertex, checks their width + return (((ValidEndpoint(s, x, y, false)) || (first->link->Width((i + 2) % 3) >= 2.0f * r)) + && ((ValidEndpoint(s->nxt(), x, y, true)) || (first->link->Width(i) >= 2.0f * r))); + } + //moves to the next edge + s = s->nxt(); + } + //if the second triangle was never found, return failure + return false; +} + +//checks if the start and goal points can validly enter the path +bool SeDcdt::ValidEndpoints(SrArray& path, float x1, float y1, float x2, float y2, float r) +{ + //if the path is size 1, no problem, otherwise checks that it can go from the first to second triangle, + //and from the last to second-last triangle + return ((path.size() < 2) || ((ValidEndpoint((SeDcdtFace *)path[0]->fac(), (SeDcdtFace *)path[1]->fac(), x1, y1, r)) + && (ValidEndpoint((SeDcdtFace *)path[path.size() - 1]->fac(), (SeDcdtFace *)path[path.size() - 2]->fac(), x2, y2, r)))); +} + +//checks if a path is valid for a unit of radius r +bool SeDcdt::ValidPath(SrArray& path, float x1, float y1, float x2, float y2, float r) +{ + //first checks if the endpoints are valid + if (!ValidEndpoints(path, x1, y1, x2, y2, r)) + { + return false; + } + //then goes through the interior points + for (int i = 1; i < path.size() - 1; i++) + { + //gets the elements of the current triangle in the path + SeBase *s = path[i]->fac()->se(); + //goes through the edges in this triangle + for (int j = 0; j < 3; j++) + { + //find the edge across which is the next triangle in the path + if (s->sym()->fac() == path[i + 1]->fac()) + { + //get the width through this triangle between the last one in the path and the next one + float width = ((SeDcdtFace *)s->fac())->link->Width( + (s->nxt()->sym()->fac() == path[i - 1]->fac()) ? j : ((j + 2) % 3)); + //if it's less than the diameter of the unit, the path is invalid + if (width < 2.0f * r) + { + return false; + } + break; + } + //moves to the next edge in the triangle + s = s->nxt(); + } + } + //if no triangle in the path was invalid, the path is valid + return true; +} + +//determines the triangles that make up the path between the start and goal, +//using the degree-3 nodes speficied in the goal node and it's back pointer chain +//direction is to remove ambiguity when a corridor's endpoints are the same +void SeDcdt::ConstructBasePath(SrArray &path, SearchNode *goalNode, SeDcdtFace *start, SeDcdtFace *goal, int direction) +{ + //construct the backwards portion of the path first + SrArray backwardsPath; + SeDcdtFace *goal2; + //if the goal triangle is degree-1, + if (goal->link->Degree() == 1) + { + //go through its edges and find the one across which is the root of the tree + for (int i = 0; i < 3; i++) + { + if (goal->link->Adjacent(i) != NULL) + { + //then walk to the root from here + WalkBetween(backwardsPath, goal, goal->link->Adjacent(i), 0.0f); + //get rid of the overlapping triangle + backwardsPath.pop(); + //record the new position in the path + goal2 = goal->link->Adjacent(i); + break; + } + } + } + else + { + //otherwise, we start from a degree-2 or 3 node + goal2 = goal; + } + //if we are at a degree-2 node, + if (goal->link->Degree() < 3) + { + //walk from there to the last degree-3 node + WalkBetween(backwardsPath, goal2, goalNode->Triangle(), 0.0f, direction); + //get rid of the overlapping triangle + backwardsPath.pop(); + } + //follow the chain of back pointers to the start + while (goalNode->Back() != NULL) + { + //create the path between the search node triangles + SrArray tempPath; + WalkBetween(tempPath, goalNode->Back()->Triangle(), goalNode->Triangle(), 0.0f, goalNode->Direction()); + //insert it into the backwards path + while (!tempPath.empty()) + { + backwardsPath.push() = tempPath.pop(); + } + //get rid of the overlapping triangle + backwardsPath.pop(); + //move back in the chain + goalNode = goalNode->Back(); + } + SeDcdtFace *start2; + //if the start triangle is degree-1, + if (start->link->Degree() == 1) + { + //find the edge across which is the root of the tree + for (int i = 0; i < 3; i++) + { + if (start->link->Adjacent(i) != NULL) + { + //walk from the start triangle to the root + WalkBetween(path, start, start->link->Adjacent(i), 0.0f); + //get rif of the overlapping triangle + path.pop(); + //now we work from the root of the tree + start2 = start->link->Adjacent(i); + break; + } + } + } + else + { + //otherwise, the start triangle is degree-2 or 3 + start2 = start; + } + //if we are at a degree-2 node, + if (start->link->Degree() < 3) + { + //add the path from there to the first degree-3 node + WalkBetween(path, start2, goalNode->Triangle(), 0.0f, goalNode->Direction()); + } + //put the path created so far onto this path + while (!backwardsPath.empty()) + { + path.push() = backwardsPath.pop(); + } + //get rid of the extra triangle + path.pop(); +} + +//searches the abstract graph for a path between two points +#if defined EXPERIMENT +bool SeDcdt::SearchPathFast(float x1, float y1, float x2, float y2, float r, Data *data, int numData)//, float &length, float &time, int &nodes) +#else +bool SeDcdt::SearchPathFast(float x1, float y1, float x2, float y2, float r)//, float &length, float &time, int &nodes) +#endif +{ +#if defined EXPERIMENT + //start timing the search + Timer startTime; + //start counting the nodes searched + int nodes = 0; + //also time the total time constructing channel and running the modified funnel algorithm + float constructionTime = 0.0f; + //count the number of triangles visited on the low level + int triangles = 0; + //count the total number of paths found + int paths = 0; + //keep track of the current data point + int currentData = 0; +#endif + //set the start and goal points + startPoint.set(x1, y1); + goalPoint.set(x2, y2); + //initialize the path to empty + currentPath.size(0); + currentChannel.size(0); + SeBase *start, *goal; + //find the triangle containing the start point + SeTriangulator::LocateResult startResult = LocatePoint(x1, y1, start); + //if it wasn't found, return a path could not be found + if (startResult == SeTriangulator::LocateResult::NotFound) + { + return false; + } + //find the triangle containing the goal point + SeTriangulator::LocateResult goalResult = LocatePoint(x2, y2, goal); + //if it couldn't, return a failure + if (goalResult == SeTriangulator::LocateResult::NotFound) + { + return false; + } + SeDcdtFace *startFace = (SeDcdtFace *)start->fac(); + SeDcdtFace *goalFace = (SeDcdtFace *)goal->fac(); + //if either triangle isn't abstracted, this search fails + //also if the triangles aren't in the same connected component, no path connecting them exists + if ((startFace->link == NULL) || (goalFace->link == NULL) + || (startFace->link->Component() != goalFace->link->Component())) + { + return false; + } + //check that the start and goal points aren't within the radius of any constraints + if ((!ValidPosition(x1, y1, startFace, r)) || (!ValidPosition(x2, y2, goalFace, r))) + { + //if either are, return false + return false; + } + //if the start and goal are in the same triangle, + if (startFace == goalFace) + { + //the path is a straight line + currentPath.push().set(x1, y1); + currentPath.push().set(x2, y2); +#if defined EXPERIMENT + //stop timing + data[0].TotalTime = startTime.GetDuration(); + //record the length of the path + data[0].Length = Length(x1, y1, x2, y2); +#endif + return true; + } + //if both start and goal faces are degree-1, + if ((startFace->link->Degree() == 1) && (goalFace->link->Degree() == 1)) + { + //checks if the start (and thus the goal as well) is in a tree component + bool inTree = true; + //(true if there are no adjacent degree-2 nodes) + for (int i = 0; i < 3; i++) + { + if (startFace->link->Adjacent(i) != NULL) + { + inTree = false; + } + } + //if they are in a tree component, + if (inTree) + { + //perform search in it + SrArray channel; +#if defined EXPERIMENT + if (Degree1Path(channel, x1, y1, startFace, x2, y2, goalFace, r, triangles)) +#else + if (Degree1Path(channel, x1, y1, startFace, x2, y2, goalFace, r)) +#endif + { + //if the search was successful, calculate the shortest path + currentChannel = channel; +#if defined EXPERIMENT + //start timing the funnel algorithm + Timer construction; + //run the funnel algorithm + data[0].Length = GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); + //record the construction time + data[0].ConstructionTime = construction.GetDuration(); + //and the number of triangles visited + data[0].ConstructionNodes = triangles; + //and finally the total time of the algorithm + data[0].TotalTime = startTime.GetDuration(); +#else + //run the modified funnel algorithm to get the shortest path in that channel + GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); +#endif + //and return success + return true; + } + else + { + //otherwise return failure + return false; + } + } + } + //if the start face is degree-1, moves to the adjacent degree-2 node + SeDcdtFace *startFace2; + float startAngle; + float startChoke; + if (startFace->link->Degree() == 1) + { + //go through the edges + for (int i = 0; i < 3; i++) + { + //determines which one is in the direction of the root of the tree + SeDcdtFace *adjacent = startFace->link->Adjacent(i); + if (adjacent != NULL) + { + //and records the root and the angle and choke point on the way there + startFace2 = adjacent; + startAngle = startFace->link->Angle(i); + startChoke = startFace->link->Choke(i); + break; + } + } + } + else + { + //if we are at a degree-2 or 3 node, don't change anything + startFace2 = startFace; + startAngle = 0.0f; + startChoke = INFINITY; + } + //if the goal face is degree-1, moves to the adjacent degree-2 node + SeDcdtFace *goalFace2; + float goalAngle; + float goalChoke; + if (goalFace->link->Degree() == 1) + { + //go through the edges + for (int i = 0; i < 3; i++) + { + //determines which one is in the direction of the root of the tree + SeDcdtFace *adjacent = goalFace->link->Adjacent(i); + if (adjacent != NULL) + { + //and records the root and the angle and choke point on the way there + goalFace2 = adjacent; + goalAngle = goalFace->link->Angle(i); + goalChoke = goalFace->link->Choke(i); + break; + } + } + } + else + { + //if we are at a degree-2 or 3 node, don't change anything + goalFace2 = goalFace; + goalAngle = 0.0f; + goalChoke = INFINITY; + } + //if the start and goal faces are both degree-1 or 2, + if ((startFace->link->Degree() < 3) && (goalFace->link->Degree() < 3)) + { + //checks if the start and goal have the same root, + if (startFace2 == goalFace2) + { + //if the start face is at the root of the tree that the goal face is in, + if (startFace2 == startFace) + { +#if defined EXPERIMENT + //start timing the channel construction and modified funnel algorithm + Timer construction; +#endif + SrArray channel; + //walk from the goal face to the start face and check that the endpoints are valid + if (WalkBetween(channel, goalFace, startFace, r) + && ValidEndpoints(channel, x1, y1, x2, y2, r)) + { +#if defined EXPERIMENT + //keep track of the number of triangles visited + triangles += channel.size(); +#endif + //invert the path and get rid of the extra triangle + channel.revert(); + channel.pop(); +#if defined EXPERIMENT + //record the length of the path found + data[0].Length = GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); + //keep track of the current channel + currentChannel = channel; + //stop timing and record the construction + data[0].ConstructionTime = construction.GetDuration(); + //record the number of triangles visited + data[0].ConstructionNodes = triangles; + //record the total time of the algorithm + data[0].TotalTime = startTime.GetDuration(); +#else + //run the modified funnel algorithm + GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); +#endif + //and return success + return true; + } + else + { + //if the path was not wide enough, return failure + return false; + } + } + //if the goal face is at the root of the tree that the start face is in, + else if (goalFace2 == goalFace) + { +#if defined EXPERIMENT + //start timing channel construction and the modified funnel algorithm + Timer construction; +#endif + SrArray channel; + //walk from the start face to the goal face and check that the endpoints are valid + if (WalkBetween(channel, startFace, goalFace, r) + && ValidEndpoints(channel, x1, y1, x2, y2, r)) + { +#if defined EXPERIMENT + //keep track of the number of triangles visited + triangles += channel.size(); +#endif + //get rid of the extra triangle + channel.pop(); + //calculate the shortest path and return success + currentChannel = channel; +#if defined EXPERIMENT + //record the length of the path + data[0].Length = GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); + //and the construction time + data[0].ConstructionTime = construction.GetDuration(); + //and the number of triangles visited + data[0].ConstructionNodes = triangles; + //and finally the total time of the algorithm + data[0].TotalTime = startTime.GetDuration(); +#else + //run the modified funnel algorithm on the channel constructed + GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); +#endif + //and return success + return true; + } + else + { + //if the path was not wide enough, return failure + return false; + } + } + //if the start and goal are both in the same tree component, + else + { + SrArray channel; + //perform search in tree component and check that the endpoints are valid +#if defined EXPERIMENT + if (Degree1Path(channel, x1, y1, startFace, x2, y2, goalFace, r, triangles) +#else + if (Degree1Path(channel, x1, y1, startFace, x2, y2, goalFace, r) +#endif + && ValidEndpoints(channel, x1, y1, x2, y2, r)) + { + //record the current channel + currentChannel = channel; +#if defined EXPERIMENT + //start timing the construction + Timer construction; + //if the search was successful, calculate the shortest path + data[0].Length = GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); + //and record the time to find it + data[0].ConstructionTime = construction.GetDuration(); + //and the triangles visited + data[0].ConstructionNodes = triangles; + //and the time of the whole algorithm + data[0].TotalTime = startTime.GetDuration(); +#else + //calculate the shortest path in the channel + GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); +#endif + //and return success + return true; + } + else + { + //otherwise, return failure + return false; + } + } + } + } + //if we can't get from the start or goal to the rest of the graph, no path exists + if ((startChoke < 2.0f * r) || (goalChoke < 2.0f * r)) + { + return false; + } + //if both the start and goal are in degree-1 or 2 nodes, check if they're on a ring + if ((startFace2->link->Degree() == 2) && (goalFace2->link->Degree() == 2)) + { + int i; + //go through the edges of the degree-2 node attached to the start face + for (i = 0; i < 3; i++) + { + //if it's adjacent to a degree-3 node, it's not a ring + if (startFace2->link->Adjacent(i) != NULL) + { + break; + } + } + //if it wasn't, the start and goal are both rooted on a degree-2 ring + if (i >= 3) + { + //create paths between the start and goal triangles and their respective roots, + //and paths for the left and right sides of the ring + SrArray startPath, goalPath, leftPath, rightPath; + //if the start triangle is the root of its tree, there is no start path + if (startFace == startFace2) + { + startPath.size(0); + startPath.push() = startFace->se(); + } + //otherwise, walk from the start to the root of its tree + else if (!WalkBetween(startPath, startFace, startFace2, r)) + { + //if the path wasn't wide enough, return failure + return false; + } + //if the goal triangle is the root of its tree, there is no goal path + if (goalFace == goalFace2) + { + goalPath.size(0); + goalPath.push() = goalFace->se(); + } + //otherwise, walk from the goal to the root of its tree + else if (!WalkBetween(goalPath, goalFace, goalFace2, r)) + { + //if the path wasn't wide enough, return failure + return false; + } + //invert the goal path since it was constructed backward + goalPath.revert(); + //go through the edges of the root of the start triangle + SeBase *s = startFace2->se(); + bool leftValid, rightValid; + for (int i = 0; i < 3; i++) + { + //when we find the edge across which there is no degree-3 node, + if (startFace2->link->Angle(i) == INVALID) + { + //constructs the right and left side paths and determines if each is wide enough + rightValid = Degree2Path(rightPath, startFace2, (SeDcdtFace *)s->nxt()->sym()->fac(), goalFace2, r); + leftValid = Degree2Path(leftPath, startFace2, (SeDcdtFace *)s->nxt()->nxt()->sym()->fac(), goalFace2, r); + break; + } + //go to the next edge + s = s->nxt(); + } + //if neither left or right paths are valid, return failure + if (!leftValid && !rightValid) + { + return false; + } + //creates two full paths + SrArray fullLeftPath, fullRightPath; + fullLeftPath.size(0); + fullRightPath.size(0); + startPath.pop(); + goalPath.pop(); + //checks if the whole left path is valid, if so, constructs it + if (leftValid && ((startPath.size() < 1) || (leftPath.size() < 1) || + CanCross(startFace2, startPath[startPath.size() - 1], (leftPath.size() < 2) ? goalFace2->se() : leftPath[1], r)) + && ((goalPath.size() < 2) || (leftPath.size() < 1) || + CanCross(goalFace2, (goalPath.size() < 2) ? goalFace->se() : goalPath[1], leftPath[leftPath.size() - 1], r))) + { + //adds the start path + for (int i = 0; i < startPath.size(); i++) + { + fullLeftPath.push() = startPath[i]; + } + //then the left side of the ring + for (int i = 0; i < leftPath.size(); i++) + { + fullLeftPath.push() = leftPath[i]; + } + //and finally, the goal path + for (int i = 0; i < goalPath.size(); i++) + { + fullLeftPath.push() = goalPath[i]; + } + } + //checks if the whole right path is valid, if so, constructs it + if (rightValid && ((startPath.size() < 1) || (rightPath.size() < 1) || + CanCross(startFace2, startPath[startPath.size() - 1], (rightPath.size() < 2) ? goalFace2->se() : rightPath[1], r)) + && ((goalPath.size() < 1) || (rightPath.size() < 1) || + CanCross(goalFace2, (goalPath.size() < 2) ? goalFace->se() : goalPath[1], rightPath[rightPath.size() - 1], r))) + { + //adds the start path + for (int i = 0; i < startPath.size(); i++) + { + fullRightPath.push() = startPath[i]; + } + //then the right side of the ring + for (int i = 0; i < rightPath.size(); i++) + { + fullRightPath.push() = rightPath[i]; + } + //and finally, the goal path + for (int i = 0; i < goalPath.size(); i++) + { + fullRightPath.push() = goalPath[i]; + } + } + //if neither path was valid, return failure + if ((fullLeftPath.size() == 0) && (fullRightPath.size() == 0)) + { + return false; + } + //if both paths are valid, + else if ((fullLeftPath.size() > 0) && (fullRightPath.size() > 0)) + { +#if defined EXPERIMENT + //start timing the construction + Timer construction; +#endif + //calculate the lengths of both + SrPolygon funnelLeft, funnelRight; + float leftLength = GetShortestPath(funnelLeft, fullLeftPath, x1, y1, x2, y2, r); + float rightLength = GetShortestPath(funnelRight, fullRightPath, x1, y1, x2, y2, r); +#if defined EXPERIMENT + //record the construction time + data[0].ConstructionTime = construction.GetDuration(); +#endif + //set the current path to the shorter of the two and return success + currentPath = (leftLength < rightLength) ? funnelLeft : funnelRight; + currentChannel = (leftLength < rightLength) ? fullLeftPath : fullRightPath; +#if defined EXPERIMENT + //record the length of the shorter path found + data[0].Length = (leftLength < rightLength) ? leftLength : rightLength; + //record the triangles visited + data[0].ConstructionNodes = startPath.size() + goalPath.size() + leftPath.size() + rightPath.size() + 2; + //stop timing the algorithm + data[0].TotalTime = startTime.GetDuration(); +#endif + //and return success + return true; + } + //if only one was valid, + else + { +#if defined EXPERIMENT + //start timing construction + Timer construction; + //calculate the length of valid path + data[0].Length = GetShortestPath(currentPath, (fullRightPath.size() == 0) ? fullLeftPath : fullRightPath, x1, y1, x2, y2, r); + //record the construction time + data[0].ConstructionTime = construction.GetDuration(); + //total up the triangles used in construction + data[0].ConstructionNodes = startPath.size() + goalPath.size() + leftPath.size() + rightPath.size() + 2; + //finish timing the algorithm + data[0].TotalTime = startTime.GetDuration(); +#else + //calculate the valid path + GetShortestPath(currentPath, (fullRightPath.size() == 0) ? fullLeftPath : fullRightPath, x1, y1, x2, y2, r); +#endif + //record the current channel + currentChannel = (fullRightPath.size() == 0) ? fullLeftPath : fullRightPath; + //and return success + return true; + } + } + } + //the end points of the edge the start triangle is on + SeDcdtFace *startTriangle1 = NULL, *startTriangle2 = NULL; + //a priority queue for searching the abstract space + std::priority_queue, check> q; + //if the start triangle is degree-3, + if (startFace2->link->Degree() == 3) + { + //enqueue a node corresponding to that triangle into the queue + SrPnt2 p = ClosestPointTo(startFace2, x2, y2, r); + q.push(new SearchNode(0.0f, Length(x1, y1, x2, y2), p, startFace2, NULL)); + } + else + { + //otherwise, go through the edges of the degree-2 node + SeBase *s = startFace2->se(); + for (int i = 0; i < 3; i++) + { + //get the adjacent triangle across that edge + SeDcdtFace *face = startFace2->link->Adjacent(i); + if (face == NULL) + { + s = s->nxt(); + continue; + } + //if it's not null, assign it to one of the end points + ((startTriangle1 == NULL) ? startTriangle1 : startTriangle2) = face; + float x_1, y_1, x_2, y_2, x_3, y_3; + SeBase *s1 = face->se(); + for (int j = 0; j < 3; j++) + { + if ((face->link->Adjacent(j) == startFace2->link->Adjacent((i + 1) % 3)) + || (face->link->Adjacent(j) == startFace2->link->Adjacent((i + 2) % 3))) + { + TriangleVertices(s1, x_1, y_1, x_2, y_2, x_3, y_3); + break; + } + s1 = s1->nxt(); + } + //get the closest point in this triangle to the start and goal, + SrPnt2 p = ClosestPointOn(x_1, y_1, x_2, y_2, x2, y2, r); + SrPnt2 p2 = ClosestPointOn(x_1, y_1, x_2, y_2, x1, y1, r); + //and the closest point to the start from the degree-2 start triangle + SrPnt2 p3 = ClosestPointTo(startFace2, x1, y1, r); + //gets the longer of the shortest paths through the triangles from the start to its root + //and the distance between the start and the closest point in its root triangle + float distance1 = Maximum(startAngle * r, Length(x1, y1, p3.x, p3.y)); + //gets the shortest path through the triangles between the root and this adjacent degree-3 node + float distance2 = startFace2->link->Angle(i) * r; + TriangleVertices(s, x_1, y_1, x_2, y_2, x_3, y_3); + //gets the angle through the triangle between the adjacent degree-3 node and the tree + float theta = (startFace2->link->Adjacent((i + 1) % 3) == NULL) ? + abs(AngleBetween(x_1, y_1, x_2, y_2, x_3, y_3)) : abs(AngleBetween(x_2, y_2, x_1, y_1, x_3, y_3)); + //adds a search node corresponding to this adjacent degree-3 node to the priority queue + q.push(new SearchNode(Maximum(Length(p2.x, p2.y, x1, y1), distance1 + distance2 + theta * r), + Length(p.x, p.y, x2, y2), p, face, NULL, i)); + //moves to the next edge + s = s->nxt(); + } + } + //creates an array of goal nodes + SrArray goalNodes; + //if the goal is on a degree-3 node, + if (goalFace2->link->Degree() == 3) + { + //there is only one goal node to consider + SrPnt2 p = ClosestPointTo(goalFace2, x1, y1, r); + goalNodes.push() = SearchNode(Length(p.x, p.y, x1, y1), 0.0f, p, goalFace2, NULL); + } + else + { + //otherwise, go through the edges of the triangle at the root of the goal node + for (int i = 0; i < 3; i++) + { + //get the degree-3 node adjacent across this edge + SeDcdtFace *face = goalFace2->link->Adjacent(i); + if (face == NULL) + { + continue; + } + //get the closest points in the triangle to the start and the goal + SrPnt2 p = ClosestPointTo(face, x2, y2, r); + SrPnt2 p2 = ClosestPointTo(face, x1, y1, r); + //enqueue a search node corresponding with this adjacent degree-3 node + goalNodes.push(SearchNode(Length(p2.x, p2.y, x1, y1), + Length(p.x, p.y, x2, y2), p, face, NULL, i)); + } + } + //start with no shortest path + float shortestPathLength = INFINITY; + SrPolygon shortestPath; + SrArray shortestChannel; + //if both the start and goal are on degree-1 or 2 nodes, check if they have the same endpoints + if ((q.size() == 2) && (goalNodes.size() == 2) && + (((startTriangle1 == goalNodes[0].Triangle()) && (startTriangle2 == goalNodes[1].Triangle())) || + ((startTriangle1 == goalNodes[1].Triangle()) && (startTriangle2 == goalNodes[0].Triangle())))) + { + if (startTriangle1 == startTriangle2) + { + //create paths between the start and goal triangles and their respective roots, + //and paths for the left and right sides of the ring + SrArray startPath, goalPath, leftPath, rightPath; + //if the start triangle is the root of its tree, there is no start path + if (startFace == startFace2) + { + startPath.size(0); + startPath.push() = startFace->se(); + } + //otherwise, walk from the start to the root of its tree + else if (!WalkBetween(startPath, startFace, startFace2, r)) + { + //if the path wasn't wide enough, return failure + return false; + } + //if the goal triangle is the root of its tree, there is no goal path + if (goalFace == goalFace2) + { + goalPath.size(0); + goalPath.push() = goalFace->se(); + } + //otherwise, walk from the goal to the root of its tree + else if (!WalkBetween(goalPath, goalFace, goalFace2, r)) + { + //if the path wasn't wide enough, return failure + return false; + } + //invert the goal path since it was constructed backward + goalPath.revert(); + //go through the edges of the root of the start triangle + SeBase *s = startFace2->se(); + bool leftValid, rightValid; + for (int i = 0; i < 3; i++) + { + //when we find the edge across which there is no degree-3 node, + if (startFace2->link->Angle(i) == INVALID) + { + //constructs the right and left side paths and determines if each is wide enough + rightValid = FollowLoop(rightPath, startFace2, (SeDcdtFace *)s->nxt()->sym()->fac(), goalFace2, startTriangle1, r); + leftValid = FollowLoop(leftPath, startFace2, (SeDcdtFace *)s->nxt()->nxt()->sym()->fac(), goalFace2, startTriangle1, r); + break; + } + //go to the next edge + s = s->nxt(); + } + //if neither left or right paths are valid, return failure + if (!leftValid && !rightValid) + { + return false; + } + //creates two full paths + SrArray fullLeftPath, fullRightPath; + fullLeftPath.size(0); + fullRightPath.size(0); + startPath.pop(); + goalPath.pop(); + //checks if the whole left path is valid, if so, constructs it + if (leftValid && ((startPath.size() < 1) || (leftPath.size() < 1) || + CanCross(startFace2, startPath[startPath.size() - 1], (leftPath.size() < 2) ? goalFace2->se() : leftPath[1], r)) + && ((goalPath.size() < 2) || (leftPath.size() < 1) || + CanCross(goalFace2, (goalPath.size() < 2) ? goalFace->se() : goalPath[1], leftPath[leftPath.size() - 1], r))) + { + //adds the start path + for (int i = 0; i < startPath.size(); i++) + { + fullLeftPath.push() = startPath[i]; + } + //then the left side of the ring + for (int i = 0; i < leftPath.size(); i++) + { + fullLeftPath.push() = leftPath[i]; + } + //and finally, the goal path + for (int i = 0; i < goalPath.size(); i++) + { + fullLeftPath.push() = goalPath[i]; + } + } + //checks if the whole right path is valid, if so, constructs it + if (rightValid && ((startPath.size() < 1) || (rightPath.size() < 1) || + CanCross(startFace2, startPath[startPath.size() - 1], (rightPath.size() < 2) ? goalFace2->se() : rightPath[1], r)) + && ((goalPath.size() < 1) || (rightPath.size() < 1) || + CanCross(goalFace2, (goalPath.size() < 2) ? goalFace->se() : goalPath[1], rightPath[rightPath.size() - 1], r))) + { + //adds the start path + for (int i = 0; i < startPath.size(); i++) + { + fullRightPath.push() = startPath[i]; + } + //then the right side of the ring + for (int i = 0; i < rightPath.size(); i++) + { + fullRightPath.push() = rightPath[i]; + } + //and finally, the goal path + for (int i = 0; i < goalPath.size(); i++) + { + fullRightPath.push() = goalPath[i]; + } + } + //if neither path was valid, return failure + if ((fullLeftPath.size() == 0) && (fullRightPath.size() == 0)) + { + return false; + } + //if both paths are valid, + else if ((fullLeftPath.size() > 0) && (fullRightPath.size() > 0)) + { +#if defined EXPERIMENT + //start timing construction + Timer construction; +#endif + //calculate the lengths of both + SrPolygon funnelLeft, funnelRight; + float leftLength = GetShortestPath(funnelLeft, fullLeftPath, x1, y1, x2, y2, r); + float rightLength = GetShortestPath(funnelRight, fullRightPath, x1, y1, x2, y2, r); +#if defined EXPERIMENT + //record the construction time + data[0].ConstructionTime = construction.GetDuration(); +#endif + //set the current path and channel to the shorter of the two + currentPath = (leftLength < rightLength) ? funnelLeft : funnelRight; + currentChannel = (leftLength < rightLength) ? fullLeftPath : fullRightPath; +#if defined EXPERIMENT + //record the length of the chosen path + data[0].Length = (leftLength < rightLength) ? leftLength : rightLength; + //and the triangles visited + data[0].ConstructionNodes = startPath.size() + goalPath.size() + leftPath.size() + rightPath.size() + 2; + //and finish timing the algorithm + data[0].TotalTime = startTime.GetDuration(); +#endif + //and return success + return true; + } + else + { +#if defined EXPERIMENT + //start timing construction + Timer construction; + //if only one was valid, calculate the valid path + data[0].Length = GetShortestPath(currentPath, (fullRightPath.size() == 0) ? fullLeftPath : fullRightPath, x1, y1, x2, y2, r);\ + //record the construction time + data[0].ConstructionTime = construction.GetDuration(); + //and the triangles visited + data[0].ConstructionNodes = startPath.size() + goalPath.size() + leftPath.size() + rightPath.size() + 2; + //and stop timing the algorithm + data[0].TotalTime = startTime.GetDuration(); +#else + //calculate the shortest path in the valid channel + GetShortestPath(currentPath, (fullRightPath.size() == 0) ? fullLeftPath : fullRightPath, x1, y1, x2, y2, r); +#endif + //record the current channel + currentChannel = (fullRightPath.size() == 0) ? fullLeftPath : fullRightPath; + //and return success + return true; + } + } + //get the root of the start triangle + SeDcdtFace *face = startFace2; + SeDcdtFace *destinationFace; + float destinationAngle; + //go through that triangle's edges + for (int i = 0; i < 3; i++) + { + //if the degree-3 node adjacent across that edge is the first goal node, + if (face->link->Adjacent(i) == goalNodes[0].Triangle()) + { + //record the angle (distance) to that node + destinationAngle = face->link->Angle(i); + break; + } + } + //next go through the edges of the triangle at the root of the goal + for (int i = 0; i < 3; i++) + { + //when we find the edge across which the first goal node is adjacent, + if (goalFace2->link->Adjacent(i) == goalNodes[0].Triangle()) + { + //we check whether this node or the other is closer to it, + //and set the destination face to be the degree-3 node adjacent to the degree-2 + //start node such that we expect to cross the degree-2 goal node when moving to it + destinationFace = goalNodes[(goalFace2->link->Angle(i) < destinationAngle) ? 0 : 1].Triangle(); + break; + } + } + //go from the degree-2 start node to the destination face, building a channel as we go + SrArray channel; + channel.size(0); + //if we reach the destination face, the start and goal are on different edges with the same endpoints + while (face != destinationFace) + { + //if we reach the degree-2 goal face, they're on the same edge, + if (face == goalFace2) + { +#if defined EXPERIMENT + //start timing construction + Timer construction; +#endif + //create the start path from the start face to its root + SrArray startPath; + WalkBetween(startPath, startFace, startFace2, 0.0f); + //discard the overlapping triangle + startPath.pop(); + //create the goal path from the goal face to its root + SrArray goalPath; + WalkBetween(goalPath, goalFace, goalFace2, 0.0f); +#if defined EXPERIMENT + //keep track of the number of triangles visited + triangles += startPath.size() + goalPath.size() + 1; +#endif + //invert the (backwards) goal path + channel.revert(); + //add the channel we built to the end of the start path + while (!channel.empty()) + { + startPath.push() = channel.pop(); + } + //then add the goal path + while (!goalPath.empty()) + { + startPath.push() = goalPath.pop(); + } + //discard the extra triangle at the end + startPath.pop(); + //if the path is valid, + if (ValidPath(startPath, x1, y1, x2, y2, r)) + { + //calculates the shortest path in that channel and saves the length of it + shortestPathLength = GetShortestPath(shortestPath, startPath, x1, y1, x2, y2, r); + shortestChannel = startPath; +#if defined EXPERIMENT + //keeps track of the number of paths found + paths++; + //records the length + data[0].Length = shortestPathLength; + //and the construction time + data[0].ConstructionTime = construction.GetDuration(); + //and the triangles visited + data[0].ConstructionNodes = triangles; + //and the time so far + data[0].TotalTime = startTime.GetDuration(); + //move to the next data point + currentData++; +#endif + } +#if defined EXPERIMENT + //stop timing construction + constructionTime += construction.GetDuration(); +#endif + //remove from the goal nodes the one that is closest to the start + goalNodes.remove((goalNodes[0].Triangle() == destinationFace) ? 1 : 0); + //and remove the other one from the queue + if (q.top()->Triangle() == destinationFace) + { + q.pop(); + } + else + { + SearchNode *temp = q.top(); + q.pop(); + q.pop(); + q.push(temp); + } + //stop building the channel + break; + } + //go through the edges of the current triangle + SeBase *s = face->se(); + for (int i = 0; i < 3; i++) + { + //when we find the edge across which the destination node is adjacent, + if (face->link->Adjacent(i) == destinationFace) + { + //add this triangle to the channel + channel.push() = s; + //and move to the triangle across that edge + face = (SeDcdtFace *)s->sym()->fac(); + break; + } + //go to the next edge + s = s->nxt(); + } + } + } + //record the Euclidean distance between the start and goal + float startGoalDistance = Length(x1, y1, x2, y2); +#if defined FIRST_PATH_FAST + //create a second queue if running to the first path + std::priority_queue, check> q2; + //and begin marking the triangle mesh + _mesh->begin_marking(); +#endif + //actual abstract degree-3 node search finally begins! + //continue until all nodes have been exhausted + while (!q.empty()) + { +#if defined EXPERIMENT + //if it is time to collect more data, + if ((currentData > 0) && (startTime.GetDuration() >= data[currentData].TotalTime * data[0].TotalTime)) + { + //if this is the last data point, + if (currentData == numData - 1) + { + //empty the main queue of nodes + while (!q.empty()) + { + //closing and deleting all nodes in there + q.top()->Close(); + delete q.top(); + q.pop(); + } +#if defined FIRST_PATH_FAST + //empty the secondary queue similarly + while (!q2.empty()) + { + //closing and deleting all nodes in there + q2.top()->Close(); + delete q2.top(); + q2.pop(); + } +#endif + //and stop searching + break; + } + //record the total time for the algorithm, + data[currentData].TotalTime = startTime.GetDuration(); + //the construction time, + data[currentData].ConstructionTime = constructionTime; + //the number of nodes searched + data[currentData].SearchNodes = nodes; + //the shortest path found, + data[currentData].Length = shortestPathLength; + //the number of paths found, + data[currentData].Paths = paths; + //and the number of triangles used in construction + data[currentData].ConstructionNodes = triangles; + //and move to the next data item + currentData++; + } +#endif + //get the most promising search node off of the queue + SearchNode *current = q.top(); + q.pop(); +#if defined FIRST_PATH_FAST + //if we want the first path fast and we don't have it yet but we have searched this triangle, + if ((shortestPathLength == INFINITY) && (_mesh->marked(current->Triangle()))) + { + //put it on the secondary queue for now and move to the next node + q2.push(current); + continue; + } + //mark the current triangle as searched + _mesh->mark(current->Triangle()); +#endif +#if defined EXPERIMENT + //keep track of the number of nodes searched + nodes++; +#endif + //if the shortest path through the current node is longer than + //the best path found so far, we are done searching + if (current->f() >= shortestPathLength) + { + //empty the queue, + while (!q.empty()) + { + //closing and deleting all nodes in there + q.top()->Close(); + delete q.top(); + q.pop(); + } +#if defined FIRST_PATH_FAST + while (!q2.empty()) + { + //closing and deleting all nodes in there + q2.top()->Close(); + delete q2.top(); + q2.pop(); + } +#endif + //and the current node + current->Close(); + delete current; + //stop searching + break; + } + //record if we found the goal + bool found = false; + //go through the goal nodes + for (int i = 0; i < goalNodes.size(); i++) + { + //if the current triangle is a goal node, + if (current->Triangle() == goalNodes[i].Triangle()) + { +#if defined EXPERIMENT + //start timing construction + Timer construction; +#endif + //construct the channel corresponding to the path so far + SrArray channel; + ConstructBasePath(channel, current, startFace, goalFace, goalNodes[i].Direction()); +#if defined EXPERIMENT + //keep track of the number of triangles visited + triangles += channel.size(); +#endif + //only check the channel is it is wide enough + if (ValidEndpoints(channel, x1, y1, x2, y2, r)) + { +#if defined FIRST_PATH_FAST + //if we found the first path, put all the nodes in the secondary queue + while (!q2.empty()) + { + //back in the main queue + q.push(q2.top()); + q2.pop(); + } +#endif + //get the shortest path and its length + SrPolygon currentShortestPath; + float currentPathLength = GetShortestPath(currentShortestPath, channel, x1, y1, x2, y2, r); + //if it's the best path found so far, record it + if (currentPathLength < shortestPathLength) + { + shortestPathLength = currentPathLength; + shortestPath.size(0); + shortestPath = currentShortestPath; + shortestChannel.size(0); + shortestChannel = channel; + } +#if defined EXPERIMENT + //keep track of the number of paths found + paths++; +#endif + } +#if defined EXPERIMENT + //record the construction time + constructionTime += construction.GetDuration(); +#endif + //we have found a solution + found = true; + } + } + //if we found the goal, don't generate any children + if (found) + { + //close and delete the goal node + current->Close(); + delete current; +#if defined EXPERIMENT + //if this is the first path, + if (currentData < 1) + { + //if it's also the final data point, stop searching + if (currentData == numData - 1) + { + break; + } + //record the total time so far + data[currentData].TotalTime = startTime.GetDuration(); + //and the construction time + data[currentData].ConstructionTime = constructionTime; + //and the total number of nodes searched + data[currentData].SearchNodes = nodes; + //and the shortest path found so far + data[currentData].Length = shortestPathLength; + //and the number of paths found so far + data[currentData].Paths = paths; + //and the number of triangles visited + data[currentData].ConstructionNodes = triangles; + //and move to the next data point + currentData++; + } +#endif + //get the next node from the queue + continue; + } + //go through the edges of this triangle + SeBase *s = current->Triangle()->se(); + for (int i = 0; i < 3; i++) + { + //make sure not to cross the starting edge back the way we came + if (((current->Triangle() == startTriangle1) && (current->Triangle()->link->Adjacent(i) == startTriangle2)) + || ((current->Triangle() == startTriangle2) && (current->Triangle()->link->Adjacent(i) == startTriangle1))) + { + continue; + } + //get the degree-3 node adjacent across that edge + SeDcdtFace *childFace = current->Triangle()->link->Adjacent(i); + //get the width through this triangle and through the corridor to the next triangle + float width = (current->Back() == NULL) ? INFINITY : + (current->Triangle()->link->Adjacent((i + 1) % 3) == current->Back()->Triangle()) ? + current->Triangle()->link->Width(i) : current->Triangle()->link->Width((i + 2) % 3); + width = Minimum(current->Triangle()->link->Choke(i), width); + //only consider this triangle if it's wide enough to get there and we haven't + //searched it before (and it's not the way we just came) + if ((!current->Searched(childFace)) && (width >= 2.0f * r) + && ((current->Back() == NULL) || (childFace != current->Back()->Triangle()))) + { + //get the angle we cross the current triangle to get to the child + float x_1, y_1, x_2, y_2, x_3, y_3; + SeBase *s1 = childFace->se(); + for (int j = 0; j < 3; j++) + { + if (childFace->link->Adjacent(j) == current->Triangle()) + { + TriangleVertices(s1, x_1, y_1, x_2, y_2, x_3, y_3); + break; + } + s1 = s1->nxt(); + } + //get the closest point on the entry edge to the goal + SrPnt2 p = ClosestPointOn(x_1, y_1, x_2, y_2, x2, y2, r); + //and the start + SrPnt2 p2 = ClosestPointOn(x_1, y_1, x_2, y_2, x1, y1, r); + float x_4 = x_1; + float y_4 = y_1; + float x_5 = x_2; + float y_5 = y_2; + TriangleVertices(s, x_1, y_1, x_2, y_2, x_3, y_3); + //get the angle through the last triangle to this one + float theta = (current->Back() == NULL) ? 0.0f : (current->Triangle()->link->Adjacent((i + 1) % 3) == current->Back()->Triangle()) ? + abs(AngleBetween(x_1, y_1, x_2, y_2, x_3, y_3)) : abs(AngleBetween(x_3, y_3, x_1, y_1, x_2, y_2)); + //the heuristic is the closest (Euclidean) distance to the goal + float h = Length(p.x, p.y, x2, y2); + //the distance travelled so far is the distance for the parent plus the max of the total angle through + //the parent triangle and the corridor from it to this triangle times the object's radius, and the difference + //between the parent nodes heuristic and this node's + float minDistance = current->g() + Maximum((current->Triangle()->link->Angle(i) + theta) * r, current->h() - h); + //maxxed with the distance between the start and the closest point to it on the entry edge + minDistance = Maximum(Length(x1, y1, p2.x, p2.y), minDistance); + //maxxed with the Euclidean distance between the start and goal minus this node's heuristic + minDistance = Maximum(minDistance, startGoalDistance - h); + //maxxed with the parent's distance travelled plus the minimum distance between its entry edge and + //that of this node, plus the angle through the parent node times the unit's radius + minDistance = Maximum(minDistance, current->g() + SegmentDistance(x_1, y_1, x_2, y_2, x_5, y_5, x_4, y_4, r) + theta * r); + //if this child might yield a shorter path, + if ((minDistance + h) < shortestPathLength) + { + //push a node with these values onto the priority queue + q.push(new SearchNode(minDistance, h, p, childFace, current, i)); + //tell the parent node it has another child + current->OpenChild(); + } + } + //move to the next edge of the parent node's triangle + s = s->nxt(); + } + //if no children were opened, + if (current->OpenChildren() <= 0) + { + //we can close and delete the current node now + current->Close(); + delete current; + } + } +#if defined FIRST_PATH_FAST + //stop marking searched triangles + _mesh->end_marking(); + //go through the secondary queue + while (!q2.empty()) + { + //closing and deleting all nodes in there + q2.top()->Close(); + delete q2.top(); + q2.pop(); + } +#endif + //save the shortest path found and its channel + currentPath = shortestPath; + currentChannel = shortestChannel; +#if defined EXPERIMENT + //record the total time of the algorithm + data[currentData].TotalTime = startTime.GetDuration(); + //and the construction time + data[currentData].ConstructionTime = constructionTime; + //and the number of nodes searched + data[currentData].SearchNodes = nodes; + //and the length of the shortest path found + data[currentData].Length = shortestPathLength; + //and the total number of paths + data[currentData].Paths = paths; + //and finally the number of triangles used for construction + data[currentData].ConstructionNodes = triangles; +#endif + //return whether or not one was found + return (shortestPathLength < INFINITY); +} + +//searches the base graph for a path between two points +#if defined EXPERIMENT +bool SeDcdt::SearchPathBaseFast(float x1, float y1, float x2, float y2, float r, Data *data, int numData)//, float &length, float &time, int &nodes) +#else +bool SeDcdt::SearchPathBaseFast(float x1, float y1, float x2, float y2, float r)//, float &length, float &time, int &nodes) +#endif +{ +#if defined EXPERIMENT + //start timing the algorithm + Timer startTime; + //keep track of the total tome spent constructing paths + float constructionTime = 0.0f; + //and the current data point + int currentData = 0; + //and the number of triangles searched + int nodes = 0; + //and the number of paths found + int paths = 0; +#endif + //initialize the path to empty + currentPath.size(0); + SeBase *start; + //find the triangle containing the start point + SeTriangulator::LocateResult startResult = LocatePoint(x1, y1, start); + //if it wasn't found, return a path could not be found + if (startResult == SeTriangulator::LocateResult::NotFound) + { + return false; + } + //get the starting triangle + SeDcdtFace *startFace = (SeDcdtFace *)start->fac(); + //create a priority queue for the search nodes + std::priority_queue, check> q; +#if defined FIRST_PATH_FAST + //if we are finding the first path quickly, initialize a secondary queue + std::priority_queue, check> q2; +#endif + //calculate the closest point to the goal in the first triangle + SrPnt2 p = ClosestPointTo(startFace, x2, y2, r); + //use the distance between that and the goal as the heuristic in the first search node + q.push(new SearchNode(0.0f, Length(p.x, p.y, x2, y2), p, startFace, NULL)); + //the shortest path so far and its length and channel + float shortestPathLength = INFINITY; + SrPolygon shortestPath; + SrArray shortestChannel; +#if defined FIRST_PATH_FAST + //start marking which triangles have been explored + _mesh->begin_marking(); +#endif + //we start with 1 unexplored triangle on the queue +// int unexplored = 1; + //calculate the Euclidean distance between the start and goal + float startGoalDistance = Length(x1, y1, x2, y2); + //continue until all nodes have been exhausted + while (!q.empty()) + { +#if defined EXPERIMENT + //if it's time to record another data point, + if ((currentData > 0) && (startTime.GetDuration() >= data[currentData].TotalTime * data[0].TotalTime)) + { + //if it's the last data point, + if (currentData == numData - 1) + { + //empty the primary queue, close and delete all remaining nodes + while (!q.empty()) + { + q.top()->Close(); + delete q.top(); + q.pop(); + } +#if defined FIRST_PATH_FAST + //empty the secondary queue, close and delete all remaining nodes + while (!q2.empty()) + { + q2.top()->Close(); + delete q2.top(); + q2.pop(); + } +#endif + //and stop searching + break; + } + //get the total time spent so far + data[currentData].TotalTime = startTime.GetDuration(); + //the time spent on construction + data[currentData].ConstructionTime = constructionTime; + //the number of nodes searched + data[currentData].SearchNodes = nodes; + //the shortest path found + data[currentData].Length = shortestPathLength; + //and the number of paths found + data[currentData].Paths = paths; + //and move to the next data point + currentData++; + } +#endif + //gets the best search node so far + SearchNode *current = q.top(); + q.pop(); +#if defined FIRST_PATH_FAST + //if we want the first path quickly and we haven't found one yet but have searched this triangle before, + if ((shortestPathLength == INFINITY) && (_mesh->marked(current->Triangle()))) + { + //push it onto the secondary queue for now + q2.push(current); + //and get the next most promising node + continue; + } + //otherwise, mark this triangle as visited + _mesh->mark(current->Triangle()); +#endif +#if defined EXPERIMENT + //keep track of the number of nodes searched + nodes++; +#endif + //if the shortest path through this node is longer than the best path we have found so far, + //or if we have no unexplored nodes on the queue and we haven't found a path yet + if ((current->f() >= shortestPathLength))// || ((unexplored <= 0) && (shortestPathLength == INFINITY))) + { + //empty the primary queue, close and delete all remaining nodes + while (!q.empty()) + { + q.top()->Close(); + delete q.top(); + q.pop(); + } +#if defined FIRST_PATH_FAST + //empty the secondary queue, close and delete all remaining nodes + while (!q2.empty()) + { + q2.top()->Close(); + delete q2.top(); + q2.pop(); + } +#endif + //close and delete the current node + current->Close(); + delete current; + //exit the loop + break; + } + //if we haven't explored the current node yet +// if (!_mesh->marked(current->Triangle())) +// { + //mark it as explored now +// _mesh->mark(current->Triangle()); + //we are taking an unexplored node off of the queue +// unexplored--; +// } + //if the goal is in the current triangle, + if (InTriangle(current->Triangle(), x2, y2)) + { +#if defined EXPERIMENT + //time the construction + Timer construct; +#endif + //construct the channel formed by our search + SrArray channel = ConstructBaseChannel(current); + //close and delete the current search node + current->Close(); + delete current; + //check if the endpoints are valid + if (ValidEndpoints(channel, x1, y1, x2, y2, r)) + { +#if defined FIRST_PATH_FAST + //once we find a path put all the nodes from the secondary queue on to the primary one + while (!q2.empty()) + { + q.push(q2.top()); + q2.pop(); + } +#endif + //calculate the path and its length + SrPolygon currentShortestPath; + float currentPathLength = GetShortestPath(currentShortestPath, channel, x1, y1, x2, y2, r); + //if this is the best path so far, + if (currentPathLength < shortestPathLength) + { + //set this as the best path + shortestPathLength = currentPathLength; + shortestPath.size(0); + shortestPath = currentShortestPath; + shortestChannel.size(0); + shortestChannel = channel; + } + } +#if defined EXPERIMENT + //keep track of the total time spent doing construction + constructionTime += construct.GetDuration(); + //keep track of the number of paths found + paths++; + //if we are on the first data point, + if (currentData < 1) + { + //also if we're on the last data point, + if (currentData == numData - 1) + { + //empty the primary queue, close and delete all remaining nodes + while (!q.empty()) + { + q.top()->Close(); + delete q.top(); + q.pop(); + } + //and stop searching + break; + } + //record the time spent so far on the algorithm + data[currentData].TotalTime = startTime.GetDuration(); + //and the time spent on construction + data[currentData].ConstructionTime = constructionTime; + //and the number of triangles searched + data[currentData].SearchNodes = nodes; + //and the length of the shortest path + data[currentData].Length = shortestPathLength; + //and the number of paths found + data[currentData].Paths = paths; + //and move to the next data point + currentData++; + } +#endif + //move to the next triangle + continue; + } + //go through the edges of the current triangle + SeBase *s = current->Triangle()->se(); + for (int i = 0; i < 3; i++) + { + //only consider crossing edges that aren't blocked + if (!Blocked(s)) + { + //get the triangle associated with the potential child node + SeDcdtFace *childFace = (SeDcdtFace *)s->sym()->fac(); + //calculate the width through this triangle between the last triangle and the next + float width = (current->Back() == NULL) ? INFINITY : (s->nxt()->sym()->fac() == current->Back()->Triangle()) ? + current->Triangle()->link->Width(i) : current->Triangle()->link->Width((i + 2) % 3); + //only generate the child node if we haven't explored it yet in this path, the width through + //the triangle is enough, and it's not the last face we explored + if ((!current->Searched(childFace)) && (width >= 2.0f * r) + && ((current->Back() == NULL) || (childFace != current->Back()->Triangle()))) + { + //get a component of the child face + SeBase *s1 = childFace->se(); + float x_1, y_1, x_2, y_2, x_3, y_3; + //go through the edges to determine which is the entry edge + for (int j = 0; j < 3; j++) + { + //that is the edge across which is the parent triangle + if (s1->sym()->fac() == s->fac()) + { + //and get the vertices starting from that edge + TriangleVertices(s1, x_1, y_1, x_2, y_2, x_3, y_3); + break; + } + //move to the next edge to check + s1 = s1->nxt(); + } + //get the closest point on the entry edge to the goal + SrPnt2 p = ClosestPointOn(x_1, y_1, x_2, y_2, x2, y2, r); + //and the start + SrPnt2 p2 = ClosestPointOn(x_1, y_1, x_2, y_2, x1, y1, r); + //get the parent triangle's vertices + TriangleVertices(s, x_1, y_1, x_2, y_2, x_3, y_3); + //and calculate the angle through that triangle to this one + float theta = (current->Back() == NULL) ? 0.0f : (s->nxt()->sym()->fac() == current->Back()->Triangle()) ? + abs(AngleBetween(x_1, y_1, x_2, y_2, x_3, y_3)) : abs(AngleBetween(x_3, y_3, x_1, y_1, x_2, y_2)); + //the heuristic is the Euclidean distance between the goal and the closest point + //to it in this triangle's entry edge + float h = Length(p.x, p.y, x2, y2); + //the distance travelled is the maximum of that for the parent triangle + //plus the maximum of the angle through the parent triangle times the + //unit's radius, and the parent triangle's heuristic minus that of this one + float minDistance = current->g() + Maximum(theta * r, current->h() - h); + //maxxed with the minimum distance between this triangle's entry edge and the start + minDistance = Maximum(Length(x1, y1, p2.x, p2.y), minDistance); + //maxxed with the Euclidean distance between the start and goal minus this + //triangle's heuristic value + minDistance = Maximum(minDistance, startGoalDistance - h); + //if this child could yield a shorter path than the best found so far, + if ((minDistance + h) < shortestPathLength) + { + //put the child search node on the queue + q.push(new SearchNode(minDistance, h, p, childFace, current)); + //tell the current node that it has an open child + current->OpenChild(); + //if we haven't explored this triangle before, +// if (!_mesh->marked(childFace)) +// { +// //we are putting an unexplored node on the queue +// unexplored++; +// } + } + } + } + //move to the next edge of the parent triangle + s = s->nxt(); + } + //if no children were opened, close the current node and delete it + if (current->OpenChildren() <= 0) + { + current->Close(); + delete current; + } + } +#if defined FIRST_PATH_FAST + //empty the queue, close and delete all remaining nodes + while (!q2.empty()) + { + q2.top()->Close(); + delete q2.top(); + q2.pop(); + } + //stop marking the triangle mesh + _mesh->end_marking(); +#endif + //set the current path and channel to the best one found + currentPath = shortestPath; + currentChannel = shortestChannel; + //set the start and goal points + startPoint.set(x1, y1); + goalPoint.set(x2, y2); +// _mesh->end_marking(); +#if defined EXPERIMENT + //stop timing the algorithm and record the total time spent + data[currentData].TotalTime = startTime.GetDuration(); + //record the total time spent on construction + data[currentData].ConstructionTime = constructionTime; + //record the number of nodes searched + data[currentData].SearchNodes = nodes; + //record the length of the shortest path + data[currentData].Length = shortestPathLength; + //and finally the total number of paths found + data[currentData].Paths = paths; +#endif + //return whether a path was found or not + return (shortestPathLength < INFINITY); +} + +//construct the channel given the last SearchNode in the solution +SrArray SeDcdt::ConstructBaseChannel(SearchNode *goal) +{ + SrArray channel; + //if there is only one node, return an empty path + if (goal->Back() == NULL) + { + return channel; + } + //otherwise, move back in the chain + SearchNode *current = goal->Back(); + //until we are at the second-to-first node + while (current->Back() != NULL) + { + //go through the edges of the triangle associated with this node + SeBase *s = current->Triangle()->se(); + for (int i = 0; i < 3; i++) + { + //if the previous triangle is across this edge, + if (s->sym()->fac() == current->Back()->Triangle()) + { + //add this edge/face to the channel + channel.push() = s; + //and move to the next node + break; + } + //go to the next edge in the triangle + s = s->nxt(); + } + //move to the previous node in the chain + current = current->Back(); + } + //add the last triangle to the channel + channel.push() = current->Triangle()->se(); + //the channel was constructed backward, so flip it, then return it + channel.revert(); + return channel; +} + +//uses the modified funnel algorithm to get the shortest path for nonzero-radius units +//and return the length of this path +float SeDcdt::GetShortestPath(SrPolygon &path, SrArray Channel, float x1, float y1, float x2, float y2, float r) +{ + //initializes the path + path.open(true); + path.size(0); + //if there are no edges in the channel, the path is a straight line + if (Channel.size() < 1) + { + path.push().set(x1, y1); + path.push().set(x2, y2); + return Length(x1, y1, x2, y2); + } + //otherwise, initialize a funnel deque + //with the starting point + FunnelDeque fd(x1, y1, r); + //get the first edge in the channel + SeBase *s = Channel[0]; + //get the reference whose sym edge + //belongs to the next triangle in the channel + for (int i = 0; i < 3; i++) + { + if (((Channel.size() > 1) && (s->sym()->fac() == Channel[1]->fac())) + || (InTriangle((SeDcdtFace *)s->sym()->fac(), x2, y2))) + { + break; + } + s = s->nxt(); + } + //get the point on the right side of that edge + SrPnt2 p; + p.set(((SeDcdtVertex *)s->vtx())->p); + //insert it into the deque + fd.Add(p, FunnelDeque::CornerType::LeftTangent, path); + SeVertex *right = s->vtx(); + //get the point on the left side of that edge + s = s->nxt(); + SeVertex *left = s->vtx(); + p.set(((SeDcdtVertex *)s->vtx())->p); + //insert it into the deque + fd.Add(p, FunnelDeque::CornerType::RightTangent, path); + //go through the other edges in the channel + for (int i = 1; i < Channel.size() - 1; i++) + { + s = Channel[i]; + //get the reference whose sym edge + //belongs to the next triangle in the channel + for (int j = 0; j < 3; j++) + { + if (s->sym()->fac() == Channel[i + 1]->fac()) + { + break; + } + s = s->nxt(); + } + //if that edge shares a vertex with the right side + //of the funnel, the opposite point should be + //added to the left side of the funnel + if (s->vtx() == right) + { + p.set(((SeDcdtVertex *)s->nxt()->vtx())->p); + fd.Add(p, FunnelDeque::CornerType::RightTangent, path); + left = s->nxt()->vtx(); + } + //otherwise, add it to the right side of the funnel + else //(s->nxt()->vtx() == left) + { + p.set(((SeDcdtVertex *)s->vtx())->p); + fd.Add(p, FunnelDeque::CornerType::LeftTangent, path); + right = s->vtx(); + } + } + //go to the last edge in the channel + s = Channel[Channel.size() - 1]; + //find the reference across which is + //the triangle with the goal in it + for (int j = 0; j < 3; j++) + { + if (InTriangle((SeDcdtFace *)s->sym()->fac(), x2, y2)) + { + break; + } + s = s->nxt(); + } + //like above, determines whether to add + //the point to the left or right side of the funnel + if (s->vtx() == right) + { + p.set(((SeDcdtVertex *)s->nxt()->vtx())->p); + fd.Add(p, FunnelDeque::CornerType::RightTangent, path); + } + else //(s->nxt()->vtx() == left) + { + p.set(((SeDcdtVertex *)s->vtx())->p); + fd.Add(p, FunnelDeque::CornerType::LeftTangent, path); + } + //finally, add the goal point to the funnel + p.set(x2, y2); + fd.Add(p, FunnelDeque::CornerType::Point, path); + //calculate the length of the path sand return it + return fd.Length(path); +} + +//returns a polygon with the boundary of the current channel +SrPolygon SeDcdt::GetChannelBoundary() +{ + //start the boundary with the starting point, go clockwise + SrPolygon boundary; + //if there is no channel, return an empty boundary + if (currentChannel.size() == 0) + { + boundary.size(0); + return boundary; + } + boundary.push().set(startPoint.x, startPoint.y); + SeBase *s; + //go through each of the triangles in the channel (except the last one) + for (int i = 0; i < currentChannel.size() - 1; i++) + { + //go through the edges of the current triangle + s = currentChannel[i]; + for (int j = 0; j < 3; j++) + { + //if this edge borders the next triangle, + if (s->sym()->fac() == currentChannel[i + 1]->fac()) + { + //add the vertex on the other side of this edge to the boundary + boundary.push().set(((SeDcdtVertex *)s->nxt()->vtx())->p.x, ((SeDcdtVertex *)s->nxt()->vtx())->p.y); + break; + } + //go to the next edge + s = s->nxt(); + } + } + //now deal with the last triangle in the channel + s = currentChannel[currentChannel.size() - 1]; + //go through its edges + for (int i = 0; i < 3; i++) + { + //find the edge shed with the triangle containing the goal point + if (InTriangle((SeDcdtFace *)s->sym()->fac(), goalPoint.x, goalPoint.y)) + { + //add the vertex on the other end of this edge, the goal point, + //and the vertex at this end of the edge to the boundary + boundary.push().set(((SeDcdtVertex *)s->nxt()->vtx())->p.x, ((SeDcdtVertex *)s->nxt()->vtx())->p.y); + boundary.push().set(goalPoint.x, goalPoint.y); + boundary.push().set(((SeDcdtVertex *)s->vtx())->p.x, ((SeDcdtVertex *)s->vtx())->p.y); + break; + } + //go to the next edge + s = s->nxt(); + } + //now go backwards through the triangles in the channel (starting with the second-last one) + for (int i = currentChannel.size() - 2; i >= 0; i--) + { + //go through the edges of the current triangle + s = currentChannel[i]; + for (int j = 0; j < 3; j++) + { + //if this edge borders the next triangle, + if (s->sym()->fac() == currentChannel[i + 1]->fac()) + { + //add the vertex on this side of the edge to the boundary + boundary.push().set(((SeDcdtVertex *)s->vtx())->p.x, ((SeDcdtVertex *)s->vtx())->p.y); + break; + } + s = s->nxt(); + } + } + //return the boundary + return boundary; +} + +//returns the current path +SrPolygon &SeDcdt::GetPath() +{ + return currentPath; +} + +//Abstract space searching function definitions } diff --git a/source/dcdt/se/Search.cpp.bak b/source/dcdt/se/Search.cpp.bak new file mode 100644 index 0000000000..adb0886c19 --- /dev/null +++ b/source/dcdt/se/Search.cpp.bak @@ -0,0 +1,1545 @@ +//Search.cpp + +//DJD: Abstract space searching function definitions { + +#include + +#include +#include + +//functions for comparing SearchNodes +bool operator <(SearchNode sn1, SearchNode sn2) +{ + //compares the f-values, but switches them so the lowest value + //is kept at the start of the queue instead of the greatest + return (sn1.f() < sn2.f()); +} +/* +bool operator <(SearchNode *sn1, SearchNode *sn2) +{ + return ((sn1->f() < sn2->f()) || ((sn1->f() == sn2->f()) && (sn1->h() < sn2->h()))); +} +*/ +struct check : public std::binary_function + { + bool operator()(SearchNode *&sn1, SearchNode *&sn2) const + { + return ((sn1->f() > sn2->f()) || ((sn1->f() == sn2->f()) && (sn1->h() > sn2->h())));; + } +}; + +//functions for comparing SearchNodes +bool operator ==(SearchNode sn1, SearchNode sn2) +{ + //compares the f-values, but switches them so the lowest value + //is kept at the start of the queue instead of the greatest + return (sn1.Triangle() == sn2.Triangle()); +} + +bool SeDcdt::IDA(SrArray startNodes, SrArray goalNodes, + SearchNode* &goal, float &depthBound, float r) +{ + //TODO: actual search! + return false; +} + +bool SeDcdt::IDA(SrArray startNodes, SrArray goalNodes, SrArray& path, float r) +{ + SrArray copy; + float depthBound = INFINITY; + for (int i = 0; i < startNodes.size(); i++) + { + if (startNodes[i]->f() < depthBound) + { + depthBound = startNodes[i]->f(); + } + } + SearchNode *goal = NULL; + while (true) + { + copy.size(0); + for (int i = 0; i < startNodes.size(); i++) + { + copy.push() = startNodes[i]; + } + if (!IDA(copy, goalNodes, goal, depthBound, r) || (goal != NULL)) + { + break; + } + } + path.size(0); + if (goal == NULL) + { + return false; + } + SrPolygon tempPath; + SrArray channel; + while (goal != NULL) + { + path.push() = goal->Triangle()->se(); + SearchNode *temp = goal; + goal = goal->Back(); + delete temp; + } + path.revert(); + return true; +} + +//gets the closest point on an unconstrained edge of the triangle to the point given +//(that is at least r from a vertex) . . . this function can be sped up +SrPnt2 SeDcdt::ClosestPointTo(SeDcdtFace *face, float x, float y, float r) +{ + if (InTriangle(face, x, y)) + { + SrPnt2 p; + p.set(x, y); + return p; + } + SrPnt2 closestPoint, point; + float closestDistance = INFINITY, distance; + SeBase *s = face->se(); + for (int i = 0; i < 3; i++) + { + float x1, y1, x2, y2, x3, y3; + TriangleVertices(s, x1, y1, x2, y2, x3, y3); + if (Blocked(s)) + { + s = s->nxt(); + continue; + } + bool accute1 = IsAccute(AngleBetween(x1, y1, x2, y2, x, y)); + bool accute2 = IsAccute(AngleBetween(x2, y2, x1, y1, x, y)); + if (accute1 && accute2) + { + //get closest point on this line + distance = PointLineDistance(x, y, x1, y1, x2, y2); + float theta = atan2(y2 - y1, x2 - x1); + theta += (theta > PI / 2.0f) ? -3.0f * PI / 2.0f : PI / 2.0f; + point.set(x + cos(theta) * distance, y + sin(theta) * distance); + if (Length(point.x, point.y, x1, y1) < r) + { + accute2 = false; + } + else if (Length(point.x, point.y, x2, y2) < r) + { + accute1 = false; + } + } + if (!accute1 && accute2) + { + //get point distance r from (x2, y2) in the direction of (x1, y1) + float theta = atan2(y1 - y2, x1 - x2); + point.set(x2 + cos(theta) * r, y2 + sin(theta) * r); + } + else if (accute1 && !accute2) + { + //get point distance r from (x1, y1) in the direction of (x2, y2) + float theta = atan2(y2 - y1, x2 - x1); + point.set(x1 + cos(theta) * r, y1 + sin(theta) * r); + } + distance = Length(point.x, point.y, x, y); + if (distance < closestDistance) + { + closestPoint.set(point.x, point.y); + closestDistance = distance; + } + s = s->nxt(); + } + return closestPoint; +} + +//walks from the degree-1 source triangle to the degree-2 destination rectangle at the root of the tree +bool SeDcdt::WalkBetween(SrArray& path, SeDcdtFace *sourceFace, SeDcdtFace *destinationFace, float r, int direction) +{ + SeDcdtFace *last = NULL; + //go through all faces between the source and destination faces + while (true) + { + //if we have reached the destination face, + if (sourceFace == destinationFace) + { + //add it to the path and exit successfully + path.push() = destinationFace->se(); + return true; + } + //get the elements of the current triangle + SeBase *s = sourceFace->se(); + //go through the edges of the triangle + for (int i = 0; i < 3; i++) + { + //if the destination face is across the current edge, + if ((i == direction) || ((direction == INVALID) + && (sourceFace->link->Adjacent(i) == destinationFace) && (s->sym()->fac() != last))) + { + last = sourceFace; + direction = INVALID; + //make sure the path is wide enough + if (sourceFace->link->Choke(i) < 2.0f * r) + { + //if not, empty the path and return failure + path.size(0); + return false; + } + else + { + //if so, add the current face to the path + path.push() = sourceFace->se(); + //and move to the next one + sourceFace = (SeDcdtFace *)s->sym()->fac(); +float x, y; +TriangleMidpoint(sourceFace, x, y); + break; + } + } + //move to the next edge in the current triangle + s = s->nxt(); + } + } +} + +//checks if a unit of radius r can go through the second triangle between the first and the third +bool SeDcdt::CanCross(SeBase *first, SeBase *second, SeBase *third, float r) +{ + //and goes through its edges + for (int i = 0; i < 3; i++) + { + //gets the face across the current edge + SeFace *current = second->sym()->fac(); + //if it is neither the first nor the third + if ((current != first->fac()) && (current != third->fac())) + { + //return if the width between the first and the third is large enough for radius r + return (((SeDcdtFace *)second)->link->Width((i + 1) % 3) >= 2.0f * r); + } + //move to the next edge + second = second->nxt(); + } + sr_out.warning("ERROR: could not find desired face (should never happen)\n"); + return false; +} + +//finds a path in the degree-1 tree with both the start and goal in it +bool SeDcdt::Degree1Path(SrArray& path, float x1, float y1, SeDcdtFace *startFace, + float x2, float y2, SeDcdtFace *goalFace, float r) +{ + //starts marking the mesh + _mesh->begin_marking(); + //create a priority queue for searching the tree +// std::priority_queue pq; + std::priority_queue, check> pq; + float x, y; + //each face is represented by its midpoint (since only one path must exist) + TriangleMidpoint(startFace, x, y); + SrPnt2 p; + p.set(x, y); + //creates the first search node + SearchNode *current = new SearchNode(Length(x, y, x1, y1), Length(x, y, x2, y2), p, startFace, NULL); + //put it in the queue to be searched + pq.push(current); + //continues until the tree has been exhausted or a path has been found + while (!pq.empty()) + { + //gets the first node to search + /*SearchNode */current = pq.top(); + pq.pop(); + //checks if that node is the goal + if (InTriangle(current->Triangle(), x2, y2)) + { + while (!pq.empty()) + { + pq.top()->Close(); + delete pq.top(); + pq.pop(); + } + SearchNode *temp = current; + //if so, visits all the triangles between the start and the goal + while (current != NULL) + { + //and adds each to the path + path.push() = current->Triangle()->se(); + //until the first node is reached +// if (current->Back() == NULL) +// { +// break; +// } + //moves to the previous triangle + current = current->Back(); + } + temp->Close(); + delete temp; + //since they were visited in reverse, flip the path around + path.revert(); + path.pop(); + //return a path was successfully found +// return true; + _mesh->end_marking(); + if (ValidPath(path, x1, y1, x2, y2, r)) + { + return true; + } + else + { + path.size(0); + return false; + } + } + //gets the triangle associated with the current node + SeDcdtFace *currentFace = current->Triangle(); + //marks it as visited + _mesh->mark(currentFace); + //get the elements of the triangle + SeBase *s = currentFace->se(); + //go through the edges + for (int i = 0; i < 3; i++) + { + //gets the face on the opposite side of this edge + SeDcdtFace *tempFace = (SeDcdtFace *)s->sym()->fac(); + //only consider it if it's degree-1 and hasn't been visited before + if ((!_mesh->marked(tempFace)) && (tempFace->link->Degree() == 1) && (!Blocked(s))) + { +/* + //make sure the current triangle is wide enough + float width; + //if it's the first triangle, assume it is for now + if (current.Back() == NULL) + { + width = INFINITY; + } + //otherwise, get the width through this triangle between + //the last triangle and the next one + else + { + if (s->nxt()->sym()->fac() == current.Back()->Triangle()) + { + width = currentFace->link->Width(i); + } + else + { + width = currentFace->link->Width((i + 2) % 3); + } + } + //only add this triangle if the path is wide enough + if (width >= 2.0f * r) + { +*/ + //get the new triangle's midpoint + TriangleMidpoint(tempFace, x, y); + p.set(x, y); + //create a search node corresponding to this triangle + SearchNode *temp = new SearchNode(current->g() + Length(current->Point().x, current->Point().y, x, y), + Length(x, y, x2, y2), p, tempFace, current); + //and add it to the search queue + pq.push(temp); + current->OpenChild(); +// } + } + //move to the next edge + s = s->nxt(); + } + if (current->OpenChildren() <= 0) + { + current->Close(); + delete current; + } + } + //stop marking the mesh + _mesh->end_marking(); + //the tree was exhausted and a path not found - return failure + return false; +} + +//follows a degree-2 ring from one node to another, making sure that each is wide enough for the given radius +bool SeDcdt::Degree2Path(SrArray& path, SeDcdtFace *startFace, SeDcdtFace *nextFace, + SeDcdtFace *goalFace, float r) +{ + //empties the path first + path.size(0); + //goes until it has reached the goal triangle + while (nextFace != goalFace) + { + //gets the elements of the current triangle + SeBase *s = nextFace->se(); + //goes through its edges + for (int i = 0; i < 3; i++) + { + //when the face across the current edge is the last triangle, + if (s->sym()->fac() == startFace) + { + //check if the triangle across the next edge is the next triangle in the ring + if (nextFace->link->Angle((i + 1) % 3) == INFINITY) + { + //makes sure this triangle is wide enough for radius r + if (nextFace->link->Width(i) < 2.0f * r) + { + //if not, empties the path and returns failure + path.size(0); + return false; + } + //otherwise, adds the current triangle to the path + path.push() = nextFace->se(); + //and moves to the next triangle in the ring + startFace = nextFace; + nextFace = (SeDcdtFace *)s->nxt()->sym()->fac(); + } + else + { + //similar to above, checks the width + if (nextFace->link->Width((i + 2) % 3) < 2.0f * r) + { + path.size(0); + return false; + } + //and continues along the ring + path.push() = nextFace->se(); + startFace = nextFace; + nextFace = (SeDcdtFace *)s->nxt()->nxt()->sym()->fac(); + } + //process the next triangle in the ring + break; + } + //move to the next edge + s = s->nxt(); + } + } + //return that a path was found successfully + return true; +} + +//determines if the given endpoint has to be checked against the current triangle +bool SeDcdt::ValidEndpoint(SeBase *s, float x, float y, bool left) +{ + //gets the vertices of the current triangle + float x1, y1, x2, y2, x3, y3, x4, y4; + TriangleVertices(s, x1, y1, x2, y2, x3, y3); + //checks if a base angle is obtuse + if ((abs(AngleBetween(x1, y1, x2, y2, x3, y3)) >= PI / 2.0f) + || (abs(AngleBetween(x1, y1, x3, y3, x2, y2)) >= PI / 2.0f)) + { + //if so, we don't have to worry + return true; + } + //gets the angle of the opposite edge + float theta = atan2(y3 - y2, x3 - x2); + //turn 90 degrees clockwise + theta += (theta <= PI / -2.0f) ? 3.0f * PI / 2.0f : PI / -2.0f; + //create a point this angle from the "top" vertex + x4 = x1 + cos(theta); + y4 = y1 + sin(theta); + //returns whether the unit doesn't have to pass through the narrowest point in this triangle + return ((Orientation(x1, y1, x4, y4, x, y) > 0) == (left)); +} + +//checks whether there is a valid path from the start or goal through their corresponding triangles +bool SeDcdt::ValidEndpoint(SeDcdtFace *first, SeDcdtFace *second, float x, float y, float r) +{ + SeBase *s = first->se(); + for (int i = 0; i < 3; i++) + { + if (s->sym()->fac() == second) + { +// return (!(((!ValidEndpoint(s, x, y, false)) && (first->link->Width((i + 2) % 3) < 2.0f * r)) +// || ((!ValidEndpoint(s->nxt(), x, y, true)) && (first->link->Width(i) < 2.0f * r)))); + return (((ValidEndpoint(s, x, y, false)) || (first->link->Width((i + 2) % 3) >= 2.0f * r)) + && ((ValidEndpoint(s->nxt(), x, y, true)) || (first->link->Width(i) >= 2.0f * r))); + } + s = s->nxt(); + } + return false; +} + +//checks if the start and goal points can validly enter the path +bool SeDcdt::ValidEndpoints(SrArray& path, float x1, float y1, float x2, float y2, float r) +{ +// return ((path.size() > 1) && (ValidEndpoint((SeDcdtFace *)path[0]->fac(), (SeDcdtFace *)path[1]->fac(), x1, y1, r)) +// && (ValidEndpoint((SeDcdtFace *)path[path.size() - 1]->fac(), (SeDcdtFace *)path[path.size() - 2]->fac(), x2, y2, r))); + return ((path.size() < 2) || ((ValidEndpoint((SeDcdtFace *)path[0]->fac(), (SeDcdtFace *)path[1]->fac(), x1, y1, r)) + && (ValidEndpoint((SeDcdtFace *)path[path.size() - 1]->fac(), (SeDcdtFace *)path[path.size() - 2]->fac(), x2, y2, r)))); +} + +//checks if a path is valid for a unit of radius r +bool SeDcdt::ValidPath(SrArray& path, float x1, float y1, float x2, float y2, float r) +{ + //first checks if the endpoints are valid + if (!ValidEndpoints(path, x1, y1, x2, y2, r)) + { + return false; + } + //then goes through the interior points + for (int i = 1; i < path.size() - 1; i++) + { + //gets the elements of the current triangle in the path + SeBase *s = path[i]->fac()->se(); + //goes through the edges in this triangle + for (int j = 0; j < 3; j++) + { + //find the edge across which is the next triangle in the path + if (s->sym()->fac() == path[i + 1]->fac()) + { + //get the width through this triangle between the last one in the path and the next one + float width = ((SeDcdtFace *)s->fac())->link->Width( + (s->nxt()->sym()->fac() == path[i - 1]->fac()) ? j : ((j + 2) % 3)); + //if it's less than the diameter of the unit, the path is invalid + if (width < 2.0f * r) + { + return false; + } + break; + } + //moves to the next edge in the triangle + s = s->nxt(); + } + } + //if no triangle in the path was invalid, the path is valid + return true; +} + +void SeDcdt::ConstructBasePath(SrArray &path, SearchNode *goalNode, SeDcdtFace *start, SeDcdtFace *goal, int direction) +{ + SrArray backwardsPath; + SeDcdtFace *goal2; + if (goal->link->Degree() == 1) + { + for (int i = 0; i < 3; i++) + { + if (goal->link->Adjacent(i) != NULL) + { + WalkBetween(backwardsPath, goal, goal->link->Adjacent(i), 0.0f); + backwardsPath.pop(); +// WalkBetween(backwardsPath, goal->link->Adjacent(i), goalNode->Triangle(), 0.0f); +// backwardsPath.pop(); + goal2 = goal->link->Adjacent(i); + break; + } + } + } + else + { + goal2 = goal; + } + if (goal->link->Degree() < 3) + { + WalkBetween(backwardsPath, goal2, goalNode->Triangle(), 0.0f, direction); + backwardsPath.pop(); + } +float x, y; + while (goalNode->Back() != NULL) + { +TriangleMidpoint(goalNode->Triangle(), x, y); +sr_out << "SearchNode at (" << x << ", " << y << ")\n"; + SrArray tempPath; + WalkBetween(tempPath, goalNode->Back()->Triangle(), goalNode->Triangle(), 0.0f, goalNode->Direction()); + while (!tempPath.empty()) + { + backwardsPath.push() = tempPath.pop(); + } +// WalkBetween(backwardsPath, goalNode->Triangle(), goalNode->Back()->Triangle(), 0.0f); + backwardsPath.pop(); + goalNode = goalNode->Back(); + } +TriangleMidpoint(goalNode->Triangle(), x, y); +sr_out << "SearchNode at (" << x << ", " << y << ")\n"; + SeDcdtFace *start2; + if (start->link->Degree() == 1) + { + for (int i = 0; i < 3; i++) + { + if (start->link->Adjacent(i) != NULL) + { + WalkBetween(path, start, start->link->Adjacent(i), 0.0f); + path.pop(); +// WalkBetween(path, start->link->Adjacent(i), goalNode->Triangle(), 0.0f); + start2 = start->link->Adjacent(i); + break; + } + } + } + else + { + start2 = start; + } + if (start->link->Degree() < 3) + { + WalkBetween(path, start2, goalNode->Triangle(), 0.0f, goalNode->Direction()); + } + while (!backwardsPath.empty()) + { + path.push() = backwardsPath.pop(); + } + path.pop(); +/* +float x, y; +sr_out << "\n{"; +for (int i = 0; i < path.size() - 1; i++) +{ +TriangleMidpoint((SeDcdtFace *)path[i]->fac(), x, y); +sr_out << "(" << x << ", " << y << "), "; +} +TriangleMidpoint((SeDcdtFace *)path[path.size() - 1], x, y); +sr_out << "(" << x << ", " << y << ")}\n\n"; +*/ +} + +//searches the abstract graph for a path between two points +bool SeDcdt::SearchPath(/*SrArray& path,*/ float x1, float y1, float x2, float y2, float r, int update) +{ +sr_out << "SEARCH PATH STARTED:\n"; + //initialize the path to empty + currentPath.size(0); + SeBase *start, *goal; + //find the triangle containing the start point + SeTriangulator::LocateResult startResult = LocatePoint(x1, y1, start); + //if it wasn't found, return a path could not be found + if (startResult == SeTriangulator::LocateResult::NotFound) + { + return false; + } + //find the triangle containing the goal point + SeTriangulator::LocateResult goalResult = LocatePoint(x2, y2, goal); + //if it couldn't, return a failure + if (goalResult == SeTriangulator::LocateResult::NotFound) + { + return false; + } + SeDcdtFace *startFace = (SeDcdtFace *)start->fac(); + SeDcdtFace *goalFace = (SeDcdtFace *)goal->fac(); +sr_out << "START AND GOAL FACES FOUND\n"; + //if either triangle isn't abstracted, this search fails + //also if the triangles aren't in the same connected component, no path connecting them exists + if ((startFace->link == NULL) || (goalFace->link == NULL) + || (startFace->link->Component() != goalFace->link->Component())) + { +sr_out << ((startFace->link == NULL) ? "start face not abstracted" : (goalFace->link == NULL) ? +"goal face not abstracted" : "start and goal in different components") << srnl; + return false; + } +sr_out << "START AND GOAL ARE ABSTRACTED AND IN THE SAME COMPONENT\n"; + //check that the start and goal points aren't within the radius of any constraints + if ((!ValidPosition(x1, y1, startFace, r)) || (!ValidPosition(x2, y2, goalFace, r))) + { + //if either are, return false + return false; + } + //if the start and goal are in the same triangle, + if (startFace == goalFace) + { + //the path is a straight line + currentPath.push().set(x1, y1); + currentPath.push().set(x2, y2); + return true; + } +sr_out << "START AND GOAL ARE VALID POSITIONS AND NOT IN THE SAME TRIANGLE\n"; + //if both start and goal faces are degree-1, + if ((startFace->link->Degree() == 1) && (goalFace->link->Degree() == 1)) + { + //checks if the start (and thus the goal as well) is in a tree component + bool inTree = true; + //(true if there are no adjacent degree-2 nodes) + for (int i = 0; i < 3; i++) + { + if (startFace->link->Adjacent(i) != NULL) + { + inTree = false; + } + } + //if they are in a tree component, + if (inTree) + { + //perform search in it + SrArray channel; + if (Degree1Path(channel, x1, y1, startFace, x2, y2, goalFace, r)) + { + GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); + return true; + } + else + { + return false; + } + } + } +sr_out << "START AND GOAL ARE NOT IN A TREE\n"; + //if the start face is degree-1, moves to the adjacent degree-2 node + SeDcdtFace *startFace2; + float startAngle; + float startChoke; + if (startFace->link->Degree() == 1) + { + for (int i = 0; i < 3; i++) + { + SeDcdtFace *adjacent = startFace->link->Adjacent(i); + if (adjacent != NULL) + { + startFace2 = adjacent; + startAngle = startFace->link->Angle(i); + startChoke = startFace->link->Choke(i); + break; + } + } + } + else + { + startFace2 = startFace; + startAngle = 0.0f; + startChoke = INFINITY; + } + //if the goal face is degree-1, moves to the adjacent degree-2 node + SeDcdtFace *goalFace2; + float goalAngle; + float goalChoke; + if (goalFace->link->Degree() == 1) + { + for (int i = 0; i < 3; i++) + { + SeDcdtFace *adjacent = goalFace->link->Adjacent(i); + if (adjacent != NULL) + { + goalFace2 = adjacent; + goalAngle = goalFace->link->Angle(i); + goalChoke = goalFace->link->Choke(i); + break; + } + } + } + else + { + goalFace2 = goalFace; + goalAngle = 0.0f; + goalChoke = INFINITY; + } + //if the start and goal faces are both degree-1 or 2, + if ((startFace->link->Degree() < 3) && (goalFace->link->Degree() < 3)) + { + //checks if the start and goal have the same root, + if (startFace2 == goalFace2) + { + //if the start face is at the root of the tree that the goal face is in, + if (startFace2 == startFace) + { + SrArray channel; + //walk from the goal face to the start face + if (WalkBetween(channel, goalFace, startFace, r)) + { + channel.revert(); +channel.pop(); + GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); + return true; + } + else + { + return false; + } + } + //if the goal face is at the root of the tree that the start face is in, + else if (goalFace2 == goalFace) + { + SrArray channel; + //walk from the start face to the goal face + if (WalkBetween(channel, startFace, goalFace, r)) + { +channel.pop(); + GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); + return true; + } + else + { + return false; + } + } + //if the start and goal are both in the same tree component, + else + { + SrArray channel; + //perform search in tree component + if (Degree1Path(channel, x1, y1, startFace, x2, y2, goalFace, r)) + { + GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); + return true; + } + else + { + return false; + } + } + } + } +sr_out << "ONE IS NOT AT THE ROOT OF THE OTHER\n"; + //if we can't get from the start or goal to the rest of the graph, no path exists + if ((startChoke < 2.0f * r) || (goalChoke < 2.0f * r)) + { +sr_out << "startChoke = " << startChoke << ", goalChoke = " << goalChoke << srnl; + return false; + } +sr_out << "START AND GOAL CAN GET ON TO THE GRAPH\n"; + //TODO: if the nodes are both in a degree-2 ring, walk from the start to the goal (both ways) + //TODO: checking that the path is wide enough at all times + //TODO: if both paths are wide enough, run the funnel algorithm and take the shorter path + //TODO: if only one is wide enough, take that, and if neither are, return false + if ((startFace2->link->Degree() == 2) && (goalFace2->link->Degree() == 2)) + { + for (int i = 0; i < 3; i++) + { + if (startFace2->link->Adjacent(i) != NULL) + { + break; + } + } + if (i >= 3) + { + //degree-2 ring! + SrArray startPath, goalPath, leftPath, rightPath; + if (startFace == startFace2) + { + startPath.size(0); + } + else if (!WalkBetween(startPath, startFace, startFace2, r)) + { + return false; + } + if (goalFace == goalFace2) + { + goalPath.size(0); + } + else if (!WalkBetween(goalPath, goalFace, goalFace2, r)) + { + return false; + } + goalPath.revert(); + SeBase *s = startFace2->se(); + bool leftValid, rightValid; + for (int i = 0; i < 3; i++) + { + if (startFace2->link->Angle(i) == INVALID) + { + rightValid = Degree2Path(rightPath, startFace2, (SeDcdtFace *)s->nxt()->sym()->fac(), goalFace2, r); + leftValid = Degree2Path(leftPath, startFace2, (SeDcdtFace *)s->nxt()->nxt()->sym()->fac(), goalFace2, r); + break; + } + s = s->nxt(); + } + if (!leftValid && !rightValid) + { + return false; + } + SrArray fullLeftPath, fullRightPath; + fullLeftPath.size(0); + fullRightPath.size(0); + if (leftValid && CanCross(startPath[startPath.size() - 2], startPath[startPath.size() - 1], leftPath[0], r) + && CanCross(goalPath[goalPath.size() - 2], goalPath[goalPath.size() - 1], leftPath[leftPath.size() - 1], r)) + { + for (int i = 0; i < startPath.size(); i++) + { + fullLeftPath.push() = startPath[i]; + } + for (int i = 0; i < leftPath.size(); i++) + { + fullLeftPath.push() = leftPath[i]; + } + for (int i = 0; i < goalPath.size(); i++) + { + fullLeftPath.push() = goalPath[i]; + } + } + if (rightValid && CanCross(startPath[startPath.size() - 2], startPath[startPath.size() - 1], rightPath[0], r) + && CanCross(goalPath[goalPath.size() - 2], goalPath[goalPath.size() - 1], rightPath[rightPath.size() - 1], r)) + { + for (int i = 0; i < startPath.size(); i++) + { + fullLeftPath.push() = startPath[i]; + } + for (int i = 0; i < rightPath.size(); i++) + { + fullLeftPath.push() = rightPath[i]; + } + for (int i = 0; i < goalPath.size(); i++) + { + fullLeftPath.push() = goalPath[i]; + } + } + if ((fullLeftPath.size() == 0) && (fullRightPath.size() == 0)) + { + return false; + } + else if ((fullLeftPath.size() > 0) && (fullRightPath.size() > 0)) + { + SrPolygon funnelLeft, funnelRight; + float leftLength = GetShortestPath(funnelLeft, fullLeftPath, x1, y1, x2, y2, r); + float rightLength = GetShortestPath(funnelRight, fullRightPath, x1, y1, x2, y2, r); + if (leftLength < rightLength) + { + fullRightPath.size(0); + //maybe save other values? + } + else + { + fullLeftPath.size(0); + //maybe save other values? + } + } + SrArray channel; + if (fullRightPath.size() == 0) + { + for (int i = 0; i < fullLeftPath.size(); i++) + { + channel.push() = fullLeftPath[i]; + } + } + else //if (fullLeftPath.size() == 0) + { + for (int i = 0; i < fullRightPath.size(); i++) + { + channel.push() = fullRightPath[i]; + } + } + GetShortestPath(currentPath, channel, x1, y1, x2, y2, r); + return true; + } + } +sr_out << "NOT A DEGREE-2 RING\n"; + //TODO: next, if either start or goal triangle is degree-2, move to adjacent degree-3 node(s) + //TODO: (whenever performing these "moves", record the distance or angle travelled) + + //TODO: search (A* or IDA*?) from start degree-3 node(s) to goal ones on degree-3 nodes only + //TODO: whenever a path is found, run funnel algorithm to determine the length + //TODO: if this path is shorter than the shortest path so far, it becomes the shortest + //TODO: ends when either there are no more nodes to try (connected component exhausted) + //TODO: or the shortest path being searched is >= the shortest path found + //TODO: return true and the shortest path found, false if none were found +// SrArray startNodes; +// std::priority_queue q; + SeDcdtFace *startTriangle1 = NULL, *startTriangle2 = NULL; + std::priority_queue, check> q; + if (startFace2->link->Degree() == 3) + { + SrPnt2 p = ClosestPointTo(startFace2, x2, y2, r); +// startNodes.push() = new SearchNode(Maximum(Length(p.x, p.y, x1, y1), startAngle * r), +// Length(p.x, p.y, x2, y2), p, startFace2, NULL); + q.push(new SearchNode(0.0f, //Maximum(Length(p.x, p.y, x1, y1), startAngle * r), + Length(p.x, p.y, x2, y2), p, startFace2, NULL)); + } + else + { + SeBase *s = startFace2->se(); +// SeDcdtFace *first = NULL; + for (int i = 0; i < 3; i++) + { + SeDcdtFace *face = startFace2->link->Adjacent(i); + if (face == NULL) + { + s = s->nxt(); + continue; + } + ((startTriangle1 == NULL) ? startTriangle1 : startTriangle2) = face; + SrPnt2 p = ClosestPointTo(face, x2, y2, r); + SrPnt2 p2 = ClosestPointTo(face, x1, y1, r); + SrPnt2 p3 = ClosestPointTo(startFace2, x1, y1, r); + float distance1 = Maximum(startAngle * r, Length(x1, y1, p3.x, p3.y)); + float distance2 = startFace2->link->Angle(i) * r; + float x_1, y_1, x_2, y_2, x_3, y_3; + TriangleVertices(s, x_1, y_1, x_2, y_2, x_3, y_3); + float theta = (startFace2->link->Adjacent((i + 1) % 3) == NULL) ? + AngleBetween(x_1, y_1, x_2, y_2, x_3, y_3) : AngleBetween(x_2, y_2, x_1, y_1, x_3, y_3); + q.push(new SearchNode(Maximum(Length(p2.x, p2.y, x1, y1), distance1 + distance2 + theta * r), + Length(p.x, p.y, x2, y2), p, face, NULL, i)); + s = s->nxt(); + } + } + SrArray goalNodes; + if (goalFace2->link->Degree() == 3) + { + SrPnt2 p = ClosestPointTo(goalFace2, x2, y2, r); + goalNodes.push() = SearchNode(Length(p.x, p.y, x1, y1), + Length(p.x, p.y, x2, y2), p, goalFace2, NULL); + } + else + { +// SeDcdtFace *first = NULL; + for (int i = 0; i < 3; i++) + { + SeDcdtFace *face = goalFace2->link->Adjacent(i); + if (face == NULL) + { + continue; + } +//TODO: better estimation of distance to goal (and what of g-value?) + SrPnt2 p = ClosestPointTo(face, x2, y2, r); + goalNodes.push(SearchNode(0.0f, //Maximum(Length(p.x, p.y, x1, y1), startAngle * r), + Length(p.x, p.y, x2, y2), p, face, NULL, i)); + } + } +int expanded = 0; + float shortestPathLength = INFINITY; + SrPolygon shortestPath; + float toGoal = Minimum(goalNodes[0].h(), (goalNodes.size() > 1) ? goalNodes[1].h() : INFINITY); + if ((q.size() == 2) && (goalNodes.size() == 2) && + (((startTriangle1 == goalNodes[0].Triangle()) && (startTriangle2 == goalNodes[1].Triangle())) || + ((startTriangle1 == goalNodes[1].Triangle()) && (startTriangle2 == goalNodes[0].Triangle())))) + { + //if they're on the same edge, walk between them for one path, + // set start to one and goal to other degree-3 nodes and continue + SeDcdtFace *face = startFace2; + SeDcdtFace *destinationFace; + float destinationAngle; + for (int i = 0; i < 3; i++) + { + if (face->link->Adjacent(i) == goalNodes[0].Triangle()) + { + destinationAngle = face->link->Angle(i); + break; + } + } + for (int i = 0; i < 3; i++) + { + if (goalFace2->link->Adjacent(i) == goalNodes[0].Triangle()) + { + destinationFace = goalNodes[(goalFace2->link->Angle(i) < destinationAngle) ? 0 : 1].Triangle(); + break; + } + } +float x, y; +TriangleMidpoint(destinationFace, x, y); +sr_out << "destinationFace = (" << x << ", " << y << ")\n"; + SrArray channel; +// bool sameEdge = false; + while (true) + { +TriangleMidpoint(face, x, y); +sr_out << "face = (" << x << ", " << y << ")\n"; + if (face == destinationFace) + { + //not on the same edge - skip to below + break; + } + else if (face == goalFace2) + { + //on the same edge - construct channel: startFace -> startFace2 -> goalFace2 -> goalFace + //calculate path and measure length and save values +// sameEdge = true; + SrArray startPath; + WalkBetween(startPath, startFace, startFace2, 0.0f); + startPath.pop(); + SrArray goalPath; + WalkBetween(goalPath, goalFace, goalFace2, 0.0f); + channel.revert(); +float x, y; +sr_out << "startPath:\n"; +for (int i = 0; i < startPath.size(); i++) +{ +TriangleMidpoint(((SeDcdtFace *)startPath[i]->fac()), x, y); +sr_out << "(" << x << ", " << y << ")\n"; +} +sr_out << "channel:\n"; +for (int i = 0; i < channel.size(); i++) +{ +TriangleMidpoint(((SeDcdtFace *)channel[i]->fac()), x, y); +sr_out << "(" << x << ", " << y << ")\n"; +} +sr_out << "goalPath:\n"; +for (int i = 0; i < goalPath.size(); i++) +{ +TriangleMidpoint(((SeDcdtFace *)goalPath[i]->fac()), x, y); +sr_out << "(" << x << ", " << y << ")\n"; +} + while (!channel.empty()) + { + startPath.push() = channel.pop(); + } + while (!goalPath.empty()) + { + startPath.push() = goalPath.pop(); + } + startPath.pop(); + if (ValidPath(startPath, x1, y1, x2, y2, r)) + { + shortestPathLength = GetShortestPath(shortestPath, startPath, x1, y1, x2, y2, r); + } + //then make goalNodes only the one at destinationFace and remove that one from q + //then skip down to searching + goalNodes.remove((goalNodes[0].Triangle() == destinationFace) ? 1 : 0); + if (q.top()->Triangle() == destinationFace) + { + q.pop(); + } + else + { + SearchNode *temp = q.top(); + q.pop(); + q.pop(); + q.push(temp); + } + break; + } + SeBase *s = face->se(); + for (int i = 0; i < 3; i++) + { + if (face->link->Adjacent(i) == destinationFace) + { + channel.push() = s; + face = (SeDcdtFace *)s->sym()->fac(); + break; + } + s = s->nxt(); + } + } +/* + //if they just have the same endpoints, walk both ways around and choose the shortest + if (!sameEdge) + { + while (!channel.empty()) + { + channel.pop(); + } + SrArray startPath, startLeftPath, startRightPath, goalPath, goalLeftPath, goalRightPath; + WalkBetween(startPath, startFace, startFace2, 0.0f); + WalkBetween(startLeftPath, startFace2, goalNodes[0].Triangle(), 0.0f); + WalkBetween(startRightPath, startFace2, goalNodes[1].Triangle(), 0.0f); + WalkBetween(goalPath, goalFace, goalFace2, 0.0f); + WalkBetween(goalLeftPath, goalFace2, goalNodes[0].Triangle(), 0.0f); + WalkBetween(goalRightPath, goalFace2, goalNodes[1].Triangle(), 0.0f); + SrArray leftChannel, rightChannel; + for (int i = 0; i < startPath.size() - 1; i++) // int i = 1 ? + { + leftChannel.push() = startPath[i]; + rightChannel.push() = startPath[i]; + } + for (int i = 0; i < startLeftPath.size() - 1; i++) + { + leftChannel.push() = startLeftPath[i]; + } + for (int i = 0; i < startRightPath.size() - 1; i++) + { + rightChannel.push() = startRightPath[i]; + } + for (int i = goalLeftPath.size() - 1; i > 0; i--) + { + leftChannel.push() = goalLeftPath[i]; + } + for (int i = goalRightPath.size() - 1; i > 0; i--) + { + rightChannel.push() = goalRightPath[i]; + } + for (int i = goalPath.size() - 1; i >= 0; i--) // i > 0 ? + { + leftChannel.push() = goalPath[i]; + rightChannel.push() = goalPath[i]; + } + SrPolygon leftPath, rightPath; + float leftPathLength = (ValidPath(leftChannel, x1, y1, x2, y2, r)) ? + GetShortestPath(leftPath, leftChannel, x1, y1, x2, y2, r) : INFINITY; + float rightPathLength = (ValidPath(rightChannel, x1, y1, x2, y2, r)) ? + GetShortestPath(rightPath, rightChannel, x1, y1, x2, y2, r) : INFINITY; + if (Minimum(leftPathLength, rightPathLength) == INFINITY) + { + return false; + } + currentPath = (leftPathLength < rightPathLength) ? leftPath : rightPath; + return true; + } +*/ + } +/* + SrArray firstChannel; + if (IDA(startNodes, goalNodes, firstChannel, r)) + { + //construct channel + SrPolygon firstPath; + float depthBound = GetShortestPath(firstPath, firstChannel, x1, y1, x2, y2, r); + SearchNode *goalNode; + IDA(startNodes, goalNodes, goalNode, depthBound, r); + } +*/ + while (!q.empty()) + { + SearchNode *current = q.top(); + q.pop(); +float x, y; +TriangleMidpoint(current->Triangle(), x, y); +sr_out << "Expanding (" << x << ", " << y << ") g = " << +current->g() << ", h = " << current->h() << ", f = " << current->f() << "\n"; + if ((current->f() + toGoal) >= shortestPathLength) + { + while (!q.empty()) + { + q.top()->Close(); + delete q.top(); + q.pop(); + } + break; + } + if (current->g() >= current->Triangle()->link->ShortestPath(update) + current->Triangle()->link->LongestEdge()) + { +sr_out << "PRUNED!\n"; + continue; + } +expanded++; + bool found = false; + for (int i = 0; i < goalNodes.size(); i++) + { + if (current->Triangle() == goalNodes[i].Triangle()) + { + SrArray channel; + ConstructBasePath(channel, current, startFace, goalFace, goalNodes[i].Direction()); + current->Close(); + delete current; + if (ValidEndpoints(channel, x1, y1, x2, y2, r)) + { + SrPolygon currentShortestPath; + float currentPathLength = GetShortestPath(currentShortestPath, channel, x1, y1, x2, y2, r); +char string[20]; +sprintf(string, "%f", currentPathLength); +if (strcmp(string, "-1.#IND00") == 0)//((currentPathLength < 0.0f) || (currentPathLength > 100.0f)) +{ +float x, y; +sr_out << "CHANNEL = {"; +for (int j = 0; j < channel.size() - 1; j++) +{ +TriangleMidpoint((SeDcdtFace *)channel[j]->fac(), x, y); +sr_out << "(" << x << ", " << y << "), "; +} +TriangleMidpoint((SeDcdtFace *)channel[channel.size() - 1]->fac(), x, y); +sr_out << "(" << x << ", " << y << ")}\n"; +sr_out << "PATH = {"; +for (int j = 0; j < currentShortestPath.size() - 1; j++) +{ +sr_out << "(" << currentShortestPath[j].x << ", " << currentShortestPath[j].y << "), "; +} +sr_out << "(" << currentShortestPath[currentShortestPath.size() - 1].x +<< ", " << currentShortestPath[currentShortestPath.size() - 1].y << ")}\n"; +} +sr_out << "GENERATED PATH WITH LENGTH = " << currentPathLength << "\n"; + if (currentPathLength < shortestPathLength) + { + shortestPathLength = currentPathLength; + shortestPath.size(0); + shortestPath = currentShortestPath; + } + } + found = true; + } + } + if (found) + { + continue; + } +// SeBase *s = current.Triangle()->se(); + for (int i = 0; i < 3; i++) + { + if (((current->Triangle() == startTriangle1) && (current->Triangle()->link->Adjacent(i) == startTriangle2)) + || ((current->Triangle() == startTriangle2) && (current->Triangle()->link->Adjacent(i) == startTriangle1))) + { + continue; + } + if (current->Triangle()->link->Adjacent(i) != NULL) //!Blocked(s)) + { + SeDcdtFace *childFace = current->Triangle()->link->Adjacent(i); //(SeDcdtFace *)s->sym()->fac(); + float width = (current->Back() == NULL) ? INFINITY : + (current->Triangle()->link->Adjacent((i + 1) % 3) == current->Back()->Triangle()) ? + current->Triangle()->link->Width(i) : current->Triangle()->link->Width((i + 2) % 3); + width = Minimum(current->Triangle()->link->Choke(i), width); + if ((!current->Searched(childFace)) && (width >= 2.0f * r) + && ((current->Back() == NULL) || (childFace != current->Back()->Triangle()))) + { + SrPnt2 p = ClosestPointTo(childFace, x2, y2, r); +// float x_1, y_1, x_2, y_2, x_3, y_3; +// TriangleVertices(s, x_1, y_1, x_2, y_2, x_3, y_3); +// float theta = (current.Back() == NULL) ? 0.0f : (current.Triangle()->link->Adjacent((i + 1) % 3) == current.Back()->Triangle()) ? +// AngleBetween(x_1, y_1, x_2, y_2, x_3, y_3) : AngleBetween(x_3, y_3, x_1, y_1, x_2, y_2); + float h = Length(p.x, p.y, x2, y2); + float minDistance = current->g() + Maximum(current->Triangle()->link->Angle(i) * r, current->h() - h); +SrPnt2 p2 = ClosestPointTo(childFace, x1, y1, r); + minDistance = Maximum(Length(x1, y1, p2.x, p2.y), minDistance); + if (minDistance < childFace->link->ShortestPath(update)) + { + childFace->link->ShortestPath(minDistance, update); + } + else if (minDistance >= childFace->link->ShortestPath(update) + childFace->link->LongestEdge()) + { +sr_out << "PREPRUNED!\n"; + continue; + } + q.push(new SearchNode(minDistance, h, p, childFace, current, i)); + current->OpenChild(); +TriangleMidpoint(childFace, x, y); +sr_out << " Generated (" << x << ", " << y << ") g = " +<< minDistance << ", h = " << h << ", f = " << (minDistance + h) << "\n"; + } + } +// s = s->nxt(); + } + if (current->OpenChildren() <= 0) + { + current->Close(); + delete current; + } + } + //TODO: make sure the SeBase*s are being added to the paths the right way + //TODO: (interior edges => path lengths should be # of triangles in path - 1) + //TODO: improve way GetShortestPath gets the vertices (see SeTriangulator::get_channel_boundary()) + //TODO: save stuff to member variables? +sr_out << expanded << " NODES EXPANDED IN SEARCH\n"; + currentPath = shortestPath; + return (shortestPathLength < INFINITY); +} + +//searches the abstract graph for a path between two points +bool SeDcdt::SearchPathBase(float x1, float y1, float x2, float y2, float r, int update) +{ +int expanded = 0; + //initialize the path to empty + currentPath.size(0); + SeBase *start;//, *goal; + //find the triangle containing the start point + SeTriangulator::LocateResult startResult = LocatePoint(x1, y1, start); + //if it wasn't found, return a path could not be found + if (startResult == SeTriangulator::LocateResult::NotFound) + { + return false; + } + SeDcdtFace *startFace = (SeDcdtFace *)start->fac(); + std::priority_queue, check> q; + SrPnt2 p = ClosestPointTo(startFace, x2, y2, r); + q.push(new SearchNode(0.0f /*Length(x1, y1, p.x, p.y)*/, Length(p.x, p.y, x2, y2), p, startFace, NULL)); + float shortestPathLength = INFINITY; + SrPolygon shortestPath; + while (!q.empty()) + { + SearchNode *current = q.top(); + q.pop(); +expanded++; +float x, y; +TriangleMidpoint(current->Triangle(), x, y); +sr_out << "Expanding (" << x << ", " << y << ") g = " << +current->g() << ", h = " << current->h() << ", f = " << current->f() << "\n"; + if (current->f() >= shortestPathLength) + { + while (!q.empty()) + { + q.top()->Close(); + delete q.top(); + q.pop(); + } + break; + } + else if (current->g() >= current->Triangle()->link->ShortestPath(update) + current->Triangle()->link->LongestEdge()) + { +sr_out << "PRUNED!\n"; + continue; + } + else if (InTriangle(current->Triangle(), x2, y2)) + { + SrArray channel = ConstructBaseChannel(current); + current->Close(); + delete current; + if (ValidEndpoints(channel, x1, y1, x2, y2, r)) + { + SrPolygon currentShortestPath; + float currentPathLength = GetShortestPath(currentShortestPath, channel, x1, y1, x2, y2, r); +sr_out << "GENERATED PATH WITH LENGTH = " << currentPathLength << "\n"; + if (currentPathLength < shortestPathLength) + { + shortestPathLength = currentPathLength; + shortestPath.size(0); + shortestPath = currentShortestPath; + } + } + continue; + } + SeBase *s = current->Triangle()->se(); + for (int i = 0; i < 3; i++) + { + if (!Blocked(s)) + { + SeDcdtFace *childFace = (SeDcdtFace *)s->sym()->fac(); + float width = (current->Back() == NULL) ? INFINITY : (s->nxt()->sym()->fac() == current->Back()->Triangle()) ? + current->Triangle()->link->Width(i) : current->Triangle()->link->Width((i + 2) % 3); + if ((!current->Searched(childFace)) && (width >= 2.0f * r) + && ((current->Back() == NULL) || (childFace != current->Back()->Triangle()))) + { + p = ClosestPointTo(childFace, x2, y2, r); + float x_1, y_1, x_2, y_2, x_3, y_3; + TriangleVertices(s, x_1, y_1, x_2, y_2, x_3, y_3); + float theta = (current->Back() == NULL) ? 0.0f : (s->nxt()->sym()->fac() == current->Back()->Triangle()) ? + AngleBetween(x_1, y_1, x_2, y_2, x_3, y_3) : AngleBetween(x_3, y_3, x_1, y_1, x_2, y_2); + float h = Length(p.x, p.y, x2, y2); + float minDistance = current->g() + Maximum(theta * r, current->h() - h); +SrPnt2 p2 = ClosestPointTo(childFace, x1, y1, r); + minDistance = Maximum(Length(x1, y1, p2.x, p2.y), minDistance); +// minDistance = Maximum(Length(x1, y1, p.x, p.y), minDistance); + if (minDistance < childFace->link->ShortestPath(update)) + { + childFace->link->ShortestPath(minDistance, update); + } + else if (minDistance >= childFace->link->ShortestPath(update) + childFace->link->LongestEdge()) + { +sr_out << "PREPRUNED!\n"; + continue; + } + q.push(new SearchNode(minDistance, h, p, childFace, current)); + current->OpenChild(); +TriangleMidpoint(childFace, x, y); +sr_out << " Generated (" << x << ", " << y << ") g = " +<< minDistance << ", h = " << h << ", f = " << (minDistance + h) << "\n"; + } + } + s = s->nxt(); + } + if (current->OpenChildren() <= 0) + { + current->Close(); + delete current; + } + } +sr_out << expanded << " NODES EXPANDED IN SEARCH\n"; + currentPath = shortestPath; + //TODO: save results as local variables? + return (shortestPathLength < INFINITY); +} + +//construct the channel given the last SearchNode in the solution +SrArray SeDcdt::ConstructBaseChannel(SearchNode *goal) +{ + SrArray channel; + if (goal->Back() == NULL) + { + return channel; + } + SearchNode *current = goal->Back(); + while (current->Back() != NULL) + { + SeBase *s = current->Triangle()->se(); + for (int i = 0; i < 3; i++) + { + if (s->sym()->fac() == current->Back()->Triangle()) + { + channel.push() = s; + break; + } + s = s->nxt(); + } + current = current->Back(); + } + channel.revert(); + return channel; +} + +//uses the modified funnel algorithm to get the shortest path for nonzero-radius units +//and return the length of this path +float SeDcdt::GetShortestPath(SrPolygon &path, SrArray Channel, float x1, float y1, float x2, float y2, float r) +{ + //initializes the path + path.open(true); + path.size(0); + //gets the edges that make up the channel +// SrArray Channel = _triangulator->get_channel_interior_edges(); + //if there are no edges, there is no path + if (Channel.size() < 1) + { + return 0.0f; + } + //if there is only one, the path is a straight line + else if (Channel.size() < 2) + { + path.push().set(x1, y1); + path.push().set(x2, y2); + return Length(x1, y1, x2, y2); + } + //otherwise, initialize a funnel deque + //with the starting point + FunnelDeque fd(x1, y1, r); + //get the first edge in the channel + SeBase *s = Channel[0]; + //get the reference whose sym edge + //belongs to the next triangle in the channel + for (int i = 0; i < 3; i++) + { + if (s->sym()->fac() == Channel[1]->fac()) + { + break; + } + s = s->nxt(); + } + //get the point on the right side of that edge + SrPnt2 p; + p.set(((SeDcdtVertex *)s->vtx())->p); + //insert it into the deque + fd.Add(p, FunnelDeque::CornerType::LeftTangent, path); + SeVertex *right = s->vtx(); + //get the point on the left side of that edge + s = s->nxt(); + SeVertex *left = s->vtx(); + p.set(((SeDcdtVertex *)s->vtx())->p); + //insert it into the deque + fd.Add(p, FunnelDeque::CornerType::RightTangent, path); + //go through the other edges in the funnel + for (int i = 1; i < Channel.size() - 1; i++) + { + s = Channel[i]; + //get the reference whose sym edge + //belongs to the next triangle in the channel + for (int j = 0; j < 3; j++) + { + if (s->sym()->fac() == Channel[i + 1]->fac()) + { + break; + } + s = s->nxt(); + } + //if that edge shares a vertex with the right side + //of the funnel, the opposite point should be + //added to the left side of the funnel + if (s->vtx() == right) + { + p.set(((SeDcdtVertex *)s->nxt()->vtx())->p); + fd.Add(p, FunnelDeque::CornerType::RightTangent, path); + left = s->nxt()->vtx(); + } + //otherwise, add it to the right side of the funnel + else //(s->nxt()->vtx() == left) + { + p.set(((SeDcdtVertex *)s->vtx())->p); + fd.Add(p, FunnelDeque::CornerType::LeftTangent, path); + right = s->vtx(); + } + } + //go to the last edge in the channel + s = Channel[Channel.size() - 1]; + //find the reference across which is + //the triangle with the goal in it + for (int j = 0; j < 3; j++) + { + if (InTriangle((SeDcdtFace *)s->sym()->fac(), x2, y2)) + { + break; + } + s = s->nxt(); + } + //like above, determines whether to add + //the point to the left or right side of the funnel + if (s->vtx() == right) + { + p.set(((SeDcdtVertex *)s->nxt()->vtx())->p); + fd.Add(p, FunnelDeque::CornerType::RightTangent, path); + } + else //(s->nxt()->vtx() == left) + { + p.set(((SeDcdtVertex *)s->vtx())->p); + fd.Add(p, FunnelDeque::CornerType::LeftTangent, path); + } + //finally, add the goal point to the funnel + p.set(x2, y2); + fd.Add(p, FunnelDeque::CornerType::Point, path); + //calculate the length of the path sand return it + return fd.Length(path); +} + +SrPolygon SeDcdt::GetChannelBoundary() +{ + SrPolygon boundary; + return boundary; +} + +SrPolygon &SeDcdt::GetPath() +{ + return currentPath; +} + +//Abstract space searching function definitions } diff --git a/source/dcdt/se/SearchNode.h b/source/dcdt/se/SearchNode.h new file mode 100644 index 0000000000..17201736f3 --- /dev/null +++ b/source/dcdt/se/SearchNode.h @@ -0,0 +1,135 @@ +//SearchNode.h + +//DJD: definition of the SearchNode class { + +#ifndef SEARCHNODE_H +#define SEARCHNODE_H + +#include "se_dcdt.h" + +//class used to store a state in the search functions +class SearchNode +{ +protected: + //the (minimum) distance travelled since the start node + float m_dMinDistance; + //the (admissible and consistent) estimate of the distance to the goal + float m_dHeuristic; + //the closest point in this triangle to the goal + SrPnt2 m_CClosestPoint; + //the triangle (face) associated with this node + SeDcdtFace *m_CTriangle; + //the parent search node of this one + SearchNode *m_CBack; + //the number of open children + int m_nOpenChildren; + //the direction taken from the last node to this one + int m_nDirection; +public: + //constructor - initializes the member variables + SearchNode(float dMinDistance, float dHeuristic, SrPnt2 CClosestPoint, SeDcdtFace *CTriangle, SearchNode *CBack, int nDirection = INVALID) + { + m_dMinDistance = dMinDistance; + m_dHeuristic = dHeuristic; + m_CClosestPoint.x = CClosestPoint.x; + m_CClosestPoint.y = CClosestPoint.y; + m_CTriangle = CTriangle; + m_CBack = CBack; + m_nOpenChildren = 0; + m_nDirection = nDirection; + } + + //returns the estimated minimum cost of a path from the start to the goal through this node + float f() + { + return (m_dMinDistance + m_dHeuristic); + } + + //returns the estimated minimum cost of a path from the start to this node + float g() + { + return m_dMinDistance; + } + + //returns the estimated minimum cost of a path from this node to the goal + float h() + { + return m_dHeuristic; + } + + //returns (a copy of) the closest point in this triangle to the goal + SrPnt2 Point() + { + SrPnt2 temp; + temp.x = m_CClosestPoint.x; + temp.y = m_CClosestPoint.y; + return temp; + } + + //returns the triangle associated with this node + SeDcdtFace *Triangle() + { + return m_CTriangle; + } + + //returns the parent search node of this one + SearchNode *Back() + { + return m_CBack; + } + + //returns the direction taken from the last node to this one + int Direction() + { + return m_nDirection; + } + + //sets the number of open children + void OpenChild() + { + m_nOpenChildren++; + } + + //closes a child + bool CloseChild() + { + return ((--m_nOpenChildren) <= 0); + } + + //closes the current search node + //(deleting its parent if it has no other open children) + void Close() + { + if ((m_CBack != NULL) && m_CBack->CloseChild()) + { + m_CBack->Close(); + delete m_CBack; + m_CBack = NULL; + } + } + + //returns the number of open children this node has + int OpenChildren() + { + return m_nOpenChildren; + } + + //checks the ancestors of this node to see if a given triangle was already searched + bool Searched(SeDcdtFace *triangle) + { + SearchNode *current = m_CBack; + while (current != NULL) + { + if (current->Triangle() == triangle) + { + return true; + } + current = current->Back(); + } + return false; + } +}; + +#endif + +//definition of the SearchNode class } diff --git a/source/dcdt/se/Utility.cpp b/source/dcdt/se/Utility.cpp new file mode 100644 index 0000000000..ea75704003 --- /dev/null +++ b/source/dcdt/se/Utility.cpp @@ -0,0 +1,248 @@ +//Utility.cpp + +//DJD: Helper function definitions { + +#include "precompiled.h" + +#include + +#include "se_dcdt.h" + +//degree value for an unabstracted triangle +#define UNABSTRACTED -1 + +//the list of unprocessed triangles +template +SrArray *> SeLinkFace::processing; + +//gets the vertex coordinates and whether or not the edges are blocked, for a face +void SeDcdt::TriangleInfo(SeDcdtFace *face, float &x1, float &y1, bool &e1, + float &x2, float &y2, bool &e2, float &x3, float &y3, bool &e3) +{ + TriangleInfo(face->se(), x1, y1, e1, x2, y2, e2, x3, y3, e3); +} + +//gets the vertex coordinates and whether or not the edges are blocked, in a certain order +void SeDcdt::TriangleInfo(SeBase *s, float &x1, float &y1, bool &e1, + float &x2, float &y2, bool &e2, float &x3, float &y3, bool &e3) +{ + TriangleVertices(s, x1, y1, x2, y2, x3, y3); + TriangleEdges(s, e1, e2, e3); +} + +//gets the vertex coordinates of a triangle, for a face +void SeDcdt::TriangleVertices(SeDcdtFace *face, float &x1, float &y1, + float &x2, float &y2, float &x3, float &y3) +{ + TriangleVertices(face->se(), x1, y1, x2, y2, x3, y3); +} + +//gets the vertex coordinates of a triangle, in a certain order +void SeDcdt::TriangleVertices(SeBase *s, float &x1, float &y1, + float &x2, float &y2, float &x3, float &y3) +{ + x1 = ((SeDcdtVertex *)s->vtx())->p.x; + y1 = ((SeDcdtVertex *)s->vtx())->p.y; + s = s->nxt(); + x2 = ((SeDcdtVertex *)s->vtx())->p.x; + y2 = ((SeDcdtVertex *)s->vtx())->p.y; + s = s->nxt(); + x3 = ((SeDcdtVertex *)s->vtx())->p.x; + y3 = ((SeDcdtVertex *)s->vtx())->p.y; +} + +//gets whether or not the edges of a triangle are blocked, for a face +void SeDcdt::TriangleEdges(SeDcdtFace *face, bool &e1, bool &e2, bool &e3) +{ + TriangleEdges(face->se(), e1, e2, e3); +} + +//gets whether or not the edges of a triangle are blocked, in a certain order +void SeDcdt::TriangleEdges(SeBase *s, bool &e1, bool &e2, bool &e3) +{ + e1 = Blocked(s); + s = s->nxt(); + e2 = Blocked(s); + s = s->nxt(); + e3 = Blocked(s); +} + +//gets the midpoint of a triangle +void SeDcdt::TriangleMidpoint(SeDcdtFace *face, float &x, float &y) +{ + float x1, y1, x2, y2, x3, y3; + TriangleVertices(face, x1, y1, x2, y2, x3, y3); + x = (x1 + x2 + x3) / 3.0f; + y = (y1 + y2 + y3) / 3.0f; +} + +//gets the midpoint of a triangle +SrPnt2 SeDcdt::TriangleMidpoint(SeDcdtFace *face) +{ + float x1, y1, x2, y2, x3, y3; + TriangleVertices(face, x1, y1, x2, y2, x3, y3); + SrPnt2 p; + p.x = (x1 + x2 + x3) / 3.0f; + p.y = (y1 + y2 + y3) / 3.0f; + return p; +} + +//gets whether or not an edge is constrained or bordering the outside face +bool SeDcdt::Blocked(SeBase *s) +{ + return (((SeDcdtEdge *)s->edg())->is_constrained() || (s->sym()->fac() == outside)); +} + +//gets the degree of node a face has been abstracted to +int SeDcdt::Degree(SeBase *s) +{ + return Degree(s->fac()); +} + +//gets the degree of node a face has been abstracted to +int SeDcdt::Degree(SeFace *face) +{ + if (((SeDcdtFace *)face)->link == NULL) + { + return UNABSTRACTED; + } + return ((SeDcdtFace *)face)->link->Degree(); +} + +//gets the angle between the given vertex coordinates +float SeDcdt::AngleBetween(float x1, float y1, float x2, float y2, float x3, float y3) +{ + float angle1 = (float)atan2(y1 - y2, x1 - x2); + float angle2 = (float)atan2(y3 - y2, x3 - x2); + float diff = angle1 - angle2; + diff += (diff <= -PI) ? 2.0f * PI : (diff > PI) ? -2.0f * PI : 0.0f; + return diff; +} + +//gets the orientation of the vertices with coordinates given +//(0 for collinear, <0 for clockwise, >0 for counterclockwise) +float SeDcdt::Orientation(float x1, float y1, float x2, float y2, float x3, float y3) +{ + return ((x1 * y2) + (x2 * y3) + (x3 * y1) - (x1 * y3) - (x2 * y1) - (x3 * y2)); +} + +//determines if an angle is accute +bool SeDcdt::IsAccute(float theta) +{ + return (abs(theta) < (PI / 2.0f)); +} + +//determines if an angle is obtuse +bool SeDcdt::IsObtuse(float theta) +{ + return (abs(theta) > (PI / 2.0f)); +} + +//calculates the distance between two points +float SeDcdt::Length(float x1, float y1, float x2, float y2) +{ + float xDiff = x1 - x2; + float yDiff = y1 - y2; + return (sqrt(xDiff * xDiff + yDiff * yDiff)); +} + +//returns the smaller of the two arguments +float SeDcdt::Minimum(float a, float b) +{ + return ((a < b) ? a : b); +} + +//returns the larger of the two arguments +float SeDcdt::Maximum(float a, float b) +{ + return ((a > b) ? a : b); +} + +//returns the minimum distance between the two segments given +float SeDcdt::SegmentDistance(float A1x, float A1y, float A2x, float A2y, + float B1x, float B1y, float B2x, float B2y) +{ + float d1 = PointSegmentDistance(A1x, A1y, B1x, B1y, B2x, B2y); + float d2 = PointSegmentDistance(A2x, A2y, B1x, B1y, B2x, B2y); + float d3 = PointSegmentDistance(B1x, B1y, A1x, A1y, A2x, A2y); + float d4 = PointSegmentDistance(B2x, B2y, A1x, A1y, A2x, A2y); + float min = Minimum(Minimum(d1, d2), Minimum(d3, d4)); + return min; +} + +//returns the minimum distance between the two segments given, at least r from the endpoints +float SeDcdt::SegmentDistance(float ls1x, float ls1y, float ls2x, float ls2y, + float ll1x, float ll1y, float ll2x, float ll2y, float r) +{ + float theta = atan2(ls2y - ls1y, ls2x - ls1x); + float A1x = ls1x + cos(theta) * r; + float A1y = ls1y + sin(theta) * r; + float A2x = ls2x - cos(theta) * r; + float A2y = ls2y - sin(theta) * r; + theta = atan2(ll2y - ll1y, ll2x - ll1x); + float B1x = ll1x + cos(theta) * r; + float B1y = ll1y + sin(theta) * r; + float B2x = ll2x - cos(theta) * r; + float B2y = ll2y - sin(theta) * r; + return SegmentDistance(A1x, A1y, A2x, A2y, B1x, B1y, B2x, B2y); +} + +//returns the minimum distance between the point and the line segment given +float SeDcdt::PointSegmentDistance(float x, float y, float x1, float y1, float x2, float y2) +{ + if (!IsAccute(AngleBetween(x1, y1, x2, y2, x, y))) + { + return Length(x2, y2, x, y); + } + else if (!IsAccute(AngleBetween(x2, y2, x1, y1, x, y))) + { + return Length(x1, y1, x, y); + } + else + { + return PointLineDistance(x, y, x1, y1, x2, y2); + } +} + +//returns the closest point on the segment given, at least r in from the endpoints, to the point given +SrPnt2 SeDcdt::ClosestPointOn(float x1, float y1, float x2, float y2, float x, float y, float r) +{ + SrPnt2 point; + float distance; + //checks if the angles along this edge and to the point are accute + bool accute1 = IsAccute(AngleBetween(x1, y1, x2, y2, x, y)); + bool accute2 = IsAccute(AngleBetween(x2, y2, x1, y1, x, y)); + if (accute1 && accute2) + { + //if both are, get closest point on this line + distance = PointLineDistance(x, y, x1, y1, x2, y2); + float theta = atan2(y2 - y1, x2 - x1); + theta += (Orientation(x, y, x1, y1, x2, y2) > 0.0f) ? PI / -2.0f : PI / 2.0f; + theta += (theta <= -PI) ? 2.0f * PI : (theta > PI) ? -2.0f * PI : 0.0f; + point.set(x + cos(theta) * distance, y + sin(theta) * distance); + //if it's < r from either end, move it out to distance r + if (Length(point.x, point.y, x1, y1) < r) + { + accute2 = false; + } + else if (Length(point.x, point.y, x2, y2) < r) + { + accute1 = false; + } + } + if (!accute1 && accute2) + { + //get point distance r from (x2, y2) in the direction of (x1, y1) + float theta = atan2(y1 - y2, x1 - x2); + point.set(x2 + cos(theta) * r, y2 + sin(theta) * r); + } + else if (accute1 && !accute2) + { + //get point distance r from (x1, y1) in the direction of (x2, y2) + float theta = atan2(y2 - y1, x2 - x1); + point.set(x1 + cos(theta) * r, y1 + sin(theta) * r); + } + return point; +} + +//Helper function definitions } diff --git a/source/dcdt/se/Width.cpp b/source/dcdt/se/Width.cpp new file mode 100644 index 0000000000..c348cf3d63 --- /dev/null +++ b/source/dcdt/se/Width.cpp @@ -0,0 +1,149 @@ +//Width.cpp + +//DJD: Width function definitions { +#include "precompiled.h" + +#include "se_dcdt.h" +#include + +//list of unprocessed triangles +template +SrArray *> SeLinkFace::processing; + +//determines the distance between a point and the closest point to it on a line +float SeDcdt::PointLineDistance(float x, float y, float x1, float y1, float x2, float y2) +{ + if (x1 == x2) + { + return abs(x - x1); + } + float rise = y2 - y1; + float run = x2 - x1; + float intercept = y1 - (rise / run) * x1; + float a = rise; + float b = -run; + float c = run * intercept; + return (abs(a * x + b * y + c) / sqrt(a * a + b * b)); +} + +//determines if an edge should be considered when calculating the width +//(if the base angles of the triangle formed by this edge joined with the "origin" point are both accute) +bool SeDcdt::Consider(float x, float y, float x1, float y1, float x2, float y2) +{ + return ((IsAccute(AngleBetween(x, y, x1, y1, x2, y2))) + && (IsAccute(AngleBetween(x, y, x2, y2, x1, y1)))); +} + +//Calculates the meaningful widths of a triangle +void SeDcdt::CalculateWidths(SeDcdtFace *face) +{ + //if the triangle hasn't been abstracted yet, return + if (face->link == NULL) + { + return; + } + //goes through the vertices of the triangle + SeBase *s = face->se(); + for (int i = 0; i < 3; i++) + { + //calculate and set the width between those edges + face->link->Width(i, TriangleWidth(s->nxt())); + s = s->nxt(); + } + float x1, y1, x2, y2, x3, y3; + TriangleVertices(face, x1, y1, x2, y2, x3, y3); +} + +//determine one width through a triangle +float SeDcdt::TriangleWidth(SeBase *s) +{ + //get the coordinates of the triangle + float x, y, x1, y1, x2, y2; + TriangleVertices(s, x, y, x1, y1, x2, y2); + //if either base angle isn't accute, the width of that + //triangle is the length of the shorter edge + if (!IsAccute(AngleBetween(x, y, x1, y1, x2, y2))) + { + return Length(x, y, x1, y1); + } + else if (!IsAccute(AngleBetween(x, y, x2, y2, x1, y1))) + { + return Length(x, y, x2, y2); + } + else + { + //otherwise, calculate the upper bound on the width of the triangle + float CurrentWidth = Minimum(Length(x, y, x1, y1), Length(x, y, x2, y2)); + //and calculate the actual value of the width + return SearchWidth(x, y, s->nxt()->sym(), CurrentWidth); + } +} + +//checks the current triangle for bounds on the triangle width +float SeDcdt::SearchWidth(float x, float y, SeBase *s, float CurrentWidth) +{ + //get the coordinates of the triangle + float x1, y1, x2, y2, x3, y3; + TriangleVertices(s, x1, y1, x2, y2, x3, y3); + //checks if the entrance edge should be considered + if (!Consider(x, y, x1, y1, x2, y2)) + { + //if not, returns the width unmodified + return CurrentWidth; + } + //calculates the distance between this edge and the "origin" point + float Distance = PointLineDistance(x, y, x1, y1, x2, y2); + //if it's farther than the upper bound + if (Distance >= CurrentWidth) + { + return CurrentWidth; + } + //if the edge is constrained, + else if (Blocked(s->sym())) + { + //return this distance as the new upper bound + return Distance; + } + //gets the distance to the vertex opposite the entrance edge + Distance = Length(x, y, x3, y3); + //if this distance is less than the current upper bound, + if (Distance <= CurrentWidth) + { + //return this distance as the new upper bound + return Distance; + } + //otherwise, searches across the other two edges for bounds + CurrentWidth = SearchWidth(x, y, s->nxt()->sym(), CurrentWidth); + return SearchWidth(x, y, s->nxt()->nxt()->sym(), CurrentWidth); +} + +//determines if a certain point (x, y) in face is at least distance r from any constraints +bool SeDcdt::ValidPosition(float x, float y, SeDcdtFace *face, float r) +{ + //get the triangle's vertices + float x1, y1, x2, y2, x3, y3; + TriangleVertices(face, x1, y1, x2, y2, x3, y3); + //check if any are closer than r to (x, y) + if ((Length(x, y, x1, y1) < r) || (Length(x, y, x2, y2) < r) || (Length(x, y, x3, y3) < r)) + { + return false; + } + //get the edges of the triangle + SeBase *s = face->se(); + //go through them one-by-one + for (int i = 0; i < 3; i++) + { + //if there are any constraints within r of (x, y) across the current edge + if (SearchWidth(x, y, s->sym(), r) < r) + { + //return that this in an invalid position + return false; + } + //move to the next edge + s = s->nxt(); + } + //if no constraints were found within r of (x, y), it is a valid position + return true; +} + +//Width function definitions } diff --git a/source/dcdt/se/data/ecol.se b/source/dcdt/se/data/ecol.se new file mode 100644 index 0000000000..8a0b8ea447 --- /dev/null +++ b/source/dcdt/se/data/ecol.se @@ -0,0 +1,163 @@ +SYMEDGE MESH DESCRIPTION + +SymEdges 76 +37 54 0 0 0 +24 3 14 0 8 +1 5 1 1 8 +35 65 14 1 18 +2 7 2 2 8 +67 62 1 2 11 +4 9 3 3 8 +39 67 2 3 19 +6 11 4 4 8 +69 39 3 4 13 +8 13 5 5 8 +75 58 4 5 12 +10 15 6 6 8 +57 32 5 6 14 +12 17 7 7 8 +49 47 6 7 4 +14 19 8 8 8 +26 49 7 8 5 +16 21 9 9 8 +42 26 8 9 1 +18 23 10 10 8 +23 44 9 10 3 +20 25 11 11 8 +45 20 10 11 3 +22 0 0 12 8 +51 40 11 12 9 +48 16 8 13 5 +19 43 12 13 1 +31 51 0 14 10 +54 61 13 14 17 +52 46 12 15 6 +50 29 13 15 10 +71 75 5 16 16 +13 56 15 16 14 +62 72 17 17 7 +64 2 1 17 18 +65 71 15 18 20 +55 1 14 18 0 +9 68 16 19 13 +66 6 3 19 19 +43 45 11 20 2 +25 50 12 20 9 +27 18 9 21 1 +44 41 12 21 2 +40 42 9 22 2 +21 22 11 22 3 +15 48 12 23 4 +30 53 6 23 6 +17 27 12 24 5 +46 14 7 24 4 +28 30 12 25 10 +41 24 0 25 9 +47 31 13 26 6 +61 57 6 26 15 +60 28 0 27 17 +0 36 15 27 0 +53 60 15 28 15 +33 12 6 28 14 +72 69 4 29 21 +11 74 17 29 12 +29 55 15 30 17 +56 52 13 30 15 +73 35 1 31 7 +5 66 16 31 11 +3 34 17 32 18 +70 37 14 32 20 +7 38 16 33 19 +63 4 2 33 11 +58 73 16 34 21 +38 8 4 34 13 +36 64 17 35 20 +74 33 15 35 16 +68 59 17 36 21 +34 63 16 36 7 +32 70 17 37 16 +59 10 5 37 12 + +Vertices 18 +51 -0.471118 -0.471118 0.000000 127 127 127 255 +62 1.006899 -0.406455 0.000000 127 127 127 255 +67 1.256315 0.027713 0.000000 127 127 127 255 +9 1.265552 0.489593 0.000000 127 127 127 255 +69 0.914523 0.748246 0.000000 127 127 127 255 +75 0.572732 0.859097 0.000000 127 127 127 255 +47 -0.147802 0.840622 0.000000 127 127 127 255 +17 -0.840622 0.840622 0.000000 127 127 127 255 +19 -1.182414 0.655870 0.000000 127 127 127 255 +18 -1.487255 0.129327 0.000000 127 127 127 255 +23 -1.441067 -0.286366 0.000000 127 127 127 255 +40 -0.932998 -0.434168 0.000000 127 127 127 255 +50 -0.907948 0.177278 0.000000 127 127 127 255 +61 -0.390490 0.162904 0.000000 127 127 127 255 +3 0.498831 -0.498831 0.000000 127 127 127 255 +36 0.122178 0.138947 0.000000 127 127 127 255 +73 1.135253 0.143426 -0.000000 127 127 127 255 +70 0.702544 0.153150 0.000000 127 127 127 255 + +Edges 38 +0 127 127 127 255 +2 127 127 127 255 +4 127 127 127 255 +6 127 127 127 255 +8 127 127 127 255 +10 127 127 127 255 +12 127 127 127 255 +14 127 127 127 255 +16 127 127 127 255 +18 127 127 127 255 +20 127 127 127 255 +22 127 127 127 255 +24 127 127 127 255 +26 127 127 127 255 +28 255 14 238 255 +30 127 127 127 255 +32 127 127 127 255 +34 127 127 127 255 +36 127 127 127 255 +38 127 127 127 255 +40 127 127 127 255 +42 127 127 127 255 +44 127 127 127 255 +46 255 72 8 255 +48 127 127 127 255 +50 78 100 255 255 +52 120 255 98 255 +54 127 127 127 255 +56 127 127 127 255 +58 127 127 127 255 +60 255 17 78 255 +62 127 127 127 255 +64 127 127 127 255 +66 127 127 127 255 +68 127 127 127 255 +70 255 22 71 255 +72 127 127 127 255 +74 55 8 255 255 + +Faces 22 +37 127 127 127 255 +19 127 127 127 255 +40 127 127 127 255 +23 127 127 127 255 +49 127 127 127 255 +17 127 127 127 255 +47 127 127 127 255 +73 127 127 127 255 +24 127 127 127 255 +51 127 127 127 255 +50 127 127 127 255 +67 127 127 127 255 +75 127 127 127 255 +69 127 127 127 255 +13 127 127 127 255 +61 127 127 127 255 +74 127 127 127 255 +29 127 127 127 255 +3 127 127 127 255 +7 127 127 127 255 +36 127 127 127 255 +68 127 127 127 255 diff --git a/source/dcdt/se/linux/makefile b/source/dcdt/se/linux/makefile new file mode 100644 index 0000000000..d73be5d53b --- /dev/null +++ b/source/dcdt/se/linux/makefile @@ -0,0 +1,24 @@ + +export ROOT = ../.. +export INCLUDEDIR = -I $(ROOT)/ -I $(ROOT)/../sr/ -I $(ROOT)/../fltk/fltk-1.1.5rc34 +export LIBDIR = -L $(ROOT)/lib -L $(ROOT)/../sr/lib -L $(ROOT)/../fltk/fltk-1.1.5rc3/lib +export LIBS = -lse -lsrfl -lsrgl -lsr -lfltk -lfltk_gl -lGL + +DIRS = se semesh setut + +export CC = g++ +export CFLAGS = $(INCLUDEDIR) +export LFLAGS = $(LIBDIR) $(LIBS) + +all: + @for dir in $(DIRS); do\ + mkdir $$dir; cd $$dir; $(MAKE) -f ../makefile.$$dir; cd ..; \ + done + +clean: + -$(RM) core *.o *~ ../lib/* ../bin/* + @for dir in $(DIRS); do \ + rm -r $$dir; \ + done + + diff --git a/source/dcdt/se/linux/makefile.se b/source/dcdt/se/linux/makefile.se new file mode 100644 index 0000000000..592d71d0e6 --- /dev/null +++ b/source/dcdt/se/linux/makefile.se @@ -0,0 +1,14 @@ + +SRCDIR = $(ROOT)/src/se/ +LIB = $(ROOT)/lib/libse.a + +CPPFILES := $(shell echo $(SRCDIR)*.cpp) +OBJFILES = $(CPPFILES:.cpp=.o) +OBJECTS = $(notdir $(OBJFILES)) + +$(LIB): $(OBJECTS) + ar -r $(LIB) $(OBJECTS) + +%.o: $(SRCDIR)%.cpp + $(CC) -c $(CFLAGS) $< -o $@ + diff --git a/source/dcdt/se/linux/makefile.semesh b/source/dcdt/se/linux/makefile.semesh new file mode 100644 index 0000000000..517e856b56 --- /dev/null +++ b/source/dcdt/se/linux/makefile.semesh @@ -0,0 +1,14 @@ + +SRCDIR = $(ROOT)/src/semesh/ +BIN = $(ROOT)/bin/semesh + +CPPFILES := $(shell echo $(SRCDIR)*.cpp) +OBJFILES = $(CPPFILES:.cpp=.o) +OBJECTS = $(notdir $(OBJFILES)) + +$(BIN): $(OBJECTS) + $(CC) $(OBJECTS) $(LFLAGS) -o $(BIN) + +%.o: $(SRCDIR)%.cpp + $(CC) -c $(CFLAGS) $< -o $@ + diff --git a/source/dcdt/se/linux/makefile.setut b/source/dcdt/se/linux/makefile.setut new file mode 100644 index 0000000000..f12aafed1a --- /dev/null +++ b/source/dcdt/se/linux/makefile.setut @@ -0,0 +1,14 @@ + +SRCDIR = $(ROOT)/src/setut/ +BIN = $(ROOT)/bin/setut + +CPPFILES := $(shell echo $(SRCDIR)*.cpp) +OBJFILES = $(CPPFILES:.cpp=.o) +OBJECTS = $(notdir $(OBJFILES)) + +$(BIN): $(OBJECTS) + $(CC) $(OBJECTS) $(LFLAGS) -o $(BIN) + +%.o: $(SRCDIR)%.cpp + $(CC) -c $(CFLAGS) $< -o $@ + diff --git a/source/dcdt/se/readme.txt b/source/dcdt/se/readme.txt new file mode 100644 index 0000000000..634ed9036b --- /dev/null +++ b/source/dcdt/se/readme.txt @@ -0,0 +1,70 @@ + +=============================================================== + SR - Simulation and Representation Toolkit + (also known as the small scene graph toolkit) + + Marcelo Kallmann 1995-2004 + + See file sr.h for compilation options and + important notes about programming conventions +=============================================================== + +==== LIBRARIES ==== + + - sr: stand alone SR library with all main classes + - srgl: OpenGL rendering of the SR scene nodes (www.opengl.org) + - srfl: FLTK-OpenGL viewers and other UI tools (www.fltk.org) + +==== EXECUTABLES ==== + + - srmodel: demonstration and processing of SrModel + - srtest: several small programs to test/demonstrate + the toolkit. All tests are compiled in the + same executable, but only one test is called + from srtest_main.cpp. + - srxapp: Example application to start a project containing + a FLTK user interface and the 3D viewer SrViewer, + which uses FLTK and OpenGL. + +==== VISUALC ==== + + - Configurations Debug and Release are the usual ones, and + configuration Compact is meant to build executables + without any dependencies on DLLs. + - Generated libs are: sr, srfl, srgl (release) + src, srflc, srglc (compact), and + srd, srfld, srgld (debug) + - Folder visualc6 contains projects for Visual C++ 6 + - Folder visualc7 contains projects for Visual C++ .NET + - FLTK include and libs are set to: ..\..\fltk + +==== LINUX ==== + + - the linux folder contains makefiles for compilation using + gnu g++ and gmake tools. + - Some makefiles will look for FLTK as specified in the + main makefile linux/makefile + - gmake must be called from inside the linux/ directory + +==== TESTED PLATFORMS ==== + + - Windows 98 with Visual C++ 6.0 + - Windows XP with Visual C++ .NET + - Linux with gmake and g++ + +==== KNOWN BUGS ==== + + - check if SrSnEditor::~SrSnEditor() bug is really fixed + + - OpenGL: Transparency is currently disabled in SrViewer, + because it had side-effects with "solid" polygons. + + - SrViewer: Need to review/fix camera control and view_all() + +==== WISH LIST ==== + + - Compare performance of using double or floats in sr_bv_math.h + - Finish texture support in SrModel + - SrPolygon: make a better grow() method + - Make a good trackball manipulator scene node + diff --git a/source/dcdt/se/se.cpp b/source/dcdt/se/se.cpp new file mode 100644 index 0000000000..12e445d58d --- /dev/null +++ b/source/dcdt/se/se.cpp @@ -0,0 +1,31 @@ +#include "precompiled.h" + +# include "se.h" + +//================================= SeElement ==================================== + +SeElement::SeElement () + { + _index=0; + _symedge=0; + _next=_prior=this; + } + +SeElement* SeElement::_remove () + { + _next->_prior = _prior; + _prior->_next = _next; + return this; + } + +// same insert_prior() implementation as in SrListNode +SeElement* SeElement::_insert ( SeElement* n ) + { + n->_prior = _prior; + _prior->_next = n; + n->_next = this; + _prior = n; + return n; + } + +//=== End of File =================================================================== diff --git a/source/dcdt/se/se.h b/source/dcdt/se/se.h new file mode 100644 index 0000000000..f348fdb6cc --- /dev/null +++ b/source/dcdt/se/se.h @@ -0,0 +1,199 @@ + +# ifndef SE_H +# define SE_H + +/** \file se.h + * Sym Edge definitions + \code + ****************************************************************************************** + * + * SymEdge Mesh Data Structure + * Marcelo Kallmann 1996 - 2004 + * + * SymEdge joins Guibas & Stolfi structure ideas with Mäntylä Euler operators. + * Several triangulation algorithms were implemented in the se toolkit + * + * References: + * 1. M. Kallmann, H. Bieri, and D. Thalmann, "Fully Dynamic Constrained Delaunay + * Triangulations", In Geometric Modelling for Scientific Visualization, + * G. Brunnett, B. Hamann, H. Mueller, L. Linsen (Eds.), ISBN 3-540-40116-4, + * Springer-Verlag, Heidelberg, Germany, pp. 241-257, 2003. + * 2. M. Kallmann, and D. Thalmann, "Star-Vertices: A Compact Representation + * for Planar Meshes with Adjacency Information", Journal of Graphics Tools, + * 2001. (In the comparisons table, the symedge was used and called as the + * "simplified quad-edge structure" ) + * 3. M. Mäntylä, "An Introduction to Solid Modeling", Computer Science Press, + * Maryland, 1988. + * 4. L. Guibas and J. Stolfi, "Primitives for the Manipulation of General + * Subdivisions and the Computation of Voronoi Diagrams", ACM Transaction + * on Graphics, 4, 75-123, 1985. + * + * The structure is based on circular lists of SeElement for vertex/edge/face nodes, + * plus the connectivity information stored with SeBase class. + * + * Implementation History: + * + * 10/2004 - Becomes again SE lib, but with dependendy on the SR lib. + * + * 10/2003 - Integration to the SR library, which is the newer verion of FG. + * + * 08/2002 - SeGeo becomes abstract, and SeGeoPoint, SeGeoFloat, SeGeoPointFloat appear. + * - SeTriangulator now covers both constrained and conforming triangulations + * - SrClassManagerBase signature is no longer used + * + * 06/2001 - The library becomes again a standalone library. + * + * 09/2000 - Changed to always keep v/e/f information lists + * - New names starting with Se + * + * 05/2000 - Finally operator mg was done to convert triangle soups with genus>0 + * + * 10/1999 - Adaptation to the FG library, using FgArray, FgListNode, FgTree, I/O, etc. + * + * 06/1998 - Abandoned the idea of using a base algorithm class MeshAlg + * + * 07/1997 - Automatic calculation of MaxindexValue + * - Import() and related virtual callbacks created + * - Triangulation now is done with virtual callbacks + * + * 07/1996 - First implementation + * + ****************************************************************************************** +\endcode */ + +//================================ Types and Constants =============================== + +class SeMeshBase; +class SeBase; +class SeElement; + +typedef unsigned int semeshindex; //!< internally used to mark elements (must be unsigned) +typedef SeElement SeVertex; //!< a vertex is an element +typedef SeElement SeEdge; //!< an edge is an element +typedef SeElement SeFace; //!< a face is an element + +//=================================== SeElement ======================================= + +/*! SeElement contains the required information to be attached + to all vertices, edges and faces: + 1. a reference to one (any) adjacent SeBase + 2. pointers maintaining a circular list of all elements of + the same type (vertices, edges or faces) + 3. an index that can be used by SeMeshBase to mark elements + To attach user-related information to an element: + 1. SeElement must be derived and all user data is declared + inside the derived class. + 2. A corresponding SrClassManager must be derived and + the required virtual methods must be re-written in order to + manage the derived SeElement class. The compare method + is not used. See also sr_class_manager.h. */ +class SeElement + { protected : + SeElement (); + public : + SeBase* se () const { return _symedge; } + SeElement* nxt() const { return _next; } + SeElement* pri() const { return _prior; } + private : + friend class SeMeshBase; + friend class SrClassManagerBase; + SeElement* _next; + SeElement* _prior; + SeBase* _symedge; + semeshindex _index; + SeElement* _remove (); + SeElement* _insert ( SeElement* n ); + }; + +/*! The following define can be called in a user derived class of SeElement + to easily redefine public SeElement methods with correct type casts. + E stands for the element type, and S for the sym edge type. */ +# define SE_ELEMENT_CASTED_METHODS(E,S) \ + S* se() const { return (S*)SeElement::se(); } \ + E* nxt() const { return (E*)SeElement::nxt(); } \ + E* pri() const { return (E*)SeElement::pri(); } + +/*! The following define can be used to fully declare a default user derived + class of SeElement, which contains no user data but correctly redefines + public SeElement methods with type casts. */ +# define SE_DEFINE_DEFAULT_ELEMENT(E,S) \ + class E : public SeElement \ + { public : \ + SE_ELEMENT_CASTED_METHODS(E,S); \ + E () {} \ + E ( const E& e ) {} \ + friend SrOutput& operator<< ( SrOutput& o, const E& e ) { return o; } \ + friend SrInput& operator>> ( SrInput& i, E& e ) { return i; } \ + friend int sr_compare ( const E* e1, const E* e2 ) { return 0; } \ + }; + +//================================== SeBase ======================================== + +/*! Used to describe all adjacency relations of the mesh topology. The mesh itself is + composed of a net of SeBase elements linked together reflecting the vertex and + face loops. SymEdge is a short name for symetrical edge, as each SeBase has a + symetrical one incident to the same edge on the opposite face, and is given by + sym(). SeBase has local traverse operators permitting to change to any adjacent + symedge so to access any information stored on a vertex, edge or face. Symedges + are also used as parameters to the topological operators of SeMeshBase, which allow + modifying the mesh. */ +class SeBase + { public : + /*! Returns the next symedge adjacent to the same face. */ + SeBase* nxt() const { return _next; } + + /*! Returns the prior symedge adjacent to the same face. */ + SeBase* pri() const { return _rotate->_next->_rotate; } + + /*! Returns the next symedge adjacent to the same vertex. */ + SeBase* rot() const { return _rotate; } + + /*! Returns the prior symedge adjacent to the same vertex. */ + SeBase* ret() const { return _next->_rotate->_next; } + + /*! Returns the symmetrical symedge, sharing the same edge. */ + SeBase* sym() const { return _next->_rotate; } + + /*! Returns the element attached to the incident vertex. */ + SeVertex* vtx() const { return _vertex; } + + /*! Returns the element attached to the incident edge. */ + SeEdge* edg() const { return _edge; } + + /*! Returns the element attached to the incident face. */ + SeFace* fac() const { return _face; } + + private : + friend class SeMeshBase; + friend class SeElement; + SeBase* _next; + SeBase* _rotate; + SeVertex* _vertex; + SeEdge* _edge; + SeFace* _face; + }; + +/*! This is the template version of the SeBase class, that redefines the methods + of SeBase including all needed type casts to the user defined classes. + All methods are implemented inline just calling the corresponding method of + the base class but correctly applying type casts to convert default types + to user types. + Important Note: no user data can be stored in sym edges. This template class + is only used as a technique to correctly perform type casts. */ +template +class Se : public SeBase + { public : + Se* nxt() const { return (Se*)SeBase::nxt(); } + Se* pri() const { return (Se*)SeBase::pri(); } + Se* rot() const { return (Se*)SeBase::rot(); } + Se* ret() const { return (Se*)SeBase::ret(); } + Se* sym() const { return (Se*)SeBase::sym(); } + V* vtx() const { return (V*)SeBase::vtx(); } + E* edg() const { return (E*)SeBase::edg(); } + F* fac() const { return (F*)SeBase::fac(); } + }; + +//============================ End of File =============================== + +# endif // SE_H + diff --git a/source/dcdt/se/se_dcdt.cpp b/source/dcdt/se/se_dcdt.cpp new file mode 100644 index 0000000000..2ab3d02bc1 --- /dev/null +++ b/source/dcdt/se/se_dcdt.cpp @@ -0,0 +1,709 @@ +#include "precompiled.h" + +# include +# include +# include +# include "sr_box.h" +# include "sr_geo2.h" +# include "se_dcdt.h" + +//# define SR_USE_TRACE1 // insert polygon +//# define SR_USE_TRACE2 // init +//# define SR_USE_TRACE3 // insert polygon +//# define SR_USE_TRACE4 // remove polygon +//# define SR_USE_TRACE5 // funnel +//# define SR_USE_TRACE6 // translate +//# define SR_USE_TRACE7 // ray +//# define SR_USE_TRACE8 // tri manager +//# define SR_USE_TRACE9 // extract contours +# include "sr_trace.h" + +//DJD: declaring static member variables { + +template +SrArray *> SeLinkFace::processing; +template +int SeLinkFace::current = 0; + +//declaring static member variables } + +//=============================== SeDcdt ================================== + +SeDcdt::SeDcdt () + { + _mesh = new SeDcdtMesh; + + _triangulator = new SeTriangulator + ( SeTriangulator::ModeConstrained, + _mesh, + new SeDcdtTriManager, + 0.000001 /*epsilon*/ ); + + _first_symedge = 0; + _cur_search_face = 0; + _xmin = _xmax = _ymin = _ymax = 0; + _radius = -1; + } + +SeDcdt::~SeDcdt () + { + delete _triangulator; + delete _mesh; + } + +void SeDcdt::get_mesh_edges ( SrArray* constr, SrArray* unconstr ) + { + SeDcdtEdge *e, *ei; + SeDcdtSymEdge *s; + SrArray* pa; + + if ( constr ) constr->size(0); + if ( unconstr ) unconstr->size(0); + + e = ei = _mesh->first()->edg(); + + do { if ( e->is_constrained() ) pa=constr; else pa=unconstr; + if ( pa ) + { s = e->se(); pa->push() = s->vtx()->p; + s = s->nxt(); pa->push() = s->vtx()->p; + } + e = e->nxt(); + } while ( e!=ei ); + } + +//================================================================================ +//================================ save ========================================== +//================================================================================ + +bool SeDcdt::save ( SrOutput& out ) + { + int elems = _polygons.elements(); + + out << "SeDcdt\n\n"; + out << "# domain:" << (elems>0?1:0) << " polygons:" << (elems-1) << srnl << srnl; + out << "epsilon " << _triangulator->epsilon() << srnl << srnl; + + if ( _radius!=-1 ) + out << "init_radius " << _radius << srnl << srnl; + + if ( elems==0 ) return true; + + int i, id, psize; + int maxid = _polygons.maxid(); + for ( id=0; id<=maxid; id++ ) + { if ( !_polygons[id] ) continue; + + SeDcdtInsPol& p = *_polygons[id]; + + if ( id==0 ) + { out << "domain\n"; } + else + { out << "polygon " << id; + if ( p.open ) out << " open"; + out << srnl; + } + + psize = p.size(); + for ( i=0; ip; + if ( i==psize-1 ) out<<';'; else out<> pol.push(); + } + } + +bool SeDcdt::load ( SrInput& inp ) + { + SrPolygon pol; + pol.capacity ( 64 ); + + SrArray invids; + + float epsilon = 0.00001f; + float radius = -1.0f; + int id, nextid=1; + + // signature: + inp.comment_style ( '#' ); + inp.get_token (); + if ( inp.last_token()!="SeDcdt") return false; + + while ( 1 ) + { inp.get_token (); + if ( inp.finished() ) break; + SrString& s = inp.last_token(); + + if ( s=="epsilon" ) + { inp >> epsilon; + } + if ( s=="init_radius" ) + { inp >> radius; + } + else if ( s=="domain" ) + { _read_pol ( inp, pol ); + pol.open ( false ); + init ( pol, epsilon, radius ); + } + else if ( s=="polygon" ) + { if ( num_polygons()==0 ) return false; + inp >> id; + while ( id>nextid ) { invids.push()=_polygons.insert(); nextid++; } + inp.get_token(); + if ( inp.last_token()=="open" ) + { pol.open(true); } + else + { inp.unget_token(); + pol.open(false); + } + _read_pol ( inp, pol ); + insert_polygon ( pol ); + nextid++; + } + } + + while ( invids.size() ) _polygons.remove(invids.pop()); + + return true; + } + +//================================================================================ +//================================ init ========================================== +//================================================================================ + +void SeDcdt::init ( const SrPolygon& domain , float epsilon, float radius ) + { + SR_TRACE2 ( "Starting init()..." ); + + // Reset cur searched face: + _cur_search_face = 0; + + // Clear structures if needed: + if ( _first_symedge ) + { _mesh->destroy(); _first_symedge=0; _polygons.init(); } + + // Checks parameters: + if ( domain.size()<3 || domain.open() ) + sr_out.fatal_error ( "SeDcdt::init(): domain polygon must be simple and closed!"); + + // Calculates an external polygon (the border) : + SR_TRACE2 ( "Calculating External Polygon for domain with "<0 ) + { if ( dxepsilon ( epsilon ); + _first_symedge = (SeDcdtSymEdge*)_triangulator->init_as_triangulated_square + ( _xmin, _ymin, _xmax, _ymin, _xmax, _ymax, _xmin, _ymax ); + _radius = radius; + + // Inserts the vertices of the domain according to radius: + if ( radius==0 ) + { _using_domain = false; + } + else + { _using_domain = true; + insert_polygon ( domain ); + } + } + +void SeDcdt::get_bounds ( float& xmin, float& xmax, float& ymin, float& ymax ) const + { + xmin=_xmin; xmax=_xmax; ymin=_ymin; ymax=_ymax; + } + +//================================================================================ +//=========================== insert polygon ==================================== +//================================================================================ + +int SeDcdt::insert_polygon ( const SrPolygon& pol ) + { + int i, i1, id; + SeVertex* v; + SeFace* sface; + + SR_TRACE1 ( "Inserting entry in the polygon set..." ); // put in _polygons + id = _polygons.insert(); + SeDcdtInsPol& ip = *_polygons[id]; + ip.open = pol.open(); + + SR_TRACE1 ( "Inserting polygon points..." ); // insert vertices + sface = _search_face(); + for ( i=0; iinsert_point ( pol[i].x, pol[i].y, sface ); + if ( !v ) sr_out.fatal_error ( "Search failure in _insert_polygon()." ); + ip.push() = (SeDcdtVertex*)v; + sface = v->se()->fac(); + } + + // should we keep here collinear vertices (which are not corners)? + // they can be removed as Steiner vertices when removing another intersecting polygon + + _cur_search_face=0; // Needed because edge constraint may call kef + + SR_TRACE1 ( "Inserting polygon edges constraints..." ); // insert edges + for ( i=0; iinsert_line_constraint ( ip[i], ip[i1], id ) ) + sr_out.fatal_error ( "Unable to insert constraint in _insert_polygon()." ); + } + + return id; + } + +int SeDcdt::polygon_max_id () const + { + return _polygons.maxid(); + } + +int SeDcdt::num_polygons () const + { + return _polygons.elements(); + } + +//================================================================================ +//=========================== remove polygon ==================================== +//================================================================================ + +void SeDcdt::_find_intermediate_vertices_and_edges ( SeDcdtVertex* vini, int oid ) + { + SeDcdtEdge *e; + SeDcdtVertex *v; + SeDcdtSymEdge *s, *si; + + _mesh->begin_marking (); + _varray.size(0); _earray.size(0); _stack.size(0); + + // initialize stack with all constrained edges from v: + _varray.push() = vini; + _mesh->mark ( vini ); + s = si = vini->se(); + do { e = s->edg(); + if ( e->has_id(oid) ) + { _stack.push() = s; + _earray.push() = s; + _mesh->mark ( e ); + } + s = s->rot(); + } while ( s!=si ); + + // advances untill all edges are reached: + while ( _stack.size() ) + { s = si = _stack.pop()->nxt(); + v = s->vtx(); + if ( !_mesh->marked(v) ) + { _varray.push() = v; + _mesh->mark ( v ); + } + do { e = s->edg(); + if ( !_mesh->marked(e) && e->has_id(oid) ) + { _stack.push() = s; + _earray.push() = s; + _mesh->mark ( e ); + } + s = s->rot(); + } while ( s!=si ); + } + + _mesh->end_marking (); + } + +bool SeDcdt::_is_intersection_vertex + ( SeDcdtVertex* v, int id, SeDcdtVertex*& v1, SeDcdtVertex*& v2 ) + { + SeDcdtSymEdge *si, *s; + v1 = v2 = 0; + + // at this point, it is guaranteed that only two constraints with index id + // are adjacent to v, because there is a test that ensures that only two + // constrained edges are incident to v in _remove_vertex_if_possible(). + // (however these two constrained edges may have more than one id) + + si = s = v->se(); + do { if ( s->edg()->has_id(id) ) + { if ( v1==0 ) v1=s->nxt()->vtx(); + else if ( v2==0 ) { v2=s->nxt()->vtx(); break; } + } + s = s->rot(); + } while ( s!=si ); + + if ( v1==0 || v2==0 ) return false; // v is an extremity of an open constraint + + double d = sr_point_segment_dist ( v->p.x, v->p.y, v1->p.x, v1->p.y, v2->p.x, v2->p.y ); + return d<=_triangulator->epsilon()? true:false; + } + +void SeDcdt::_remove_vertex_if_possible ( SeDcdtVertex* v, const SrArray& vids ) + { + SR_TRACE4 ( "Try to remove vertex with ref="<remove_vertex(v); + return; + } + + int i; + int num_of_incident_constrained_edges=0; + SeDcdtSymEdge *s; + s = v->se(); + do { if ( s->edg()->is_constrained() ) num_of_incident_constrained_edges++; + s = s->rot(); + } while ( s!=v->se() ); + + if ( num_of_incident_constrained_edges!=2 ) return; + + // test if all polygons using the vertex no more need it: + SeDcdtVertex *v1, *v2; + SrArray& va = _varray2; // internal buffer + va.size ( 0 ); + + // (note that we can have vids.size()>1 at this point) + for ( i=0; iremove_vertex(v); + + // and recover all deleted constraints: + for ( i=0; iinsert_line_constraint ( va[i*2], va[i*2+1], vids[i] ); + } + } + +void SeDcdt::remove_polygon ( int polygonid ) + { + int i; + SR_TRACE4 ( "Entering remove_polygon..." ); + + if ( polygonid==0 ) + sr_out.fatal_error("Domain cannot be removed by SeDcdt::remove_polygon()"); + + if ( polygonid<0 || polygonid>_polygons.maxid() ) + sr_out.fatal_error("Invalid id sent to SeDcdt::remove_polygon()"); + + if ( !_polygons[polygonid] ) + sr_out.fatal_error("SeDcdt::remove_polygon(): polygon already removed"); + + // _search_face can be invalidated so make it unavailable: + _cur_search_face = 0; + + // Recuperate intermediate vertices inserted as Steiner points: + SR_TRACE4 ( "Recuperating full polygon..." ); + _find_intermediate_vertices_and_edges ( _polygons[polygonid]->get(0), polygonid ); + SR_TRACE4 ( "polygon has "<<_varray.size()<<" vertices " << "and "<<_earray.size()<<" edges." ); + + // Remove all ids which are equal to polygonid in the edge constraints + SR_TRACE4 ( "Updating edge constraints..." ); + for ( i=0; i<_earray.size(); i++ ) + { _earray[i]->edg()->remove_id ( polygonid ); // remove all occurences + } + + SR_TRACE4 ( "Removing free vertices..." ); + for ( i=0; i<_varray.size(); i++ ) + { _varray[i]->get_references ( _ibuffer ); //vobs + SR_TRACE4 ( "Vertex "<p << ")" ); + _remove_vertex_if_possible ( _varray[i], _ibuffer ); + } + + _polygons.remove ( polygonid ); + SR_TRACE4 ( "Finished." ); + } + +//================================================================================ +//============================= get polygon ====================================== +//================================================================================ + +void SeDcdt::get_polygon ( int polygonid, SrPolygon& polygon ) + { + polygon.size(0); + + if ( polygonid<0 || polygonid>_polygons.maxid() ) return; + if ( !_polygons[polygonid] ) return; + + SeDcdtInsPol& p = *_polygons[polygonid]; + + int i; + polygon.size ( p.size() ); + polygon.open ( p.open? true:false ); + for ( i=0; ip; + } + +void SeDcdt::get_triangulated_polygon ( int polygonid, SrArray* vtxs, SrArray* edgs ) + { + if ( polygonid<0 || polygonid>_polygons.maxid() ) return; + if ( !_polygons[polygonid] ) return; + + _find_intermediate_vertices_and_edges ( _polygons[polygonid]->get(0), polygonid ); + + if ( vtxs ) *vtxs = _varray; + + if ( edgs ) // need to convert symedges to edges + { int i; + edgs->size ( _earray.size() ); + for ( i=0; isize(); i++ ) (*edgs)[i] = _earray[i]->edg(); + } + } + +//================================================================================ +//=========================== ray intersection =================================== +//================================================================================ + +static bool _intersects ( double p1x, double p1y, double p2x, double p2y, SeDcdtSymEdge* s ) + { + SeDcdtVertex* v1 = s->vtx(); + SeDcdtVertex* v2 = s->nxt()->vtx(); + return sr_segments_intersect ( p1x, p1y, p2x, p2y, + v1->p.x, v1->p.y, v2->p.x, v2->p.y ); + } + +void SeDcdt::ray_intersection ( float x1, float y1, float x2, float y2, + SrArray& polygons, int depth ) + { + SeTriangulator::LocateResult res; + SeBase *ses; + SeDcdtSymEdge *s; + + polygons.size(0); + if ( depth==0 ) return; + + double p1x=x1, p1y=y1, p2x=x2, p2y=y2; + + res = _triangulator->locate_point ( _search_face(), p1x, p1y, ses ); + if ( res==SeTriangulator::NotFound ) + { SR_TRACE7 ( "First point not found!" ); + return; + } + + s = (SeDcdtSymEdge*)ses; + _cur_search_face = s->fac(); + + // Find first intersection with the first triangle: + int i=0; + while ( !_intersects(p1x,p1y,p2x,p2y,s) ) + { s=s->nxt(); + if ( i++==3 ) + { SR_TRACE7 ( "None intersections, ray in triangle!" ); + return; + } + } + + // Continues until end: + while ( true ) + { if ( s->edg()->is_constrained() ) + { //int r = rand() % e->ids_size; + SR_TRACE7 ( "found one!" ); + SrArray& a = s->edg()->ids; + for ( i=0; i0 && polygons.size()>=depth ) + { SR_TRACE7 ( "depth reached!" ); return; } + } + s = s->sym(); + if ( _intersects(p1x,p1y,p2x,p2y,s->nxt()) ) s=s->nxt(); + else + if ( _intersects(p1x,p1y,p2x,p2y,s->pri()) ) s=s->pri(); + else + { SR_TRACE7 ( "Finished Ray!" ); return; } + } + } + +void SeDcdt::_add_contour ( SeDcdtSymEdge* s, SrPolygon& vertices, SrArray& pindices ) + { + SeDcdtSymEdge* si=s; + pindices.push() = vertices.size(); + + SR_TRACE9 ( "Begin contour: "<vtx()->p; + _mesh->mark ( s->edg() ); + do { s=s->rot(); } while ( !s->edg()->is_constrained() ); + s = s->sym(); + } while ( s!=si ); + + pindices.push() = vertices.size()-1; + SR_TRACE9 ( "End contour: "<marked(s->edg()) ) continue; + if ( s->edg()->is_constrained() ) return s; + _mesh->mark(s->edg()); + + s = s->sym()->nxt(); + if ( !_mesh->marked(s->edg()) ) { _earray.push()=s; } + s = s->nxt(); + if ( !_mesh->marked(s->edg()) ) { _earray.push()=s; } + } + + return 0; + } + +void SeDcdt::extract_contours ( SrPolygon& vertices, SrArray& pindices, float x, float y ) + { + SeTriangulator::LocateResult res; + SeBase *ses; + + vertices.size(0); + pindices.size(0); + + SR_TRACE9 ( "Begin Extract" ); + + res = _triangulator->locate_point ( _search_face(), x, y, ses ); + if ( res==SeTriangulator::NotFound ) return; + + _mesh->begin_marking(); + _earray.size(0); + SeDcdtSymEdge *s, *si; + s = si = (SeDcdtSymEdge*)ses; + do { _earray.push() = s; + s = s->nxt(); + } while ( s!=si ); + + while (1) + { s = _find_one_obstacle (); + if ( !s ) break; + _add_contour ( s, vertices, pindices ); + } + + _mesh->end_marking(); + SR_TRACE9 ( "End Extract" ); + } + +//================================================================================ +//=========================== inside polygon ==================================== +//================================================================================ + +static int interhoriz ( float px, float py, float p1x, float p1y, float p2x, float p2y ) + { + if ( p1y>p2y ) { float tmp; SR_SWAP(p1x,p2x); SR_SWAP(p1y,p2y); } // swap + if ( p1y>=py ) return false; // not intercepting + if ( p2y* allpolys ) + { + if ( _polygons.elements()<=1 ) return -1; // if there is only the domain, return. + + int cont=0, i, size; + SeDcdtInsPol* pol; + SeDcdtVertex *v1, *v2; + + if ( allpolys ) allpolys->size(0); + + int polid=0; + if ( _using_domain ) polid++; + for ( ; polid<=_polygons.maxid(); polid++ ) + { pol = _polygons[polid]; + if ( !pol ) continue; + + size = pol->size(); + for ( i=0; iget(i); + v2 = pol->get( (i+1)%size ); + cont ^= interhoriz ( x, y, v1->p.x, v1->p.y, v2->p.x, v2->p.y ); + } + if (cont) + { if ( allpolys ) allpolys->push()=polid; + else return polid; + } + } + + if ( allpolys ) if ( allpolys->size()>0 ) return allpolys->get(0); + return -1; + } + +int SeDcdt::pick_polygon ( float x, float y ) + { + if ( _polygons.elements()==0 ) return -1; + if ( _using_domain && _polygons.elements()==1 ) return -1; // if there is only the domain, return. + + SeTriangulator::LocateResult res; + SeBase *sse; + + res = _triangulator->locate_point ( _search_face(), x, y, sse ); + if ( res==SeTriangulator::NotFound ) return -1; + + SeDcdtSymEdge *s = (SeDcdtSymEdge*)sse; + SrPnt2 pt(x,y); + float d1 = dist2 ( pt, s->vtx()->p ); + float d2 = dist2 ( pt, s->nxt()->vtx()->p ); + float d3 = dist2 ( pt, s->nxt()->nxt()->vtx()->p ); + if ( d2<=d1 && d2<=d3 ) s=s->nxt(); + if ( d3<=d1 && d3<=d2 ) s=s->nxt()->nxt(); + + SeDcdtSymEdge* k=s; + do { SrArray& ids = k->edg()->ids; + if ( ids.size()>0 ) { if ( !_using_domain || ids[0]>0 ) return ids[0]; } // 0 is the domain + k=k->rot(); + } while ( k!=s ); + + return -1; + } + +//================================================================================ +//=============================== search path ====================================== +//================================================================================ + +bool SeDcdt::search_path ( float x1, float y1, float x2, float y2, + const SeDcdtFace* iniface, bool vistest ) + { + // fast security test to ensure at least that points are not outside the border limits: + if ( x1<_xmin || x1>_xmax || x2<_xmin || x2>_xmax ) return false; + + if ( !iniface ) iniface = _search_face(); + + bool found = _triangulator->search_path ( x1, y1, x2, y2, iniface, vistest ); + + // to optimize searching for next queries around the same point, + // set the next starting search face to the first channel face: + if ( _triangulator->get_channel_interior_edges().size()>0 ) + _cur_search_face = (SeDcdtFace*)_triangulator->get_channel_interior_edges()[0]->fac(); + + return found; + } + +//============================ End of File =============================== + diff --git a/source/dcdt/se/se_dcdt.h b/source/dcdt/se/se_dcdt.h new file mode 100644 index 0000000000..fda371b677 --- /dev/null +++ b/source/dcdt/se/se_dcdt.h @@ -0,0 +1,493 @@ + +# ifndef SE_DCDT_H +# define SE_DCDT_H + +/** \file se_dcdt.h + * Dynamic Constrained Delaunay Triangulaiton + */ + +# include "sr_set.h" +# include "sr_vec2.h" +# include "sr_polygon.h" + +# include "se_triangulator.h" + +//DJD: Including definition of abstraction and search node classes { + +//#define EXPERIMENT +#define DEBUG +#define FIRST_PATH_FAST + +#include "Abstraction.h" +#include "SearchNode.h" +#if defined EXPERIMENT +#include "Experiments.h" +#endif + +//Including definition of abstraction and search node classes } + +//============================ DCDT Mesh definitions ================================== + +class SeDcdtVertex; // contains 2d coordinates +class SeDcdtEdge; // contains constraints ids + +//DJD: changing definition of SeDcdtFace { + +//class SeDcdtFace; // a default element + +//changing definition of SeDcdtFace } + +typedef Se < SeDcdtVertex, SeDcdtEdge, SeDcdtFace > SeDcdtSymEdge; +typedef SeMesh < SeDcdtVertex, SeDcdtEdge, SeDcdtFace > SeDcdtMesh; + +//DJD: defining SeLinkFace { + +//SE_DEFINE_DEFAULT_ELEMENT(SeDcdtFace,SeDcdtSymEdge); + +template +class SeLinkFace : public SeElement +{ +public: + //the number of new faces that haven't been dealt with + static int Faces() {return processing.size();} + //retrieve a particular unprocessed face + static SeLinkFace *Face(int n) {return ((n < 0) || (n >= processing.size())) ? NULL : processing[n];} + //clear unprocessed face list, increment the global update + static void Clear() {while (!processing.empty()) processing.pop(); current++;} + //link to external information + T *link; + SE_ELEMENT_CASTED_METHODS(SeLinkFace, SeDcdtSymEdge); + //default constructor - set the link to null and initialize the face + SeLinkFace() : link (NULL) {Initialize();} + //copy constructor - initialize the link to that of the other face, and initialize it + SeLinkFace(const SeLinkFace& f) : link (f.link) {Initialize();} + //constructor - initialize the link to the value passed and initialize the face + SeLinkFace(T *l) : link (l) {Initialize();} + //destructor - deletes a non-null link, and nulls its entry in the unprocessed list if it's in there + ~SeLinkFace() {if (link != NULL) delete link; if (update == current) processing[index] = NULL;} + friend SrOutput& operator<<(SrOutput& out, const SeLinkFace& f) {return out;} + friend SrInput& operator>>(SrInput& inp, SeLinkFace& f) {return inp;} + friend int sr_compare(const SeLinkFace* f1, const SeLinkFace* f2) {return 0;} +protected: + //the index of this face in the unprocessed list + int index; + //the update when this face was inserted in the unprocessed list + int update; + //initializes the face - puts it in the unprocessed list and sets its index and update + void Initialize() {index = processing.size(); processing.push() = this; update = current;} + //the unprocessed list + static SrArray *> processing; + //the global update + static int current; +}; + +//defining SeLinkFace } + +class SeDcdtVertex : public SeElement + { public : + SrPnt2 p; // 2d coordinates + public : + SE_ELEMENT_CASTED_METHODS(SeDcdtVertex,SeDcdtSymEdge); + SeDcdtVertex () {} + SeDcdtVertex ( const SeDcdtVertex& v ) : p(v.p) {} + SeDcdtVertex ( const SrPnt2& pnt ) : p(pnt) {} + void set ( float x, float y ) { p.x=x; p.y=y; } + void get_references ( SrArray& ids ); // get all constr edges referencing this vertex + friend SrOutput& operator<< ( SrOutput& out, const SeDcdtVertex& v ); + friend SrInput& operator>> ( SrInput& inp, SeDcdtVertex& v ); + friend int sr_compare ( const SeDcdtVertex* v1, const SeDcdtVertex* v2 ) { return 0; } // not used + }; + +class SeDcdtEdge : public SeElement + { public : + SrArray ids; // ids of all constraints sharing this edge + public : + SE_ELEMENT_CASTED_METHODS(SeDcdtEdge,SeDcdtSymEdge); + SeDcdtEdge () { } + SeDcdtEdge ( const SeDcdtEdge& e ) { ids=e.ids; } + bool is_constrained() const { return ids.size()>0? true:false; } + void set_unconstrained () { ids.size(0); } + bool has_id ( int id ) const; + bool has_other_id_than ( int id ) const; + bool remove_id ( int id ); + void add_constraints ( const SrArray& ids ); + friend SrOutput& operator<< ( SrOutput& out, const SeDcdtEdge& e ); + friend SrInput& operator>> ( SrInput& inp, SeDcdtEdge& e ); + friend int sr_compare ( const SeDcdtEdge* e1, const SeDcdtEdge* e2 ) { return 0; } // not used + }; + +//================================ DCDT internal classes ===================================== + +class SeDcdtTriManager: public SeTriangulatorManager + { public : + virtual void get_vertex_coordinates ( const SeVertex* v, double& x, double & y ); + virtual void set_vertex_coordinates ( SeVertex* v, double x, double y ); + virtual bool is_constrained ( SeEdge* e ); + virtual void set_unconstrained ( SeEdge* e ); + virtual void get_constraints ( SeEdge* e, SrArray& ids ); + virtual void add_constraints ( SeEdge* e, const SrArray& ids ); + }; + +class SeDcdtInsPol: public SrArray // keeps one "inserted polygon" + { public : + char open; + public : + SeDcdtInsPol () { open=0; } + SeDcdtInsPol ( const SeDcdtInsPol& ob ) : SrArray((const SrArray&)ob) { open=0; } + friend SrOutput& operator<< ( SrOutput& o, const SeDcdtInsPol& ob ); + friend SrInput& operator>> ( SrInput& in, SeDcdtInsPol& ob ); + friend int sr_compare ( const SeDcdtInsPol* c1, const SeDcdtInsPol* c2 ) { return 0; } // not used + }; + +//=================================== DCDT class ======================================== + +/*! Mantains a dynamic constrained Delaunay triangulation of given polygons + for the purpose of path planning and other queries. */ +class SeDcdt + { private : + SeDcdtMesh* _mesh; // the mesh + SeTriangulator* _triangulator; // the used triangulator + SeDcdtSymEdge* _first_symedge; // symedge at the border, but not at the back face + SeDcdtFace* _cur_search_face; // a face near the last change in the mesh, 0 if not valid + SrSet _polygons; // inserted polygons, polygon 0 is always the domain + SrArray _varray; // internal buffer + SrArray _earray; // internal buffer + SrArray _stack; // internal buffer + SrArray _varray2; // internal buffer + SrArray _ibuffer; + float _radius; + bool _using_domain; + float _xmin, _xmax, _ymin, _ymax; + + public : + /*! Default constructor */ + SeDcdt (); + + /*! Destructor */ + ~SeDcdt (); + + /*! Returns a pointer to the internal maintained mesh. The user can then + use low level methods of SeMesh for additional computation. However it + is the user responsability to not conflict with SeDcdt methods. */ + const SeDcdtMesh* mesh() const { return _mesh; } + + /*! Put in the given arrays the coordinates of the constrained and + unconstrained edges endpoints. Each two consecutive points in the + returned arrays give the first and end points of one dge. + Parameters can be null indicating that no data of that type is requested. + The two parameters are also allowed to point to the same array */ + void get_mesh_edges ( SrArray* constr, SrArray* unconstr ); + + /*! Save the current dcdt by saving the list of all inserted obstacles. + Note that the polygons ids are preserved. */ + bool save ( SrOutput& out ); + + /*! Destructs the current map and loads a new one */ + bool load ( SrInput& inp ); + + /*! Initializes the triangulation with a domain polygon. + The domain is considered to be the constraint polygon with id 0; and can + be retrieved again by calling get_polygon(0). + An external box is automatically computed as an expanded bounding box of + the domain, where the expansion vector length is 1/5 of the bounding box sides. + This external box defines the border of the triangulation and its coordinates + can be retrieved with get_bounds(). + Note that all polygons inserted afterwards must be inside the external box. + Parameter radius can be used, for instance, to have a security margin in order + to allow growing polygons without intersecting with the external box. + If parameter radius is >0, it is used as minimum length for the + expansion vector used to compute the external box. + Special Case: If radius is 0, the domain polygon is not inserted, and the + triangulation is initialized with border equal to the bounding box of domain. + Parameter epsilon is simply passed to the triangulator. */ + void init ( const SrPolygon& domain, float epsilon, float radius=-1 ); + + /*! Internally, the border is generated containing the domain polygon. + This method allows to retrieve the coordinates of the border rectangle. */ + void get_bounds ( float& xmin, float& xmax, float& ymin, float& ymax ) const; + + /*! Inserts a polygon as a constraint in the CDT, returning its id. + In case of error, -1 is returned. Polygons can be open. + The returned id can be used to remove the polygon later on. + All kinds of intersections and overllapings are handled. + Collinear vertices are inserted. If not desired, method + SrPolygon::remove_collinear_vertices() should be called prior insertion. */ + int insert_polygon ( const SrPolygon& polygon ); + + /*! Returns the max id currently being used. Note that a SrSet controls + the ids, so that the max id may not correspond to the number of + polygons inserted; ids values are not changed with polygon removal */ + int polygon_max_id () const; + + /*! Returns the number of inserted polygons, including the domain (if inserted). */ + int num_polygons () const; + + /*! Remove a previoulsy inserted polygon. false may be returned if the id is + not valid or if some internal error occurs. The domain cannot be removed with + this method. Steiner points may stay as part of other polygons if the + triangulation is in conforming mode. */ + void remove_polygon ( int polygonid ); + + /*! Retrieves the original vertices of the given polygon (without collinear vertices). + If the returned polygon is empty, it means that polygonid is invalid. */ + void get_polygon ( int polygonid, SrPolygon& polygon ); + + /*! Returns the vertices and edges used by the polygon in the triangulation. + Elements may be out of order, specially when they have intersections. + If an argument is a null pointer, nothing is done with it. */ + void get_triangulated_polygon ( int polygonid, SrArray* vtxs, SrArray* edgs ); + + /*! Returns a list with the ids of the polygons having some edge traversed by the segment + [(x1,y1),(x2,y2)]. The length of the returned array will not be more than depth, + corresponding to 'depth' edges being crossed. Note: the id of a polygon will + appear both when the ray enters the polygon and when it leaves the polygon. + If depth is <0, no depth control is used. + This routine can also be used to fastly determine if a point is inside a polygon + by looking if the number of intersections is odd or even. */ + void ray_intersection ( float x1, float y1, float x2, float y2, SrArray& polygons, int depth ); + + /*! Returns all polygons describing the contours of an "eating virus" starting at x,y. + Array pindices contains, for each contour, the starting and ending vertex index, + which are sequentially stored in array vertices. */ + void extract_contours ( SrPolygon& vertices, SrArray& pindices, float x, float y ); + + /*! Returns the id of the first found polygon containing the given point (x,y), + or -1 if no polygons are found. The domain polygon, if used in init(), will not + be considered. The optional parameter allpolys can be sent to return all polygons + containing the point, and only the first one. + Note: this method does a linear search over each polygon, alternativelly, the + ray_intersection() method might also be used to detect polygon containement. */ + int inside_polygon ( float x, float y, SrArray* allpolys=0 ); + + /*! Returns the id of one polygon close to the given point (x,y), or -1 otherwise. + This method locates the point (x,y) in the triangulation and then takes the + nearest polygon touching that triangle. + The domain polygon, if used in init(), will not be considered. */ + int pick_polygon ( float x, float y ); + + /*! Search for the channel connecting x1,y1 and x2,y2. + It simply calls SeTriangulator::search_path(), however here parameter iniface is optional */ + bool search_path ( float x1, float y1, float x2, float y2, + const SeDcdtFace* iniface=0, bool vistest=false ); + + /*! Returns a reference to the list with the interior edges of the last channel + determined by a sussesfull call to search_path */ + const SrArray& get_channel_interior_edges () const + { return _triangulator->get_channel_interior_edges(); } + + /*! Returns a polygon describing the current channel, and thus, method find_path must + be succesfully called before to determine the channel to consider. */ + void get_channel_boundary ( SrPolygon& channel ) { _triangulator->get_channel_boundary(channel); } + + /*! Returns the canonical path, which is the path passing through the midpoint of + the channel interior edges. Method search_path must be succesfully called before + in order to determine the channel. The path is returned as an open polygon. */ + void get_canonical_path ( SrPolygon& path ) { _triangulator->get_canonical_path(path); } + + /*! Returns the shortest path inside the current channel using the funnel algorithm. */ + void get_shortest_path ( SrPolygon& path ) { _triangulator->get_shortest_path(path); } + +//DJD: prototypes for functions used in abstraction { + +public: +#if defined DEBUG + //the triangle on which the mouse pointer currently resides + SeDcdtFace *cursor; +#endif + //adds abstraction information to the triangulation + void Abstract(); + //deletes all abstraction information from the triangulation + void DeleteAbstraction(); +private: + //the "outside" face of the triangulation + SeDcdtFace *outside; + //abstract an acyclic component of the triangulation graph + void TreeAbstract(SeDcdtFace *first, int component); + //collapse an acyclic portion into the degree-2 root given + void TreeCollapse(SeDcdtFace *root, SeDcdtFace *currentFace, int component); + +//prototypes for functions used in abstraction } + +//DJD: prototypes for functions used in width calculation { + +private: + //calculates all widths through a triangle + void CalculateWidths(SeDcdtFace *face); + //calculates a particular width through a triangle + float TriangleWidth(SeBase *s); + //search across an edge for the width through the triangle + float SearchWidth(float x, float y, SeBase *s, float CurrentWidth); + //checks if the position given in the face given is at least r from any obstacle + bool ValidPosition(float x, float y, SeDcdtFace *face, float r); + //determines if angles (x, y)-(x1, y1)-(x2, y2) and (x, y)-(x2, y2)-(x1, y1) are accute + bool Consider(float x, float y, float x1, float y1, float x2, float y2); + //determines the minimum distance between the point (x, y) and the line going through (x1, y1)-(x2, y2) + float PointLineDistance(float x, float y, float x1, float y1, float x2, float y2); + +//prototypes for functions used in width calculation } + +//DJD: protptypes for functions used in point location { + +public: + //perform initialization for sector-based point location on the unprocessed triangles + void InitializeSectors(); + //checks if the given point is inside the given triangle + bool InTriangle(SeDcdtFace *face, float x, float y); + //use sector-based point location to find the face in which the given point resides + SeTriangulator::LocateResult LocatePoint(float x, float y, SeBase* &result); + //use regular point location to perform the above + SeTriangulator::LocateResult LocatePointOld(float x, float y, SeBase* &result); + //generate a random valid x value for testing point location + float RandomX(); + //generate a random valid y value for testing point location + float RandomY(); +private: + //the number of sectors in the vertical dimension + const static int ySectors = 10; + //the number of sectors in the horizontal dimension + const static int xSectors = 10; + //the width of a single sector + float sectorWidth; + //the height of a single sector + float sectorHeight; + //the array of sectors + SeDcdtFace *sectors[ySectors][xSectors]; + //return the maximum of the 3 numbers passed + float Max(float a, float b, float c); + //return the maximum of the 3 numbers passed + float Min(float a, float b, float c); + //return a random value between those given + float RandomBetween(float min, float max); + +//protptypes for functions used in point location } + +//DJD: prototypes for functions used in abstracted space searching { + +public: +#if defined EXPERIMENT + //find given path with TRA* and return experimental data + bool SearchPathFast(float x1, float y1, float x2, float y2, float r, Data *data, int numData); + //find given path with TA* and return experimental data + bool SearchPathBaseFast(float x1, float y1, float x2, float y2, float r, Data *data, int numData); +#else + //find given path with TRA* + bool SearchPathFast(float x1, float y1, float x2, float y2, float r); + //find given path with TA* + bool SearchPathBaseFast(float x1, float y1, float x2, float y2, float r); +#endif + //get the shortest path between the given points within the given channel of triangles + float GetShortestPath(SrPolygon& path, SrArray Channel, float x1, float y1, float x2, float y2, float r); + //return the polygon around the current channel + SrPolygon GetChannelBoundary(); + //return the current path + SrPolygon &GetPath(); +private: +#if defined EXPERIMENT + //search for a path in an acyclic portion of the triangulation graph and count the triangles searched + bool Degree1Path(SrArray& path, float x1, float y1, SeDcdtFace *startFace, + float x2, float y2, SeDcdtFace *goalFace, float r, int &nodes); +#else + //search for a path in an acyclic portion of the triangulation graph + bool Degree1Path(SrArray& path, float x1, float y1, SeDcdtFace *startFace, + float x2, float y2, SeDcdtFace *goalFace, float r); +#endif + //search for a path along a degree-2 corridor + bool Degree2Path(SrArray& path, SeDcdtFace *startFace, SeDcdtFace *nextFace, + SeDcdtFace *goalFace, float r); + //search for a path along a degree-2 corridor, crossing a degree-3 node + bool FollowLoop(SrArray& path, SeDcdtFace *startFace, SeDcdtFace *nextFace, + SeDcdtFace *goalFace, SeDcdtFace *degree3, float r); + //walk from one face to a given adjacent face, in a direction specified + bool WalkBetween(SrArray& path, SeDcdtFace *sourceFace, SeDcdtFace *destinationFace, float r, int direction = INVALID); + //determine if an object of given radius can cross the middle triangle between the adjacent ones given + bool CanCross(SeBase *first, SeBase *second, SeBase *third, float r); + bool CanCross(SeDcdtFace *middle, SeBase *end1, SeBase *end2, float r); + //checks if a unit of radius r can enter the path from the start and goal + bool ValidEndpoints(SrArray& path, float x1, float y1, float x2, float y2, float r); + //checks if a unit has to cross the middle of the triangle to enter the path from the point given + bool ValidEndpoint(SeBase *s, float x, float y, bool left); + //determines if a unit can fit through the middle of the given triangle to get to the path + bool ValidEndpoint(SeDcdtFace *first, SeDcdtFace *second, float x, float y, float r); + //checks if a given path is valid for a unit of radius r + bool ValidPath(SrArray& path, float x1, float y1, float x2, float y2, float r); + //returns the closest point in a triangle to a given point for a unit of given radius + SrPnt2 ClosestPointTo(SeDcdtFace *face, float x, float y, float r); + //given a search node, construct the channel it represents + SrArray ConstructBaseChannel(SearchNode *goal); + //given a search node, construct a channel, filling in degree-1 and 2 nodes between those searched + void ConstructBasePath(SrArray &path, SearchNode *goalNode, SeDcdtFace *start, SeDcdtFace *goal, int direction = INVALID); + //the current channel + SrArray currentChannel; + //the current path + SrPolygon currentPath; + //the current start position + SrPnt2 startPoint; + //the current goal position + SrPnt2 goalPoint; + +//prototypes for functions used in abstracted space searching } + +//DJD: prototypes for utility functions { + +public: + //retrieves the vertices and whether the edges are constrained, for a given triangle + void TriangleInfo(SeDcdtFace *face, float &x1, float &y1, bool &e1, + float &x2, float &y2, bool &e2, float &x3, float &y3, bool &e3); + void TriangleInfo(SeBase *s, float &x1, float &y1, bool &e1, + float &x2, float &y2, bool &e2, float &x3, float &y3, bool &e3); + //revireves the vertices for a given triangle + void TriangleVertices(SeDcdtFace *face, float &x1, float &y1, + float &x2, float &y2, float &x3, float &y3); + void TriangleVertices(SeBase *s, float &x1, float &y1, + float &x2, float &y2, float &x3, float &y3); + //retrieves whether the edges are constrained, for a given triangle + void TriangleEdges(SeDcdtFace *face, bool &e1, bool &e2, bool &e3); + void TriangleEdges(SeBase *s, bool &e1, bool &e2, bool &e3); + //returns the midpoint of a given triangle + void TriangleMidpoint(SeDcdtFace *face, float &x, float &y); + SrPnt2 TriangleMidpoint(SeDcdtFace *face); + //returns whether a given edge is either constrained, or borders the "outside" polygon + bool Blocked(SeBase *s); + //returns the degree of a triangle's node (UNABSTRACTED if it has not been mapped) + int Degree(SeBase *s); + int Degree(SeFace *face); +private: + //returns whether the given angle is accute + bool IsAccute(float theta); + //returns whether the given angle is obtuse + bool IsObtuse(float theta); + //returns the length of the line segment (x1, y1)-(x2, y2) + float Length(float x1, float y1, float x2, float y2); + //returns the smaller of the two values passed + float Minimum(float a, float b); + //returns the larger of the two values passed + float Maximum(float a, float b); + //determines the angle between (x1, y1)-(x2, y2)-(x3, y3) + float AngleBetween(float x1, float y1, float x2, float y2, float x3, float y3); + //returns 0 if the points are colinear, <0 if they are in clockwise order, and >0 for counterclockwise + float Orientation(float x1, float y1, float x2, float y2, float x3, float y3); + //returns the minimum distance between the line segments (A1x, A1y)-(A2x, A2y) and (B1x, B1y)-(B2x, B2y) + float SegmentDistance(float A1x, float A1y, float A2x, float A2y, float B1x, float B1y, float B2x, float B2y); + //returns the minimum distance between the line segments (ls1x, ls1y)-(ls2x, ls2y) and (ll1x, ll1y)-(ll2x, ll2y), + //at least r away from the segments' end points + float SegmentDistance(float ls1x, float ls1y, float ls2x, float ls2y, float ll1x, float ll1y, float ll2x, float ll2y, float r); + //returns the minimum distance between point (x, y) and segment (x1, y1)-(x2, y2) + float PointSegmentDistance(float x, float y, float x1, float y1, float x2, float y2); + //returns the closest point on the segment (x1, y1)-(x2, y2) to point (x, y), + //at least r away from th segments' midpoints + SrPnt2 ClosestPointOn(float x1, float y1, float x2, float y2, float x, float y, float r); + +//prototypes for utility functions } + + private : + SeDcdtFace* _search_face() { return _cur_search_face? _cur_search_face:_first_symedge->fac(); } + void _find_intermediate_vertices_and_edges ( SeDcdtVertex* vini, int oid ); + bool _is_intersection_vertex ( SeDcdtVertex* v, int oid, SeDcdtVertex*& v1, SeDcdtVertex*& v2 ); + void _remove_vertex_if_possible ( SeDcdtVertex* v, const SrArray& vids ); + void _add_contour ( SeDcdtSymEdge* s, SrPolygon& vertices, SrArray& pindices ); + SeDcdtSymEdge* _find_one_obstacle (); + }; + +//================================== End of File ========================================= + +# endif // SE_DCDT_H diff --git a/source/dcdt/se/se_dcdt_mesh.cpp b/source/dcdt/se/se_dcdt_mesh.cpp new file mode 100644 index 0000000000..e373ca7847 --- /dev/null +++ b/source/dcdt/se/se_dcdt_mesh.cpp @@ -0,0 +1,148 @@ +#include "precompiled.h" + +# include "se_dcdt.h" + +//============================== SeDcdtVertex ================================== + +static void insert_if_not_there ( SrArray& a, int id ) + { + int i=0; + for ( i=0; i& a, const SrArray& ids ) + { + int i; + for ( i=0; i& ids ) + { + SeDcdtEdge *e; + SeDcdtSymEdge *si, *s; + + ids.size ( 0 ); + si = s = se(); + do { e = s->edg(); + if ( e->is_constrained() ) insert_ids ( ids, e->ids ); + s = s->rot(); + } while ( s!=si ); + } + +SrOutput& operator<< ( SrOutput& out, const SeDcdtVertex& v ) + { + return out << v.p; + } + +SrInput& operator>> ( SrInput& inp, SeDcdtVertex& v ) + { + return inp >> v.p; + } + +//=============================== SeDcdtEdge ================================== + +bool SeDcdtEdge::has_id ( int id ) const + { + for ( int i=0; i& cids ) + { + int i; + for ( i=0; i> ( SrInput& inp, SeDcdtEdge& e ) + { + return inp >> e.ids; + } + +//============================== SeDcdtTriManager ================================== + +void SeDcdtTriManager::get_vertex_coordinates ( const SeVertex* v, double& x, double & y ) + { + x = (double) ((SeDcdtVertex*)v)->p.x; + y = (double) ((SeDcdtVertex*)v)->p.y; + } + +void SeDcdtTriManager::set_vertex_coordinates ( SeVertex* v, double x, double y ) + { + ((SeDcdtVertex*)v)->p.x = (float) x; + ((SeDcdtVertex*)v)->p.y = (float) y; + } + +bool SeDcdtTriManager::is_constrained ( SeEdge* e ) + { + return ((SeDcdtEdge*)e)->is_constrained(); + } + +void SeDcdtTriManager::set_unconstrained ( SeEdge* e ) + { + ((SeDcdtEdge*)e)->set_unconstrained(); + } + +void SeDcdtTriManager::get_constraints ( SeEdge* e, SrArray& ids ) + { + ids = ((SeDcdtEdge*)e)->ids; + } + +void SeDcdtTriManager::add_constraints ( SeEdge* e, const SrArray& ids ) + { + ((SeDcdtEdge*)e)->add_constraints ( ids ); + } + +//============================== SeDcdtTriManager ================================== + +SrOutput& operator<< ( SrOutput& o, const SeDcdtInsPol& ob ) + { + if (ob.open) o << "(open) "; + return o;// << (const SrArray)ob; + } + +SrInput& operator>> ( SrInput& in, SeDcdtInsPol& ob ) + { + in.get_token(); +/* + if ( in.last_token()[0]=='(' ) + { ob.open = true; + in.get_token(); + in.get_token(); + } + else + { ob.open = false; + in.unget_token(); + } +*/ + return in;// >> (SrArray)ob; + } + +//=============================== End of File =============================== diff --git a/source/dcdt/se/se_mesh.cpp b/source/dcdt/se/se_mesh.cpp new file mode 100644 index 0000000000..b3ddf1fc59 --- /dev/null +++ b/source/dcdt/se/se_mesh.cpp @@ -0,0 +1,237 @@ +#include "precompiled.h" + +# include "sr_array.h" + +# include "se_mesh.h" + +# define IFTYPE(t,v,e,f) t==TypeVertex? (v) : t==TypeEdge? (e) : (f) +# define GETINFO(t,x) t==TypeVertex? (SeElement*)x->vertex : t==TypeEdge? (SeElement*)x->edge : (SeElement*)x->face + +//================================= static functions ===================================== + +static void fatal_error ( char* msg ) + { + sr_out.fatal_error ( msg ); + } + +//============================= SeMeshBase Private Methods ===================================== + +void SeMeshBase::_defaults () + { + _first = 0; + _op_last_msg = OpNoErrors; + _curmark = 0; + _marking = false; + _indexing = false; + _vertices = _edges = _faces = 0; + } + +SeBase* SeMeshBase::_op_error ( OpMsg m ) + { + _op_last_msg = (OpMsg)m; + + if ( _output_op_errors ) + { sr_out << "Error in SeMeshBase operation: " + << translate_op_msg(m) << srnl; + } + + return (SeBase*) 0; + } + +//============================ SeMeshBase general ===================================== + +semeshindex se_index_max_value = ((semeshindex)0)-1; // This is the unsigned greatest value; + +SeMeshBase::SeMeshBase ( SrClassManagerBase* vtxman, SrClassManagerBase* edgman, SrClassManagerBase* facman ) + { + _defaults (); + _output_op_errors = 0; + if ( !vtxman || !edgman || !facman ) + fatal_error ( "SeMeshBase::SeMeshBase(): null pointer received!" ); + _vtxman = vtxman; + _edgman = edgman; + _facman = facman; + _op_check_parms = 0; + } + +SeMeshBase::~SeMeshBase () + { + destroy (); + _vtxman->unref(); + _edgman->unref(); + _facman->unref(); + } + +/* from [mantyla] : + v-e+f = h0-h1+h2 ( Betti numbers ) + h0 = shells + h1 = 2 * genus ( every closed curve cut a part of the surface away ) + h2 = orientability = h0 + ==> v-e+f = s-2g+s = 2s-2g = 2(s-g), if s==1, => v-e+f=2-2g */ +int SeMeshBase::euler () const + { + return _vertices - _edges + _faces; + } + +int SeMeshBase::genus () const + { + return (_vertices-_edges+_faces-2)/(-2); + } + +int SeMeshBase::elems ( ElemType type ) const + { + switch ( type ) + { case TypeVertex : return _vertices; + case TypeEdge : return _edges; + case TypeFace : return _faces; + } + return 0; + } + +void SeMeshBase::invert_faces () + { + SeBase *se; + SeElement *edg, *edgi; + int i, j, symedges = _edges*2; + + if ( !_first ) return; + + SrArray nse(symedges,symedges); + SrArray nxt(symedges,symedges); + SrArray rot(symedges,symedges); + SrArray vtx(symedges,symedges); + + i=0; + edg = edgi = _first->edg(); + do { se = edg->se(); + for ( j=0; j<2; j++ ) + { if ( j==1 ) se = se->sym(); + vtx[i] = se->nxt()->vtx(); + nxt[i] = se->pri(); + rot[i] = se->nxt()->sym(); + nse[i] = se; + i++; + } + edg = edg->nxt(); + } while ( edg!=edgi ); + + for ( i=0; i_next = nxt[i]; + nse[i]->_rotate = rot[i]; + nse[i]->_vertex = vtx[i]; + vtx[i]->_symedge = nse[i]; + } + } + +int SeMeshBase::vertices_in_face ( SeBase *e ) const + { + int i; + SeBase *x=e->nxt(); + for ( i=1; x!=e; i++ ) x=x->nxt(); + return i; + } + +int SeMeshBase::vertex_degree ( SeBase *e ) const + { + int i=1; + SeBase *x; + for ( x=e->rot(); x!=e; i++ ) x=x->rot(); + return i; + } + +float SeMeshBase::mean_vertex_degree () const + { + if ( !_vertices ) return 0; + + SeVertex* vi = _first->vtx(); + SeVertex* v = vi; + float deg=0; + do { deg += vertex_degree(v->se()); + v = v->nxt(); + } while ( v!=vi ); + + return deg / (float)_vertices; + } + +//============================ SeMeshBase marking ===================================== + +// replace _marking and _indexing by _mark_status !! + +void SeMeshBase::_normalize_mark () // private method + { + SeElement *ei, *e; + + _curmark=1; + + if ( !_first ) return; + + ei=e=_first->vtx(); do { e->_index=0; e=e->nxt(); } while ( e!=ei ); + ei=e=_first->edg(); do { e->_index=0; e=e->nxt(); } while ( e!=ei ); + ei=e=_first->fac(); do { e->_index=0; e=e->nxt(); } while ( e!=ei ); + } + +void SeMeshBase::begin_marking () + { + if ( _indexing || _marking ) + fatal_error("SeMeshBase::begin_marking() not allowed as marking or indexing is already in use!"); + + _marking = true; + + if ( _curmark==se_index_max_value ) + { _normalize_mark (); + } + else _curmark++; + } + +void SeMeshBase::end_marking () + { + _marking = false; + } + +bool SeMeshBase::marked ( SeElement* e ) + { + if ( !_marking ) fatal_error ( "SeMeshBase::marked(e): marking is not active!\n" ); + return e->_index==_curmark? true:false; + } + +void SeMeshBase::mark ( SeElement* e ) + { + if ( !_marking ) fatal_error ( "SeMeshBase::mark(e): marking is not active!\n" ); + e->_index = _curmark; + } + +void SeMeshBase::unmark ( SeElement* e ) + { + if ( !_marking ) fatal_error ( "SeMeshBase::unmark(e): marking is not active!\n"); + e->_index = _curmark-1; + } + +//============================ SeMeshBase indexing ===================================== + +void SeMeshBase::begin_indexing () + { + if ( _marking || _indexing ) + fatal_error("SeMeshBase::begin_indexing() not allowed as marking or indexing is already in use!"); + _indexing = true; + } + +void SeMeshBase::end_indexing () + { + _indexing = false; + _normalize_mark (); + } + +semeshindex SeMeshBase::index ( SeElement* e ) + { + if ( !_indexing ) fatal_error ("SeMeshBase::index(e): indexing is not active!"); + return e->_index; + } + +void SeMeshBase::index ( SeElement* e, semeshindex i ) + { + if ( !_indexing ) fatal_error ("SeMeshBase::index(e,i): indexing is not active!"); + e->_index = i; + } + +//=== End of File =================================================================== diff --git a/source/dcdt/se/se_mesh.h b/source/dcdt/se/se_mesh.h new file mode 100644 index 0000000000..1867386eec --- /dev/null +++ b/source/dcdt/se/se_mesh.h @@ -0,0 +1,419 @@ + +# ifndef SE_MESH_H +# define SE_MESH_H + +/** \file se_mesh.h + * Symmetrical Edge Mesh Data Structure */ + +# include "sr_array.h" +# include "sr_input.h" +# include "sr_output.h" +# include "sr_class_manager.h" + +# include "se.h" + +// ====================================== SeMeshBase ====================================== + +// SeMeshBase documentation in the end of this file +class SeMeshBase + { public : + /*! Enumerator with the possible results of check_all(). */ + enum ChkMsg { ChkOk, //!< The check_all() method had success + ChkNxtError, //!< Some face loop defined by the nxt() operators is wrong + ChkRotError, //!< Some vertex loop defined by the rot() operators is wrong + ChkVtxError, //!< Vertex referencing is wrong + ChkEdgError, //!< Edge referencing is wrong + ChkFacError //!< Face referencing is wrong + }; + + /*! Enumerator with possible errors raised during the use of the topological operators. */ + enum OpMsg { OpNoErrors, //!< No Errors occured with operators + OpMevParmsEqual, //!< Parms in mev are equal + OpMevNotSameVtx, //!< Parms in mev not in the same vertex + OpMefNotSameFace, //!< Parms in mef not in the same face + OpKevLastEdge, //!< Cannot delete the last edge with kev + OpKevManyEdges, //!< More then one edge in parm vertex in kev + OpKevOneEdge, //!< Vertex of parm has only one edge in kev + OpKefSameFace, //!< Parm in kef must be between two different faces + OpMgUncompatibleFaces,//!< Parms are adjacent to faces with diferent number of vertices + OpMgSameFace, //!< Parm in mg must be between two different faces + OpFlipOneFace, //!< Parm in flip must be between two different faces + OpEdgColOneFace, //!< Parm in EdgCol must be between two different faces + OpVtxSplitNotSameVtx, //!< Parms in vtxsplit are not in the same vertex + OpVtxSplitParmsEqual //!< Parms in VtxSplit are equal + }; + + /*! Enumerator with the type of elements used in many methods arguments. */ + enum ElemType { TypeVertex, + TypeEdge, + TypeFace + }; + + private : // Data =================================================================================== + + SeBase* _first; // A pointer to the symedge of the mesh that is considered the first + char _op_check_parms; // Flag to tell if must check operators parameters (default false) + char _op_last_msg; // Keeps the last result of an operator (OpMsg type) + char _output_op_errors; // Flag that outputs all errors ocurred during operators (default true) + int _vertices, _edges, _faces; // Elem counters + semeshindex _curmark; // Current values for marking + char _marking, _indexing; // flags to indicate that marking or indexing is on + SrClassManagerBase *_vtxman, *_edgman, *_facman; // keep used managers + + private : // Private methods ========================================================================== + void _defaults (); + SeBase* _op_error ( OpMsg m ); + + public : // General mesh functions =================================================================== + + /*! Inits an empty mesh. It requires to receive three manager classes + dynamically allocated with operator new. These classes will manage data + stored in vertices, edges and faces. If no associated data is required, + the user can simply pass as argument "new SrClassManagerMeshElementManager", otherwise + an allocated user-derived object must be created and passed to this + constructor. SeMeshBase destructor will automatically delete them. */ + SeMeshBase ( SrClassManagerBase* vtxman, SrClassManagerBase* edgman, SrClassManagerBase* facman ); + + /*! Deletes all associated data: internal buffer, all mesh and its infos. */ + virtual ~SeMeshBase (); + + /*! Returns the initial symedge of the mesh, can be 0. */ + SeBase* first () const { return _first; } + + /*! Returns true if the mesh is empty, what happens when first() is null. */ + bool empty () const { return _first? false:true; } + + /*! Returns v-e+f. */ + int euler () const; + + /*! Returns (v-e+f-2)/(-2). */ + int genus () const; + + /*! Returns the number of elements of the given type. */ + int elems ( ElemType type ) const; + + /*! Returns the number of vertices in the mesh. */ + int vertices () const { return _vertices; } + + /*! Returns the number of edges in the mesh. */ + int edges () const { return _edges; } + + /*! Returns the number of faces in the mesh. */ + int faces () const { return _faces; } + + /*! Invert orientation of all faces. */ + void invert_faces (); + + /*! Returns the number of vertices in the face adjacent of the given symedge e. */ + int vertices_in_face ( SeBase* e ) const; + + /*! Returns the number of edges incident to the vertex adjacent of the given symedge e, + ie, the degree of the vertex. */ + int vertex_degree ( SeBase* e ) const; + + /*! Returns the mean vertex degree of the whole mesh */ + float mean_vertex_degree () const; + + /*! Returns true if the face incident to the given symedge e has exactly three edges. */ + bool is_triangle ( SeBase* e ) const { return e->nxt()->nxt()->nxt()==e? true:false; } + + private : // Marking and Indexing ===================================================== + void _normalize_mark (); + + public : + + /*! Allows the user to safely mark elements with mark(), marked() and unmark() + methods. Marking elements is necessary for many algorithms. + SeMeshBase mantains a current marker value, so that each time begin_marking() + is called, the marker value is incremented, guaranteing that old marked + elements have a different mark value. + When the max marking value is reached, a global normalization is done. + Attention: Marking and indexing can not be used at the same time. Also, + end_marking() must be called when marking operations are finished! */ + void begin_marking (); + + /*! Must be called after a begin_marking() call, when the user finishes using the + marking methods. Note: There is no problem if end_marking() is called without + a previously call to begin_marking() */ + void end_marking (); + + /*! See if an element is marked (begin_marking() should be called first) */ + bool marked ( SeElement* e ); + + /*! Unmark an element (begin_marking() should be called first) */ + void unmark ( SeElement* e ); + + /*! Mark an element (begin_marking() should be called first) */ + void mark ( SeElement* e ); + + /*! Indexing methods can be used only after a call to begin_indexing(), which permits + the user to associate any index to any element. Note however that after a call to + begin_indexing() the existant indices will have undefined values. This happens + because indices and markers share the same variables, so that both can not be used + at a same time. Method end_marking() must always be called after */ + void begin_indexing (); + + /*! To finish indexing mode, end_indexing() must be called. The method will do a global + normalization of the internal indices values, preparing them to be used for a later + marking or indexing session. It can be called without a previous begin_marking() + call, but note that it will always peform the global normalization. */ + void end_indexing (); + + /*! Retrieves the index of an element (begin_indexing() should be called first) */ + semeshindex index ( SeElement* e ); + + /*! Sets the index of an element (begin_indexing() should be called first) */ + void index ( SeEdge* e, semeshindex i ); + + public : // Operators Control =========================================================================== + + /*! Get safe mode status. When it is true, error msgs are generated, but + operations become a little slower because some minor checkings are + performed. The default mode is true only for the + debug version of the library (compiled with _DEBUG macro defined).*/ + bool safe_mode () { return _op_check_parms? true:false; } + + /*! Change the safe mode status. The default mode is false. */ + void safe_mode ( bool b ) { _op_check_parms=(char)b; } + + /*! Say if error messages should be printed or not (using sr_out), default is false. */ + void output_errors ( bool b ) { _output_op_errors=(char)b; } + + /*! Whenever an operator returns null, an error message is generated and + can be retrieved here. Each time an operator is called, a new message + is generated. And whenever the operator finishes succesfully OpNoErrors + is generated. */ + OpMsg last_op_msg () { return (OpMsg)_op_last_msg; } + + /*! Get a string description of the given message. */ + static char *translate_op_msg ( OpMsg m ); + + /*! Makes a global consistency check, for debug purposes, should always return ChkOk. */ + ChkMsg check_all () const; + + private : + void _new_vtx(SeBase* s); void _new_edg(SeBase* s); void _new_fac(SeBase* s); + void _del_vtx(SeElement* e); void _del_edg(SeElement* e); void _del_fac(SeElement* e); + + public : // Operators ================================================================================== + + /*! Destroy all the mesh and associated data. */ + void destroy (); + + /*! Initialize the mesh. See also SeMeshBase documentation. */ + SeBase* init (); + + /*! Make edge and vertex operator. See also SeMeshBase documentation. */ + SeBase* mev ( SeBase* s ); + + /*! Make edge and vertex operator, splitting a vertex loop. See also SeMeshBase documentation. */ + SeBase* mev ( SeBase* s1, SeBase* s2 ); + + /*! Make edge and face operator. See also SeMeshBase documentation. */ + SeBase* mef ( SeBase* s1, SeBase* s2 ); + + /*! Kill edge and vertex operator. See also SeMeshBase documentation. */ + SeBase* kev ( SeBase* x ); + + /*! Kill edge and vertex operator that can join two vertex loops. See also SeMeshBase documentation. */ + SeBase* kev ( SeBase* x, SeBase* *s ); + + /*! Kill edge and face operator. See also SeMeshBase documentation. */ + SeBase* kef ( SeBase* x, SeBase* *s ); + + /*! Make genus by joining two face contours and deleting two faces, + vertices and edges of s2 face. s1 and s2 must be symmetrically + placed, each one on its contour to join. (not fully tested) */ + SeBase* mg ( SeBase* s1, SeBase* s2 ); + + /*! Flip the edge of a face. See also SeMeshBase documentation. */ + SeBase* flip ( SeBase* x ); + + /*! Edge collide operator. See also SeMeshBase documentation. */ + SeBase* ecol ( SeBase* x, SeBase* *s ); + + /*! Vertex split operator. See also SeMeshBase documentation. */ + SeBase* vsplit ( SeBase* s1, SeBase* s2 ); + + /*! Add vertex operator. See also SeMeshBase documentation. */ + SeBase* addv ( SeBase* x ); + + /*! Delete vertex operator. See also SeMeshBase documentation. */ + SeBase* delv ( SeBase* y ); + + private : // IO ======================================================================================== + void _elemsave ( SrOutput& out, ElemType type, SeElement* first ); + void _elemload ( SrInput& inp, SrArray& e, const SrArray& s, SrClassManagerBase* man ); + + public : + + /*! Writes the mesh to the given output. save() will invoke the + virtual methods output() of the associated element managers. + Note: indexing is used during save(). */ + bool save ( SrOutput& out ); + + /*! Reads the mesh from the given input. load() will invoke the + virtual methods input() of the associated element managers. + Note: indexing is used during load(). */ + bool load ( SrInput& inp ); + }; + +/*! This is the template version of the SeMeshBase class, that redefines some methods + of SeBase only for the purpose of including type casts to user defined classes. + All methods are implemented inline just calling the corresponding method of + the base class but correctly applying type casts to convert default types + to user types. It must be used together with SeTpl. */ +template //, class VM, class EM, class FM> +class SeMesh : public SeMeshBase + { public : + typedef Se S; + typedef SrClassManager VM; + typedef SrClassManager EM; + typedef SrClassManager FM; + + SeMesh () : SeMeshBase ( new VM, new EM, new FM ) {} + + S* first () const { return (S*)SeMeshBase::first(); } + + S* init () { return (S*)SeMeshBase::init(); } + S* mev ( S* s ) { return (S*)SeMeshBase::mev(s); } + S* mev ( S* s1, S* s2 ) { return (S*)SeMeshBase::mev(s1,s2); } + S* mef ( S* s1, S* s2 ) { return (S*)SeMeshBase::mef(s1,s2); } + S* kev ( S* x ) { return (S*)SeMeshBase::kev(x); } + S* kev ( S* x, S* *s ) { return (S*)SeMeshBase::kev(x,(SeBase**)s); } + S* kef ( S* x, S* *s ) { return (S*)SeMeshBase::kef(x,(SeBase**)s); } + S* mg ( S* s1, S* s2 ) { return (S*)SeMeshBase::mg(s1,s2); } + S* flip ( S* x ) { return (S*)SeMeshBase::flip(x); } + S* ecol ( S* x, S* *s ) { return (S*)SeMeshBase::ecol(x,(SeBase**)s); } + S* vsplit ( S* s1, S* s2 ) { return (S*)SeMeshBase::vsplit(s1,s2); } + S* addv ( S* x ) { return (S*)SeMeshBase::addv(x); } + S* delv ( S* y ) { return (S*)SeMeshBase::delv(y); } + }; + +/*! \class SeMeshBase se_mesh.h + \brief manages topology and attached information of a symedge mesh. + + SeMeshBase uses SeBase as argument for many methods, specially for the + topological operators. Each SeBase instance has only one vertex, edge + and face adjacent (or associated) and so it can be used to reference any + of these elements. SeMeshBase destructor calls destroy(), that will call also + each free() method of registered SeElementManagers in the mesh. To attach info + to a mesh, the user must derive SrClassManagerBase, suply the required virtual + methods, and push it in SeMeshBase with push_info() method. + + Operators error handling is performed when the checking mode is true (the + default), all operators may then return a 0 (null) pointer when their + parameters are illegal. In such cases, a call to last_op_msg() will return + the error occured. Next is a schema explaining each operator: \code + + *======================================================================== + * Operator init() + * + * x=init() v1 o o v2 -> v1 o------o v2 + * destroy() <----- + * destroy all the previous structure. x + * + + *======================================================================== + * Operator mev(s) + * + * s x + * <----- <----- + * x=mev(s) o------o o v -> o------o------o v + * s=kev(x) / | / | + * / | / | + * To kev(x), only o o o o + * one edge in v; + * + + *======================================================================== + * Operator mev(s1,s2) + * o o + * | \ + * | s1 \ + * | ----> \ + * x=mev(s1,s2) o------o---------o -> o------o------o--o + * s1=kev(x,&s2) <---- | o | <--- v + * s2 | v | x + * To mev, s1 & s2 on o o + * the same vertex + * and s1!=s2 + * + * To kev(x), more than one edge of x must be incident to v + * + + *========================================================================= + * Operator mef(s1,s2) + * + * x=mef(s1,s2) o-------o-------o o-------o-------o + * s1=kef(x,&s2) | <---- | | | | + * | s1 | | | | + * | | | || | + * s1 & s2 on | | -> | e||x | + * same face | | | |V | + * | s2 | | | f | + * in kef(x), x->sym | ----> | | | | + * must be in a diff o-------o-------o o-------o-------o + * face than x + * + + *======================================================================== + * Operator flip(x) + * + * y=flip(x) o---o---o o---o---o + * / | | / | + * / | | / | + * x & x->sym must / || | / | + * be in diff faces o ||x o -> o---------------o + * and share the | |V / | <------ / + * same edge | | / | y / + * | | / | / + * o---o---o o---o---o + * + * This operator acts as an "edge rotation"; that in triangulations is the + * same as the well known flip (or swap) operator. + * + + *======================================================================== + * Operator ecol(x,&s2) + * + * s1=ecol(x,&s2) o o + * x=vsplit(s1,s2) / | s1| + * / | ^| + * \ / x | / \ || / + * x and x->sym must \ / --> |/ \ |/ + * be in diff faces o---------o v -> o v + * and share the / \ |\ / |\ + * same edge / \ | \ / | \ + * / \ | / / | + * / \ | / /s2 | + * o V o + * + * In the process, 3 edges, 2 faces and 1 vertex are created/destroyed. + * This operator acts as an "edge collapse", and has + * the "vertex split" operator as inverse. + * The three destroyed edges are incident to x->vtx(), and not v. + * + + *======================================================================== + * Operator addv() + * + * y=addv(x) ----------- ----------- + * x=delv(y) | <-- | |\ /| + * | x | | \ y^/ | + * | | | \ // | + * | | | \ / | + * | o | -> | o | + * | | | / \ | + * | | | / \ | + * | | | / \ | + * | | |/ \| + * ----------- ----------- + * + * + \endcode */ + +//============================ End of File =============================== + +# endif // SE_MESH_H + diff --git a/source/dcdt/se/se_mesh_import.cpp b/source/dcdt/se/se_mesh_import.cpp new file mode 100644 index 0000000000..e86f6a41c3 --- /dev/null +++ b/source/dcdt/se/se_mesh_import.cpp @@ -0,0 +1,535 @@ +#include "precompiled.h" + +# include "sr_tree.h" +# include "se_mesh_import.h" + +//# define SR_USE_TRACE1 // basic messages +//# define SR_USE_TRACE2 // more messages +//# define SR_USE_TRACE4 // active contour +//# define SR_USE_TRACE5 // tree +# include "sr_trace.h" + +//======================= static data ============================================= + +// Question: Could I substitute EdgeTree by an EdgeHashTable ...? + +class Edge : public SrTreeNode + { public : + int a, b; // Edge vertices, not oriented + int v; // The other vertex of the adjacent face + int f; // The index of the edge's face + Edge *sym; // The other edge, belonging to the other face sharing this edge + Edge *e2, *e3; // The other two edges of this face + public : + Edge ( int i1=0, int i2=0, int i3=0 ) : f(0), sym(0), e2(0), e3(0) { a=i1; b=i2; v=i3; } + void set_fac_elems ( Edge *b, Edge *c, int fac ) { e2=b; e3=c; f=fac; } + void swap() { int tmp=a; a=b; b=tmp; } + + friend SrOutput& operator<< ( SrOutput& o, const Edge& e ) + { return o<<'['<> ( SrInput& i, Edge& e ) { return i; } + + friend int sr_compare ( const Edge* e1, const Edge* e2 ); + }; + +int sr_compare ( const Edge *e1, const Edge *e2 ) + { + if ( e1->aa ) return -1; + if ( e1->a>e2->a ) return 1; + if ( e1->bb ) return -1; + if ( e1->b>e2->b ) return 1; + return 0; + } + +/* +# ifdef SE_USR_TRACE4 +struct PFace { SeBase *e; SeMeshBase *m; } PFaceInst; +FgOutput &operator << ( FgOutput &o, PFace &f ) + { + o << '['; + SeBase *e=f.e; + while ( true ) + { o<<(int)f.m->index(e->vtx()); + e = e->nxt(); + if ( e!=f.e ) o<<' '; else break; + } + return o<<']'; + } +# define SR_TRACE_CONTOUR(x,s) { PFaceInst.e=x; PFaceInst.m=s; SR_TRACE4 ( "Active Contour = "<< PFaceInst ); } +# else +# define SR_TRACE_CONTOUR(x,s) +# endif +*/ + +# define SR_TRACE_CONTOUR(x,s) + +//=========================== EdgeTree ======================================== + +class EdgeTree : public SrTree + { public : +// EdgeTree() {} + public : + Edge *insert_edge ( int a, int b, int v ); + bool insert_face ( int a, int b, int c, int f ); + Edge* search_edge ( int a, int b ); + void remove_face ( Edge *e ); + }; + +Edge *EdgeTree::insert_edge ( int a, int b, int v ) + { + Edge *e = new Edge ( a, b, v ); + if ( !insert(e) ) + { Edge *x = cur(); + e->swap(); + if ( !insert(e) ) + { SR_TRACE2 ( "Could not insert edge ("<sym = e; + e->sym = x; + } + else // search if the sym edge is there and set sym pointers + { // (to try: sort (a,b) to avoid checking if the sym exists...) + Edge eba ( b, a, v ); + Edge *esym = search ( &eba ); + if ( esym ) + { esym->sym = e; + e->sym = esym; + } + } + return e; + } + +bool EdgeTree::insert_face ( int a, int b, int c, int f ) + { + Edge *e1 = insert_edge ( a, b, c ); + Edge *e2 = insert_edge ( b, c, a ); + Edge *e3 = insert_edge ( c, a, b ); + + if (e1) e1->set_fac_elems ( e2, e3, f ); + if (e2) e2->set_fac_elems ( e1, e3, f ); + if (e3) e3->set_fac_elems ( e1, e2, f ); + + return e1&&e2&&e3? true:false; + } + +Edge* EdgeTree::search_edge ( int a, int b ) + { + Edge key(a,b,0); + + Edge *e = search(&key); + if ( !e ) + { key.swap(); + e=search(&key); + } + + return e; + } + +void EdgeTree::remove_face ( Edge *e ) + { + if (!e) return; + + // e2 and/or e3 can be null in case non-manifold edges were found during the + // construction of the edge tree with start() + Edge *e2 = e->e2; + Edge *e3 = e->e3; + + extract ( e ); + if ( e->sym ) e->sym->sym=0; + + if (e2) + { if ( e2->sym ) e2->sym->sym=0; + remove ( e2 ); + } + + if (e3) + { if ( e3->sym ) e3->sym->sym=0; + remove ( e3 ); + } + } + +//======================= internal data ================================== + +static SeBase* init ( SeMeshImport *self, SeMeshBase* m, int a, int b ) + { + SeBase *s = m->init (); + m->index(s->vtx(),(semeshindex)b); + m->index(s->nxt()->vtx(),(semeshindex)a); + self->attach_vtx_info ( s->vtx(), b ); + self->attach_vtx_info ( s->nxt()->vtx(), a ); + self->attach_edg_info ( s->edg(), b, a ); + return s; + } + +static SeBase* mev ( SeMeshImport *self, SeMeshBase* m, SeBase* s, int ev, int v ) + { + s = m->mev ( s ); + self->attach_edg_info ( s->edg(), ev, v ); + self->attach_vtx_info ( s->vtx(), v ); + m->index ( s->vtx(), (semeshindex)v ); + return s; + } + +static SeBase* mef ( SeMeshImport *self, SeMeshBase* m, SeBase* s1, SeBase* s2, int a, int b, int f ) + { + s2 = m->mef ( s1, s2 ); + self->attach_edg_info ( s2->edg(), a, b ); + self->attach_fac_info ( s2->fac(), f ); + return s2; + } + +class SeMeshImport::Data + { public : + SeMeshImport *self; + EdgeTree tree; + SrArray active_contours; // stack of contours + SrArray contours_to_solve; // keep contours that will result in genus or holes + SrArray vtxsymedges; // keep pointer for vertices already inserted + public : + Data ( SeMeshImport *x ); + SeMeshBase *init_shell (); + int start ( const int *triangles, int numtris, int numvtx ); + void solve_lasting_contours ( SeMeshBase *m ); + void glue_face_on_one_edge ( SeMeshBase *m, SeBase *&s, Edge *e ); + void glue_face_on_two_edges ( SeMeshBase *m, SeBase *&s, Edge *e ); + void glue_face_on_three_edges ( SeMeshBase *m, SeBase *s, Edge *e ); + }; + +SeMeshImport::Data::Data ( SeMeshImport *x ) + { + vtxsymedges.size(0); + vtxsymedges.capacity(256); + active_contours.size(0); + active_contours.capacity(32); + self = x; + } + +SeMeshBase *SeMeshImport::Data::init_shell () + { + SR_TRACE1 ( "Getting new shell..." ); + SeMeshBase *m = self->get_new_shell (); + if ( !m ) return 0; + + SR_TRACE1 ( "Getting seed face from the root of the tree..." ); + + Edge *e = tree.root(); + int a=e->a; int b=e->b; int c=e->v; // the order of the vertices is random + int f=e->f; + tree.remove_face ( e ); + delete e; + + SR_TRACE1 ( "Creating seed face = ["<begin_indexing (); + + SeBase *s; + + s = init ( self, m, a, b ); + s = mev ( self, m, s, b, c ); + mef ( self, m, s, s->nxt()->nxt(), a, c, f ); + + vtxsymedges[c] = s; + vtxsymedges[b] = s->nxt(); + vtxsymedges[a] = s->nxt()->nxt(); + + active_contours.push() = s; + + SR_TRACE2 ("seed face info: "<<(int)s->sym()->fac()<<", active face info: "<<(int)s->fac() ); + + return m; + } + +int SeMeshImport::Data::start ( const int *triangles, int numtris, int numvtx ) + { + int i; + + active_contours.size(0); + + SR_TRACE1 ( "Initializing vertices table with size "<vtx() != x->nxt()->vtx() ) + { x = x->nxt(); + if ( x==s2 ) return 0; // not coincident + } + + // a common starting vertex was found, so that the contours will be coincident + // or we'll have non-manifold vertices. Let's verify : + s2 = x; + do { if ( s1->vtx() != s2->nxt()->vtx() ) + { printf("Non-manifold vertex detected!\n"); return 0; } + s1 = s1->nxt(); + s2 = s2->pri(); + } while ( s2!=x ); + + return x; + } + +// should finished the contours by detecting loops to join or holes info to attach +void SeMeshImport::Data::solve_lasting_contours ( SeMeshBase *m ) + { + int i; + bool genus_increased; + SeBase *s, *c; + + while ( !contours_to_solve.empty() ) + { genus_increased = false; + s = contours_to_solve.pop(); + SR_TRACE_CONTOUR(s,m); + for ( i=0; img ( s, c ); + break; + } + } + if ( !genus_increased ) + { SR_TRACE1 ( "Closing last contour as a hole..." ); + self->attach_hole_info ( s->fac() ); + } + } + } + +void SeMeshImport::Data::glue_face_on_one_edge ( SeMeshBase *m, SeBase *&s, Edge *e ) + { + tree.remove_face ( e ); + + SeBase *snxt = s->nxt(); + + s = mev ( self, m, s, e->a, e->v ); + s = mef ( self, m, snxt, s, e->b, e->v, e->f ); + + vtxsymedges[e->a] = s->nxt()->sym(); + vtxsymedges[e->b] = snxt; + vtxsymedges[e->v] = snxt->pri(); + s = snxt; + + delete e; + } + +void SeMeshImport::Data::glue_face_on_two_edges ( SeMeshBase* m, SeBase*& s, Edge* e ) + { + tree.remove_face ( e ); + SeElement *ve = vtxsymedges[e->v]->vtx(); + SeBase *s1, *s2; + + if ( s->pri()->vtx()==ve ) + { s1=s->nxt(); s2=s->pri(); } + else + { s1=s->nxt()->nxt(); s2=s; } + + mef ( self, m, s1, s2, m->index(s1->vtx()), m->index(s2->vtx()), e->f ); + s = s1->pri(); + + vtxsymedges[(int)m->index(s->vtx())] = s; + vtxsymedges[(int)m->index(s1->vtx())] = s1; + delete e; + } + +void SeMeshImport::Data::glue_face_on_three_edges ( SeMeshBase* /*m*/, SeBase* s, Edge* e ) + { + tree.remove_face ( e ); + self->attach_fac_info ( s->fac(), e->f ); + delete e; + } + +//======================== SeMeshImport ======================================== + +SeMeshImport::SeMeshImport () + { + _data = new Data ( this ); + } + +SeMeshImport::~SeMeshImport () + { + for ( int i=0; ivtxsymedges.capacity(0); + _data->active_contours.capacity(0); + } + +SeMeshImport::Msg SeMeshImport::start ( const int *triangles, int numtris, int numvtxs ) + { + SR_TRACE1 ( "Start :" ); + + int not_inserted = _data->start ( triangles, numtris, numvtxs ); + + shells.size(0); + SeMeshBase *m = _data->init_shell (); // will push something in active_contours + if ( !m ) return MsgNullShell; + shells.push()=m; + + SR_TRACE_CONTOUR(_data->active_contours.top(),m); + SR_TRACE4 ( "Face info of active contour = " << (int)(_data->active_contours.top()->fac()) ); + SR_TRACE5 ( "Tree = "<<_data->tree ); + SR_TRACE1 ( "Start finished !" ); + SR_TRACE1 ( "Faces Lost: "<0? MsgNonManifoldFacesLost : MsgOk; + } + +static SeBase* in_same_face ( SeBase *e, SeElement *v ) + { + SeBase *x = e; + do { if ( x->vtx()==v ) return x; + x = x->nxt(); + } while ( x!=e ); + return 0; + } + +SeMeshImport::Msg SeMeshImport::next_step () + { + int a, b; + Edge *e; + SeBase *ini, *s, *sv; + + int edges_found = 0; + + SR_TRACE2 ( " " ); + SR_TRACE2 ( "Next step :" ); + + EdgeTree &tree = _data->tree; + SrArray &active_contours = _data->active_contours; + SrArray &contours_to_solve = _data->contours_to_solve; + SrArray &vtxsymedges = _data->vtxsymedges; + + SR_TRACE2 ( "Active contours : "<solve_lasting_contours ( shells.top() ); + } + for ( int i=0; iend_indexing(); + SR_TRACE1 ( "Finished : "<init_shell(); + return MsgOk; // return now to cope with degenerated data (a shell with a single triangle). + } + + s = ini = active_contours.top(); + + while ( true ) + { + a = (int) shells.top()->index(s->vtx()); + b = (int) shells.top()->index(s->nxt()->vtx()); + SR_TRACE2 ( "Searching edge "<v] ) // the other vertex is already in the mesh + sv = in_same_face ( s, vtxsymedges[e->v]->vtx() ); + + if ( !vtxsymedges[e->v] || !sv ) // if the other vertex is free or is in another contour + { SR_TRACE1 ( "Gluing face "<v<<" on one edge... " ); + _data->glue_face_on_one_edge ( shells.top(), s, e ); + active_contours.top() = s; + SR_TRACE_CONTOUR(s,shells.top()); + SR_TRACE5 ( "Tree = "<nxt()->nxt()->nxt()==s ) // trying to close the active contour that is a triangle + { SR_TRACE1 ( "Gluing last face "<v<<" of current active contour..." ); + _data->glue_face_on_three_edges ( shells.top(), s, e ); + active_contours.pop(); + SR_TRACE1 ( "Active contour popped... " ); + SR_TRACE5 ( "Tree = "<nxt()->nxt()->vtx()==vtxsymedges[e->v]->vtx() || + s->pri()->vtx()==vtxsymedges[e->v]->vtx() ) + { SR_TRACE1 ( "Gluing face "<v<<" on two edges... " ); + _data->glue_face_on_two_edges ( shells.top(), s, e ); + active_contours.top() = s; + SR_TRACE_CONTOUR(s,shells.top()); + SR_TRACE5 ( "Tree = "<mef ( s, sv ); + attach_edg_info ( s->edg(), shells.top()->index(s->vtx()), shells.top()->index(sv->vtx()) ); + active_contours.push()=sv; + return MsgOk; + } + else + { SR_TRACE1 ( "XXXXXXXXXXXXXXXXXX undef situation!! XXXXXXXXXXXXXXXXXXXXXXXXXXXX" ); + SR_TRACE_CONTOUR(active_contours.top(),shells.top()); + SR_TRACE1 ( "s = ["< shells; + + private : + class Data; + Data *_data; + + public : + /*! Default constructor */ + SeMeshImport (); + + /*! The destructor will delete all meshes stored in the shells array. + So that if the user wants to become responsible to maintain the + imported meshes, he/she must take the pointers in the shell array + and set the array size to 0. */ + virtual ~SeMeshImport (); + + /*! Compress some used internal buffers */ + void compress_buffers (); + + /*! Defines the information describing the triangle-based description to + be imported. The list of triangles contains indices to the vertices. + Geometrical information can be attached to the mesh by re-writing + the availble virtual methods, which allow transforming indices + into any kind of user-related data. */ + Msg start ( const int *triangles, int numtris, int numvtx ); + + /*! Join another face to the mesh, expanding the current active contour. + This method should be called until MsgFinished or errors occur */ + Msg next_step (); + + public : + + /* This virtual method must be derived to return a valid SeMesh for + each new shell encountered in the data being analysed. */ + virtual SeMeshBase* get_new_shell ()=0; + + /*! Optional virtual method to attach vertex information. + The default implementation does nothing. */ + virtual void attach_vtx_info ( SeVertex* v, int vi ); + + /*! Optional virtual method to attach edge information. + The default implementation does nothing. */ + virtual void attach_edg_info ( SeEdge* e, int a, int b ); + + /*! Optional virtual method to attach face information. + The default implementation does nothing. */ + virtual void attach_fac_info ( SeFace* f, int fi ); + + /*! Optional virtual method to attach face information of holes found. + The default implementation does nothing. */ + virtual void attach_hole_info ( SeFace* f ); + }; + +//============================== end of file =============================== + +# endif // SE_MESH_IMPORT_H diff --git a/source/dcdt/se/se_mesh_io.cpp b/source/dcdt/se/se_mesh_io.cpp new file mode 100644 index 0000000000..96775a9243 --- /dev/null +++ b/source/dcdt/se/se_mesh_io.cpp @@ -0,0 +1,219 @@ +#include "precompiled.h" + +# include +# include "se_mesh.h" + +//# define SR_USE_TRACE1 // IO trace +# include "sr_trace.h" + +//================================ IO ========================================= + +# define ID(se) int(se->_edge) + +void SeMeshBase::_elemsave ( SrOutput& out, SeMeshBase::ElemType type, SeElement* first ) + { + SrClassManagerBase* man; + SeElement *e; + int esize; + char* st; + + if ( type==TypeVertex ) + { esize=_vertices; man=_vtxman; st="Vertices"; } + else if ( type==TypeEdge ) + { esize=_edges; man=_edgman; st="Edges"; } + else + { esize=_faces; man=_facman; st="Faces"; } + + out << srnl << st << srspc << esize << srnl; + //fprintf ( f, "\n%s %d\n", st, esize ); + + e = first; + do { e->_index = 0; + out << ID(e->_symedge) <_symedge) ); + man->output(out,e); + out << srnl; + //fprintf ( f, "\n" ); + e = e->nxt(); + } while (e!=first); + + SR_TRACE1 ( esize << " elements written !" ); + } + +bool SeMeshBase::save ( SrOutput& out ) + { + SeBase *se; + SeElement *el, *eli; + int symedges, i, j; + + out << "SYMEDGE MESH DESCRIPTION\n\n"; + //fprintf ( f, "SYMEDGE MESH DESCRIPTION\n\n" ); + SR_TRACE1("Header written."); + + symedges = _edges*2; + out << "SymEdges " << symedges << srnl; + //fprintf ( f, "SymEdges %d\n", symedges ); + + if ( empty() ) return true; + + SrArray ses(symedges); + SrArray edg(symedges); + + // get adjacency information of symedges to save: + SR_TRACE1("Mounting S array..."); + i=0; + el = eli = _first->edg(); + do { se = el->se(); + el->_index = i/2; + for ( j=0; j<2; j++ ) + { if ( j==1 ) se = se->sym(); + ses[i] = se; + edg[i] = se->_edge; // save edge pointer + se->_edge = (SeEdge*)i; // and use it to keep indices + i++; + } + el = el->nxt(); + } while ( el!=eli ); + + // adjust vtx and fac indices: + SR_TRACE1("Adjusting indexes..."); + + i = 0; + el = eli = _first->vtx(); + do { el->_index=i++; el=el->nxt(); } while (el!=eli); + + i = 0; + el = eli = _first->fac(); + do { el->_index=i++; el=el->nxt(); } while (el!=eli); + + for ( i=0; i_next) << srspc + << ID(ses[i]->_rotate) << srspc + << ses[i]->_vertex->_index << srspc + << edg[i]->_index << srspc + << ses[i]->_face->_index << srspc + << srnl; + /*fprintf ( f, "%d %d %d %d %d\n", + ID(ses[i]->_next), + ID(ses[i]->_rotate), + ses[i]->_vertex->_index, + edg[i]->_index, + ses[i]->_face->_index );*/ + } + + SR_TRACE1("Symedges written."); + + _elemsave ( out, TypeVertex, _first->vtx() ); + _elemsave ( out, TypeEdge, edg[0] ); + _elemsave ( out, TypeFace, _first->fac() ); + + _curmark = 1; + _marking = _indexing = false; + for ( i=0; i_edge = edg[i]; + + SR_TRACE1 ( "save OK !" ); + return true; + } + +# undef ID + +//---------------------------------- load -------------------------------- + +/* +static int load_int ( FILE* f ) + { + int i; + fscanf ( f, "%d", &i ); + return i; + } + +static void skip ( FILE* f, int n ) + { + char s[60]; + while ( n-- ) fscanf ( f, "%s", s ); + } +*/ +void SeMeshBase::_elemload ( SrInput& inp, SrArray& E, + const SrArray& S, SrClassManagerBase* man ) + { + int i, x; + + inp.get_token(); // skip elem type label + //skip ( f, 1 ); // skip elem type label + + inp >> x; E.size(x); + //x = load_int(f); E.size(x); + + for ( i=0; i> x; //x = load_int(f); + E[i] = (SeElement*)man->alloc(); + E[i]->_symedge = S.get(x); + man->input ( inp, E[i] ); + //man->read ( E[i], f ); + if ( i>0 ) E[0]->_insert(E[i]); + } + + SR_TRACE1 ( e.size() << " elements loaded !" ); + } + +bool SeMeshBase::load ( SrInput& inp ) + { + //char buf[64]; + int i, x; + + //fscanf ( f, "%s", buf ); if ( strcmp(buf,"SYMEDGE") ) return false; + //fscanf ( f, "%s", buf ); if ( strcmp(buf,"MESH") ) return false; + //fscanf ( f, "%s", buf ); if ( strcmp(buf,"DESCRIPTION") ) return false; + inp.get_token(); if ( inp.last_token()!="SYMEDGE" ) return false; + inp.get_token(); if ( inp.last_token()!="MESH" ) return false; + inp.get_token(); if ( inp.last_token()!="DESCRIPTION" ) return false; + SR_TRACE1 ( "Signature ok." ); + + // destroy actual structure to load the new one: + destroy (); + + // load indices and store them in an array: + inp.get_token(); // skip SymEdges label + //skip ( f, 1 ); // skip SymEdges label + inp >> i; //i = load_int ( f ); + SrArray S(i); + for ( i=0; i>x; S[i]->_next = (SeBase*)x; + inp>>x; S[i]->_rotate = (SeBase*)x; + inp>>x; S[i]->_vertex = (SeVertex*)x; + inp>>x; S[i]->_edge = (SeEdge*)x; + inp>>x; S[i]->_face = (SeFace*)x; + } + SR_TRACE1 ( "Symedges loaded." ); + + // load infos: + SrArray V; + SrArray E; + SrArray F; + _elemload ( inp, V, S, _vtxman ); + _elemload ( inp, E, S, _edgman ); + _elemload ( inp, F, S, _facman ); + + // convert indices to pointers: + for ( i=0; i_next = S[(int)(S[i]->_next)]; + S[i]->_rotate = S[(int)(S[i]->_rotate)]; + S[i]->_vertex = V[(int)(S[i]->_vertex)]; + S[i]->_edge = E[(int)(S[i]->_edge)]; + S[i]->_face = F[(int)(S[i]->_face)]; + } + + // adjust internal variables: + _first = S[0]; + _vertices = V.size(); + _edges = E.size(); + _faces = F.size(); + _curmark = 1; + _marking = _indexing = false; + + SR_TRACE1 ( "load OK !" ); + return true; + } + +//=== End of File =================================================================== diff --git a/source/dcdt/se/se_mesh_operators.cpp b/source/dcdt/se/se_mesh_operators.cpp new file mode 100644 index 0000000000..93d934537a --- /dev/null +++ b/source/dcdt/se/se_mesh_operators.cpp @@ -0,0 +1,571 @@ +#include "precompiled.h" + +# include +# include "se_mesh.h" + +//# define CHECKALL_OPS // Will call check_all() after each operator +//# define SR_USE_TRACE1 // Operators trace +# include "sr_trace.h" + +//===================================== Some Macros ======================================= + +# ifdef CHECKALL_OPS +# include +# define CHECKALL { int err=check_all(); if(err!=0) { printf("CHECK ERROR: %d\n",err ); exit(1); } } +# else +# define CHECKALL +# endif + +# define SE_MAX3(a,b,c) (a>b? (a>c?(a):(c)):(b>c?(b):(c))) + +# define EDG_CREATE(x,y) x=new SeBase; y=new SeBase; \ + x->_next=y; x->_rotate=x; \ + y->_next=x; y->_rotate=y; + +# define DEF_SPLICE_VARS SeBase *sp_xnxt, *sp_ynxt, *sp_xsym + +// the effect is: swap(x.nxt,y.nxt); swap(x.nxt.rot,y.nxt.rot) +# define SPLICE(x,y) sp_xnxt = x->_next; \ + sp_ynxt = y->_next; \ + sp_xsym = sp_xnxt->_rotate; \ + x->_next = sp_ynxt; \ + y->_next = sp_xnxt; \ + sp_xnxt->_rotate = sp_ynxt->_rotate; \ + sp_ynxt->_rotate = sp_xsym + +//======================================== private members ======================================== + +void SeMeshBase::_new_vtx ( SeBase *s ) + { + _vertices++; + SeVertex* v = (SeVertex*)_vtxman->alloc(); + v->_symedge = s; + if ( _first ) _first->vtx()->_insert(v); + s->_vertex = v; + } + +void SeMeshBase::_new_edg ( SeBase *s ) + { + _edges++; + SeEdge* e = (SeEdge*)_edgman->alloc(); + e->_symedge = s; + if ( _first ) _first->edg()->_insert(e); + s->_edge = e; + } + +void SeMeshBase::_new_fac ( SeBase *s ) + { + _faces++; + SeFace* f = (SeFace*)_facman->alloc(); + f->_symedge = s; + if ( _first ) _first->fac()->_insert(f); + s->_face = f; + } + +void SeMeshBase::_del_vtx ( SeElement *e ) + { + _vertices--; + if ( e==_first->vtx() ) _first = e->nxt()==e? 0:e->nxt()->se(); + e->_remove(); + _vtxman->free ( e ); + } + +void SeMeshBase::_del_edg ( SeElement *e ) + { + _edges--; + if ( e==_first->edg() ) _first = e->nxt()==e? 0:e->nxt()->se(); + e->_remove(); + _edgman->free ( e ); + } + +void SeMeshBase::_del_fac ( SeElement *e ) + { + _faces--; + if ( e==_first->fac() ) _first = e->nxt()==e? 0:e->nxt()->se(); + e->_remove(); + _facman->free ( e ); + } + +//==================================== operator related funcs ====================================== + +char* SeMeshBase::translate_op_msg ( OpMsg m ) + { + switch (m) + { case OpNoErrors : return "No Errors occured"; + case OpMevParmsEqual : return "Parms in mev are equal"; + case OpMevNotSameVtx : return "Parms in mev are not in the same vertex"; + case OpMefNotSameFace : return "Parms in mef are not in the same face"; + case OpKevLastEdge : return "Cannot delete the last edge with kev"; + case OpKevManyEdges : return "More then one edge in parm vertex in kev"; + case OpKevOneEdge : return "Vertex of parm has only one edge in kev"; + case OpKefSameFace : return "Parm in kef must be between two different faces"; + case OpMgUncompatibleFaces : return "Parms in mg must be adjacent to faces with the same number of vertices"; + case OpMgSameFace : return "Parms in mg must be, each one, adjacent to a different face"; + case OpFlipOneFace : return "Parm in flip must be between two different faces"; + case OpEdgColOneFace : return "Parm in edgcol must be between two different faces"; + case OpVtxSplitParmsEqual : return "Parms in vtxsplit are equal"; + case OpVtxSplitNotSameVtx : return "Parms in vtxsplit not in the same vertex"; + } + return "Undefined error code"; + } + + +static SeMeshBase::ChkMsg check ( SeBase *x, int max ) + { + int i=0; + SeBase *xi = x; + x = xi->nxt(); + while ( x!=xi ) { x=x->nxt(); i++; if(i>max) return SeMeshBase::ChkNxtError; } + i=0; + x = xi->rot(); + while ( x!=xi ) { x=x->rot(); i++; if(i>max) return SeMeshBase::ChkRotError; } + return SeMeshBase::ChkOk; + } + +SeMeshBase::ChkMsg SeMeshBase::check_all () const + { + ChkMsg m; + int i, max = 2*SE_MAX3(_vertices,_edges,_faces); // A very high roof to avoid infinite loop + SeElement *e, *ei; + + i = 0; + e = ei = _first->vtx(); + do { if ( e->se()->vtx()!=e || i++>max ) return ChkVtxError; + e = e->nxt(); + } while ( e!=ei ); + + i = 0; + e = ei = _first->edg(); + do { if ( e->se()->edg()!=e || i++>max ) return ChkEdgError; + e = e->nxt(); + } while ( e!=ei ); + + i = 0; + e = ei = _first->fac(); + do { if ( e->se()->fac()!=e || i++>max ) return ChkFacError; + e = e->nxt(); + } while ( e!=ei ); + + e = ei = _first->edg(); + do { m = check ( e->se(), max ); + if ( m!=ChkOk ) return m; + m = check ( e->se()->sym(), max ); + if ( m!=ChkOk ) return m; + e = e->nxt(); + } while ( e!=ei ); + return ChkOk; + } + +//======================================== SeMeshBase Operators ======================================== + +void SeMeshBase::destroy () + { + if (!_first) return; + SR_TRACE1("destroy..."); + + int i; + SeElement *ini, *cur, *curn; + + // Save all symedges in a list: + SrArray S(0,_edges*2); + cur = ini = _first->edg(); + do { S.push() = cur->se(); + S.push() = cur->se()->sym(); + cur = cur->nxt(); + } while ( cur!=ini ); + + // delete element lists : + SrClassManagerBase* man; + for ( i=0; i<3; i++ ) // for each element type + { if ( i==0 ) + { ini=_first->vtx(); man=_vtxman; } + else if ( i==1 ) + { ini=_first->edg(); man=_edgman; } + else + { ini=_first->fac(); man=_facman; } + + cur=ini; + do { curn = cur->nxt(); // for each element + man->free ( cur ); + cur = curn; + } while ( cur!=ini ); + } + + // delete all symedges: + for ( i=0; ivtx()->_insert(ne1->vtx()); + _new_edg(ne1); ne2->_edge=ne1->edg(); + _new_fac(ne1); ne2->_face=ne1->fac(); + + _first = ne2; // _first must be set only here! + + CHECKALL; + SR_TRACE1("Ok."); + return ne2; + } + +SeBase *SeMeshBase::mev ( SeBase *s ) + { + SR_TRACE1("mev..."); + + SeBase *ne1, *ne2; + EDG_CREATE ( ne1, ne2 ); + + ne1->_vertex = s->vtx(); + _new_vtx(ne2); ne1->_vertex=s->vtx(); + _new_edg(ne2); ne1->_edge=ne2->edg(); + ne1->_face = ne2->_face = s->fac(); + + DEF_SPLICE_VARS; + SPLICE ( s->pri(), ne2 ); + + _op_last_msg = OpNoErrors; + + CHECKALL; + SR_TRACE1("Ok."); + + return ne2; + } + +SeBase *SeMeshBase::mev ( SeBase *s1, SeBase *s2 ) + { + SR_TRACE1("mev2..."); + + if ( _op_check_parms ) + { if ( s1==s2 ) return _op_error(OpMevParmsEqual); + if ( s1->vtx()!=s2->vtx() ) return _op_error(OpMevNotSameVtx); + _op_last_msg = OpNoErrors; + } + + SeBase *ne1, *ne2; + EDG_CREATE ( ne1, ne2 ); + + _new_vtx(ne1); ne2->_vertex=s2->vtx(); + _new_edg(ne1); ne2->_edge = ne1->edg(); + ne1->_face = s1->sym()->fac(); + ne2->_face = s2->sym()->fac(); + s2->_vertex->_symedge = s2; + + DEF_SPLICE_VARS; + SPLICE ( s1->sym(), ne2 ); + SPLICE ( s2->sym(), ne1 ); + SPLICE ( ne1, ne2 ); + + s2=s1; + while ( s1!=ne1 ) + { s1->_vertex = ne1->vtx(); + s1=s1->rot(); + if(s1==s2) { printf("MEV2 INFINITE LOOP FOUND!\n"); break; } + // Prevents infinite loop by incoherent args + } + + CHECKALL; + + SR_TRACE1("Ok."); + + return ne1; + } + +SeBase *SeMeshBase::mef ( SeBase *s1, SeBase *s2 ) + { + SR_TRACE1("mef..."); + + if ( _op_check_parms ) + { if ( s1->fac()!=s2->fac() ) return _op_error (OpMefNotSameFace); + _op_last_msg = OpNoErrors; + } + + SeBase *ne1, *ne2; + EDG_CREATE ( ne1, ne2 ); + + ne1->_vertex = s2->vtx(); + ne2->_vertex = s1->vtx(); + _new_edg(ne1); ne2->_edge=ne1->edg(); + _new_fac(ne2); + ne1->_face=s1->fac(); s1->_face->_symedge=ne1; + + DEF_SPLICE_VARS; + SPLICE ( s1->pri(), ne1 ); + SPLICE ( s2->pri(), ne2 ); + + // s2 = ne2->nxt(); + while ( s2!=ne2 ) { s2->_face=ne2->fac(); s2=s2->nxt(); } + + CHECKALL; + SR_TRACE1("Ok."); + + return ne2; + } + +SeBase *SeMeshBase::kev ( SeBase *x ) + { + SR_TRACE1("kev..."); + + if ( _op_check_parms ) + { if ( _edges==1 ) return _op_error(OpKevLastEdge); + if ( x->rot()!=x ) return _op_error(OpKevManyEdges); + _op_last_msg = OpNoErrors; + } + + SeBase *xn = x->nxt(); + xn->_vertex->_symedge = xn; + xn->_face->_symedge = xn; + + _del_vtx ( x->vtx() ); + _del_edg ( x->edg() ); + + SeBase *xsp = x->sym()->pri(); + DEF_SPLICE_VARS; + SPLICE ( xsp, x ); + delete x->nxt(); delete x; + + CHECKALL; + SR_TRACE1("Ok."); + + return xsp->nxt(); + } + +SeBase *SeMeshBase::kev ( SeBase *x, SeBase **s ) + { + SR_TRACE1("kev2..."); + + SeBase *xs = x->sym(); + SeBase *xp = x->pri(); + SeBase *xsp = xs->pri(); + + if ( _op_check_parms ) + { if ( _edges==1 ) return _op_error(OpKevLastEdge); + if ( x->rot()==x ) return _op_error(OpKevOneEdge); + _op_last_msg = OpNoErrors; + } + + x->nxt()->vtx()->_symedge = x->nxt(); + x->fac()->_symedge = xp; + x->sym()->fac()->_symedge = xsp; + + _del_vtx ( x->vtx() ); + _del_edg ( x->edg() ); + + DEF_SPLICE_VARS; + SPLICE ( x, xs ); + SPLICE ( xsp, x ); + SPLICE ( xp, xs ); + + delete x->nxt(); delete x; + + x=xp=xp->sym(); xs=xsp->sym(); + + while ( x!=xs ) + { x->_vertex=xs->vtx(); + x=x->rot(); + if(x==xp) break; // Prevents infinite loop by incoherent args + } + *s=xs; + + CHECKALL; + SR_TRACE1("Ok."); + + return xp; + } + +SeBase *SeMeshBase::kef ( SeBase *x, SeBase **s ) + { + SR_TRACE1("kef..."); + + SeBase *xs = x->sym(); + SeBase *xp = x->pri(); + SeBase *xsp = xs->pri(); + + if ( _op_check_parms ) + { if ( x->fac()==xs->fac() ) return _op_error(OpKefSameFace); + _op_last_msg = OpNoErrors; + } + + x->fac()->_symedge = xp; + xs->fac()->_symedge = xsp; + x->vtx()->_symedge = xs->nxt(); + xs->vtx()->_symedge = x->nxt(); + + _del_edg ( x->edg() ); + _del_fac ( x->fac() ); + + DEF_SPLICE_VARS; + SPLICE ( xsp, x ); + SPLICE ( xp, xs ); + delete x->nxt(); delete x; + + xp=xp->nxt(); x=xs=xsp->nxt(); + + while ( x!=xp ) { x->_face=xp->fac(); x=x->nxt(); } + if (s) *s=xs; + + CHECKALL; + SR_TRACE1("Ok."); + + return xp; + } + +SeBase *SeMeshBase::mg ( SeBase *s1, SeBase *s2 ) // elems of s2 will be deleted + { + SR_TRACE1("mg..."); + + if ( _op_check_parms ) + { if ( vertices_in_face(s1)!=vertices_in_face(s2) ) return _op_error(OpMgUncompatibleFaces); + if ( s1->fac()==s2->fac() ) return _op_error(OpMgSameFace); + _op_last_msg = OpNoErrors; + } + + _del_fac ( s1->fac() ); + _del_fac ( s2->fac() ); + + SrArray contour(0,256); + SeBase *x1, *x2, *s; + + // Not fully tested: it seems that some fixes will be required like vtx->_symedge=vtx, etc... + + x1=s1; x2=s2; + do { _del_edg ( x2->edg() ); + _del_vtx ( x2->vtx() ); + x2->sym()->_edge = x1->edg(); + contour.push()=x1; contour.push()=x2; + contour.push()=x1->sym(); contour.push()=x2->sym(); + x1 = x1->nxt(); + s=x2; do { s->_vertex=x1->vtx(); s=s->rot(); } while (s!=x2); + x2 = x2->pri(); + } while ( x1!=s1 ); + + for ( int i=0; i_next->_rotate=contour[i+3]; + contour[i+3]->_next->_rotate=contour[i+2]; + delete contour[i]; + delete contour[i+1]; + } + + CHECKALL; + return s; + } + +SeBase *SeMeshBase::flip ( SeBase *x ) + { + SR_TRACE1("flip..."); + + SeBase *xs = x->sym(); + SeBase *xp = x->pri(); + SeBase *xsp = xs->pri(); + + if ( _op_check_parms ) + { if ( x->fac()==xs->fac() ) return _op_error(OpFlipOneFace); + _op_last_msg = OpNoErrors; + } + + DEF_SPLICE_VARS; + SPLICE ( xsp, x ); + SPLICE ( xp, xs ); + + xp = xp->nxt(); + xsp = xsp->nxt(); + + x->_vertex = xp->nxt()->vtx(); + xs->_vertex = xsp->nxt()->vtx(); + xsp->_face = xs->fac(); + xp->_face = x->fac(); + + xp->vtx()->_symedge = xp; + xsp->vtx()->_symedge = xsp; + xsp->fac()->_symedge = xsp; + xp->fac()->_symedge = xp; + + SPLICE ( xp, xs ); + SPLICE ( xsp, x ); + + CHECKALL; + SR_TRACE1("Ok."); + + return xs; + } + +SeBase *SeMeshBase::ecol ( SeBase *x, SeBase **s ) + { + SR_TRACE1("ecol..."); + + SeBase *s1, *s2, *z; + + if ( _op_check_parms ) + { if ( x->fac()==x->sym()->fac() ) return _op_error(OpEdgColOneFace); + _op_last_msg = OpNoErrors; + } + + s1 = kev ( x, &s2 ); + s1 = kef ( s1, &z ); + s2 = kef ( s2->sym()->nxt(), &z ); + + *s = s2->rot(); + + return s1; + } + +SeBase *SeMeshBase::vsplit ( SeBase *s1, SeBase *s2 ) + { + SR_TRACE1("vtxsplit using mev..."); + + if ( _op_check_parms ) + { if ( s1==s2 ) return _op_error(OpVtxSplitParmsEqual); + if ( s1->vtx()!=s2->vtx() ) return _op_error(OpVtxSplitNotSameVtx); + _op_last_msg = OpNoErrors; + } + + SeBase* s2s = s2->sym(); + mef ( s2s->nxt(), s2s ); + s1 = mef ( s1, s1->nxt() ); + + return mev ( s1, s2 ); + } + +SeBase* SeMeshBase::addv ( SeBase* x ) + { + SR_TRACE1("addv..."); + + if ( _op_check_parms ) + { _op_last_msg = OpNoErrors; + } + + SeBase *y = mev ( x ); + + x=x->nxt(); + do { y = mef ( y, x ); + x=x->nxt(); + } while ( x->nxt()!=y ); + + return y; + } + +SeBase* SeMeshBase::delv ( SeBase* y ) + { + SR_TRACE1("delv..."); + + if ( _op_check_parms ) + { _op_last_msg = OpNoErrors; + } + + SeBase *s2; + + while ( y!=y->rot() ) y = kef ( y, &s2 ); + + return kev ( y ); + } + +//============================ End of File =============================== diff --git a/source/dcdt/se/se_triangulator.cpp b/source/dcdt/se/se_triangulator.cpp new file mode 100644 index 0000000000..c5f4ebc2d1 --- /dev/null +++ b/source/dcdt/se/se_triangulator.cpp @@ -0,0 +1,1446 @@ +#include "precompiled.h" + +# include +# include + +# include "sr_geo2.h" +# include "sr_heap.h" +# include "sr_deque.h" + +# include "se_triangulator.h" + +//# define SR_TRACE_NO_FILENAME + +//# define SR_USE_TRACE1 // locate point +//# define SR_USE_TRACE2 // ins delaunay point +//# define SR_USE_TRACE3 // ins line constraint +//# define SR_USE_TRACE4 // triangulate face +//# define SR_USE_TRACE5 // search path +//# define SR_USE_TRACE6 // remove vertex +//# define SR_USE_TRACE7 // shortest path (funnel) +# include "sr_trace.h" + +//=========================== debugging tools ==================================== + +# define PNL printf("\n") + +# define PRINTP(x,y) printf("(%+9.7f,%+9.7f) ", x, y) + +# define PRINTV(v) { double x, y; \ + _man->get_vertex_coordinates ( v, x, y ); \ + PRINTP(x,y); } + +//================================================================================ +//========================= SeTriangulatorManager ================================ +//================================================================================ + +void SeTriangulatorManager::triangulate_face_created_edge ( SeEdge* /*e*/ ) + { + } + +void SeTriangulatorManager::new_intersection_vertex_created ( SeVertex* /*v*/ ) + { + } + +void SeTriangulatorManager::new_steiner_vertex_created ( SeVertex* /*v*/ ) + { + } + +void SeTriangulatorManager::vertex_found_in_constrained_edge ( SeVertex* /*v*/ ) + { + } + +bool SeTriangulatorManager::is_constrained ( SeEdge* /*e*/ ) + { + return false; + } + +void SeTriangulatorManager::set_unconstrained ( SeEdge* /*e*/ ) + { + } + +void SeTriangulatorManager::get_constraints ( SeEdge* /*e*/, SrArray& /*ids*/ ) + { + } + +void SeTriangulatorManager::add_constraints ( SeEdge* /*e*/, const SrArray& /*ids*/ ) + { + } + +bool SeTriangulatorManager::ccw ( SeVertex* v1, SeVertex* v2, SeVertex* v3 ) + { + double x1, y1, x2, y2, x3, y3; + get_vertex_coordinates ( v1, x1, y1 ); + get_vertex_coordinates ( v2, x2, y2 ); + get_vertex_coordinates ( v3, x3, y3 ); + return sr_ccw ( x1, y1, x2, y2, x3, y3 )>0? true : false; + } + +bool SeTriangulatorManager::in_triangle ( SeVertex* v1, SeVertex* v2, SeVertex* v3, SeVertex* v ) + { + double x1, y1, x2, y2, x3, y3, x, y; + get_vertex_coordinates ( v1, x1, y1 ); + get_vertex_coordinates ( v2, x2, y2 ); + get_vertex_coordinates ( v3, x3, y3 ); + get_vertex_coordinates ( v, x, y ); + return sr_in_triangle ( x1, y1, x2, y2, x3, y3, x, y ); + } + +bool SeTriangulatorManager::in_triangle ( SeVertex* v1, SeVertex* v2, SeVertex* v3, + double x, double y ) + { + double x1, y1, x2, y2, x3, y3; + get_vertex_coordinates ( v1, x1, y1 ); + get_vertex_coordinates ( v2, x2, y2 ); + get_vertex_coordinates ( v3, x3, y3 ); + return sr_in_triangle ( x1, y1, x2, y2, x3, y3, x, y ); + } + +bool SeTriangulatorManager::in_segment ( SeVertex* v1, SeVertex* v2, SeVertex* v, double eps ) + { + double x1, y1, x2, y2, x, y; + get_vertex_coordinates ( v1, x1, y1 ); + get_vertex_coordinates ( v2, x2, y2 ); + get_vertex_coordinates ( v, x, y ); + return sr_in_segment ( x1, y1, x2, y2, x, y, eps ); + } + +bool SeTriangulatorManager::is_delaunay ( SeEdge* e ) + { + SeBase* s1 = e->se(); + SeBase* s2 = s1->sym(); + SeBase* s3 = s2->nxt()->nxt(); + SeBase* s4 = s1->nxt()->nxt(); + + double x1, y1, x2, y2, x3, y3, x4, y4; + get_vertex_coordinates ( s1->vtx(), x1, y1 ); + get_vertex_coordinates ( s2->vtx(), x2, y2 ); + get_vertex_coordinates ( s3->vtx(), x3, y3 ); + get_vertex_coordinates ( s4->vtx(), x4, y4 ); + + return sr_in_circle ( x1, y1, x2, y2, x4, y4, x3, y3 )? false:true; + } + +bool SeTriangulatorManager::is_flippable_and_not_delaunay ( SeEdge* e ) + { + SeBase* s1 = e->se(); + SeBase* s2 = s1->sym(); + SeBase* s3 = s2->nxt()->nxt(); + SeBase* s4 = s1->nxt()->nxt(); + + if ( s3->nxt()!=s2 ) return false; // not a triangle ! + + double x1, y1, x2, y2, x3, y3, x4, y4; + get_vertex_coordinates ( s1->vtx(), x1, y1 ); + get_vertex_coordinates ( s2->vtx(), x2, y2 ); + get_vertex_coordinates ( s3->vtx(), x3, y3 ); + get_vertex_coordinates ( s4->vtx(), x4, y4 ); + + if ( sr_ccw(x3,y3,x2,y2,x4,y4)<=0 || sr_ccw(x4,y4,x1,y1,x3,y3)<=0 ) return false; + + return sr_in_circle ( x1, y1, x2, y2, x4, y4, x3, y3 ); + } + +// attention: s must be well positioned +int SeTriangulatorManager::test_boundary ( SeVertex* v1, SeVertex* v2, SeVertex* v3, + double x, double y, double eps, SeBase*& s ) + { + double x1, y1, x2, y2, x3, y3; + get_vertex_coordinates ( v1, x1, y1 ); + get_vertex_coordinates ( v2, x2, y2 ); + get_vertex_coordinates ( v3, x3, y3 ); + + if ( sr_next(x1,y1,x,y,eps) ) { return SeTriangulator::VertexFound; } + if ( sr_next(x2,y2,x,y,eps) ) { s=s->nxt(); return SeTriangulator::VertexFound; } + if ( sr_next(x3,y3,x,y,eps) ) { s=s->nxt()->nxt(); return SeTriangulator::VertexFound; } + if ( sr_in_segment(x1,y1,x2,y2,x,y,eps) ) { return SeTriangulator::EdgeFound; } + if ( sr_in_segment(x2,y2,x3,y3,x,y,eps) ) { s=s->nxt(); return SeTriangulator::EdgeFound; } + if ( sr_in_segment(x3,y3,x1,y1,x,y,eps) ) { s=s->nxt()->nxt(); return SeTriangulator::EdgeFound; } + return SeTriangulator::NotFound; + } + +bool SeTriangulatorManager::segments_intersect ( SeVertex* v1, SeVertex* v2, + SeVertex* v3, SeVertex* v4, + double& x, double& y ) + { + double x1, y1, x2, y2, x3, y3, x4, y4; + get_vertex_coordinates ( v1, x1, y1 ); + get_vertex_coordinates ( v2, x2, y2 ); + get_vertex_coordinates ( v3, x3, y3 ); + get_vertex_coordinates ( v4, x4, y4 ); + return sr_segments_intersect ( x1, y1, x2, y2, x3, y3, x4, y4, x, y ); + } + +bool SeTriangulatorManager::segments_intersect ( SeVertex* v1, SeVertex* v2, + SeVertex* v3, SeVertex* v4 ) + { + double x1, y1, x2, y2, x3, y3, x4, y4; + get_vertex_coordinates ( v1, x1, y1 ); + get_vertex_coordinates ( v2, x2, y2 ); + get_vertex_coordinates ( v3, x3, y3 ); + get_vertex_coordinates ( v4, x4, y4 ); + return sr_segments_intersect ( x1, y1, x2, y2, x3, y3, x4, y4 ); + } + +bool SeTriangulatorManager::segments_intersect ( double x1, double y1, double x2, double y2, + SeVertex* v3, SeVertex* v4 ) + { + double x3, y3, x4, y4; + get_vertex_coordinates ( v3, x3, y3 ); + get_vertex_coordinates ( v4, x4, y4 ); + return sr_segments_intersect ( x1, y1, x2, y2, x3, y3, x4, y4 ); + } + +void SeTriangulatorManager::segment_midpoint ( SeVertex* v1, SeVertex* v2, double& x, double& y ) + { + double x1, y1, x2, y2; + get_vertex_coordinates ( v1, x1, y1 ); + get_vertex_coordinates ( v2, x2, y2 ); + x = (x1+x2)/2.0; + y = (y1+y2)/2.0; + } + +//================================================================================ +//=============================== PathTree ======================================= +//================================================================================ + +class SeTriangulator::PathNode // a node can be seen as one edge + { public : + int parent; // index of the parent node, -1 if root + SeBase* s; // the arrival edge of the node + double ncost; // accumulated cost until touching s->edg() + double hcost; // heuristic cost from s-edg() + public : + void init ( int pnode, SeBase* q, double nc, double hc ) + { parent=pnode; s=q; ncost=nc; hcost=hc; } + }; + +class SeTriangulator::PathTree + { public : + SrArray nodes; + SrHeap leafs; + public : + PathTree () + { nodes.capacity(64); leafs.capacity(64); } + + void init () + { nodes.size(0); leafs.init(); } + + void add_child ( int pleaf, SeBase* s, double ncost, double hcost ) + { nodes.push().init ( pleaf, s, ncost, hcost ); + int ni = nodes.size()-1; + leafs.insert ( ni, cost(ni) ); + } + + double cost ( int i ) + { return nodes[i].ncost + nodes[i].hcost; } + + int lowest_cost_leaf () + { + if ( leafs.empty() ) return -1; + int i = leafs.top(); + leafs.remove(); + return i; + } + + void print () + { int i; + sr_out << "NParents: "; + for ( i=0; i + { public : + }; + +//================================================================================ +//============================ SeTriangulator ==================================== +//================================================================================ + +SeTriangulator::SeTriangulator ( Mode mode, SeMeshBase* m, + SeTriangulatorManager* man, double epsilon ) + { + _mode = mode; + _mesh = m; + _man = man; + _epsilon = epsilon; + _ptree = 0; + _path_found = false; + _fdeque = 0; + } + +SeTriangulator::~SeTriangulator () + { + _man->unref(); + delete _ptree; + delete _fdeque; + } + +//================================================================================ +//============================= init triangulation =============================== +//================================================================================ + +SeBase* SeTriangulator::init_as_triangulated_square + ( double x1, double y1, double x2, double y2, + double x3, double y3, double x4, double y4 ) + { + SeBase* s = _mesh->init(); + _man->set_vertex_coordinates ( s->vtx(), x3, y3 ); + _man->set_vertex_coordinates ( s->nxt()->vtx(), x4, y4 ); + s = _mesh->mev ( s ); + _man->set_vertex_coordinates ( s->vtx(), x2, y2 ); + s = _mesh->mev ( s ); + _man->set_vertex_coordinates ( s->vtx(), x1, y1 ); + _mesh->mef ( s, s->nxt()->nxt()->nxt() ); // close the square + _mesh->mef ( s, s->nxt()->nxt() ); // triangulate square + + return s; + } + +//================================================================================ +//============================= face triangulation =============================== +//================================================================================ + +bool SeTriangulator::triangulate_face ( SeFace* f, bool optimize ) + { + SeBase *s, *x, *xn, *xp, *v, *vp, *vn; + SrArray& stack = _buffer; + bool ok; + + x = s = f->se(); + + SR_TRACE4 ( "starting triangulate..." ); + + stack.size(0); + if (optimize) + { _mesh->begin_marking (); + do { _mesh->mark(x->edg()); x=x->nxt(); } while ( x!=s ); + } + + SR_TRACE4 ( "entering loop..." ); + + while ( !_mesh->is_triangle(x) ) + { ok = true; + xn=x->nxt(); xp=x->pri(); v=xn->nxt(); + + if ( _man->ccw(xp->vtx(),x->vtx(),xn->vtx()) ) + { do { if ( _man->in_triangle(xp->vtx(),x->vtx(),xn->vtx(),v->vtx()) ) // boundary is considered inside + { ok=false; break; } + v = v->nxt(); + } while ( v!=xp ); + } + else ok=false; + + if (ok) + { s = _mesh->mef ( xp, xn ); + _man->triangulate_face_created_edge ( s->edg() ); + if (optimize) stack.push()=s; + s=xn; + } + x=xn; + if ( !ok && x==s ) + { // Loop occured: this already happened because of dup points, and could + // also occur because of self-intersecting or non CCW faces + if (optimize) _mesh->end_marking (); + sr_out.warning("Loop occured in SeTriangulator::triangulate_face!\n"); + // do { PRINTV(x->vtx()); PNL; x=x->nxt(); } while ( x!=s ); while(1); + return false; + } + } + + SR_TRACE4 ( "optimizing..."<0 ) + { SR_TRACE4 ( "optimizing... "<is_flippable_and_not_delaunay(x->edg()) ) // Flip will optimize triangulation + { xn=x->nxt(); xp=xn->nxt(); vn=x->sym()->nxt(); vp=vn->nxt(); + if ( !_mesh->marked(vp->edg()) ) stack.push(vp); + if ( !_mesh->marked(vn->edg()) ) stack.push(vn); + if ( !_mesh->marked(xp->edg()) ) stack.push(xp); + if ( !_mesh->marked(xn->edg()) ) stack.push(xn); + _mesh->flip ( x ); + } + } + + if (optimize) _mesh->end_marking (); + + SR_TRACE4 ( "ok!" ); + + return true; + } + +//================================================================================ +//============================= Locate Point=== ================================== +//================================================================================ + +// Still this one seems to be the most tricky algorithm, the problem is that any +// small perturbations given from the other methods will make location to crash. +// typical example is when a triangle becomes non-ccw. Have to re-checkk all other +// methods to avoid this to happen. +// Note: the "random walk" is generally worse than this mark/linear solution +SeTriangulator::LocateResult SeTriangulator::locate_point + ( const SeFace* iniface, + double x, double y, + SeBase*& result ) + { + SeVertex *v1, *v2, *v3; + SeBase *sn, *snn; + SeBase *s = iniface->se(); + SrArray& stack = _buffer; + int sres = NotFound; + int triangles = _mesh->faces(); + int visited_count=0; + int ccws; +// bool epsilon_mode = false; + bool walk_failed = false; + double x1, y1, x2, y2, x3, y3; + + SR_TRACE1 ( "locate_point..." ); + + if ( !_mesh->is_triangle(s) ) + { sr_out.warning ("NON-TRIANGLE CASE FOUND IN SEARCH_TRIANGLE!\n"); + return NotFound; + } + + _mesh->begin_marking (); + _mesh->mark ( s->fac() ); + + while ( true ) + { + sn=s->nxt(); snn=sn->nxt(); + v1=s->vtx(); v2=sn->vtx(); v3=snn->vtx(); + +/* if ( epsilon_mode ) + { sres = _man->test_boundary ( v1, v2, v3, x, y, _epsilon, s ); + if ( sres!=NotFound ) + { SR_TRACE1 ("LOOP SOLVED IN EPSILON MODE!"); + break; + } + }*/ + + stack.size(0); + ccws=0; + _man->get_vertex_coordinates ( v1, x1, y1 ); + _man->get_vertex_coordinates ( v2, x2, y2 ); + _man->get_vertex_coordinates ( v3, x3, y3 ); + + if ( sr_ccw(x,y,x2,y2,x1,y1)>0 ) { ccws++; if(_mesh->is_triangle(s->sym())) stack.push()=s->sym(); } + if ( sr_ccw(x,y,x3,y3,x2,y2)>0 ) { ccws++; if(_mesh->is_triangle(sn->sym())) stack.push()=sn->sym(); } + if ( sr_ccw(x,y,x1,y1,x3,y3)>0 ) { ccws++; if(_mesh->is_triangle(snn->sym())) stack.push()=snn->sym(); } + + if ( ccws!=stack.size() ) + { sr_out.warning ("BORDER OR NON TRIANGULAR/CCW FACE ENCOUNTERED IN SEARCH_TRIANGLE!\n"); + PRINTV(v1);PRINTV(v2);PRINTV(v3); sr_out<marked(stack[0]->fac())? stack[1]:stack[0]; + } + else + { SR_TRACE1 ("DEGENERATED CASE FOUND IN SEARCH_TRIANGLE!"); // already happen + walk_failed = true; + break; + } + + if ( _mesh->marked(s->fac()) ) + { SR_TRACE1 ("SEARCH_TRIANGLE: Already Marked Found and test mode on!"); + //_mesh->end_marking (); + //_mesh->begin_marking (); // increment the marking flag + //epsilon_mode=true; + walk_failed = true; + break; + } + +/* if ( epsilon_mode && _mesh->marked(s->fac()) ) + { SR_TRACE1 ("LOOP DETECTED IN EPSILON_MODE!"); // Not detected yet + walk_failed = true; + break; + }*/ + if ( visited_count++>triangles ) + { SR_TRACE1 ("WALKED MORE THAN NUMBER OF TRIANGLES!\n"); + walk_failed = true; + break; + } + + _mesh->mark ( s->fac() ); + } + + SR_TRACE1 ( "Triangles Visited: " << visited_count ); + + if ( walk_failed ) // do linear search... + { SeFace *f, *fi; + f = fi = s->fac(); + sr_out <<"PERFORMING LINEAR SEARCH... \n"; + do { s=f->se(); sn=s->nxt(); snn=sn->nxt(); + v1=s->vtx(); v2=sn->vtx(); v3=snn->vtx(); + if ( snn->nxt()==s ) // is triangle + { if ( _man->in_triangle(v1,v2,v3,x,y) ) + { result=s; + sres = TriangleFound; + break; + } + } + f = f->nxt(); + } while ( f!=fi ); + if ( sres==NotFound ) // try now with test boundary... + { sr_out <<"PERFORMING 2ND LINEAR SEARCH... \n"; + f = fi; + do { s=f->se(); sn=s->nxt(); snn=sn->nxt(); + v1=s->vtx(); v2=sn->vtx(); v3=snn->vtx(); + if ( snn->nxt()==s ) // is triangle + { sres = _man->test_boundary ( v1, v2, v3, x, y, _epsilon, s ); + if ( sres!=NotFound ) + { result = s; + break; + } + } + f = f->nxt(); + } while ( f!=fi ); + } + /*if ( sres==NotFound ) // now just get the closest vertex... + { sr_out <<"PERFORMING 3RD LINEAR SEARCH... \n"; + v1 = v2 = v3 = f->se()->vtx(); + double dist2, min_dist2=-1; + do { _man->get_vertex_coordinates ( v1, x1, y1 ); + dist2 = x*x1+y*y1; + if ( dist2nxt(); + } while ( v1!=v2 ); + sres = VertexFound; + s = v3->se(); + }*/ + if ( sres==NotFound ) sr_out.warning ("LINEAR SEARCH NOT FOUND!\n"); + } + + if ( sres==TriangleFound ) // perform extra tests + { sres = _man->test_boundary ( v1, v2, v3, x, y, _epsilon, s ); + if ( sres==NotFound ) sres=TriangleFound; + } + + SR_TRACE1 ( "Search Result : " << (sres==NotFound?"NotFound" : sres==VertexFound?"VertexFound" : sres==EdgeFound?"EdgeFound":"TriangleFound") ); + + _mesh->end_marking (); + result = s; + return (LocateResult)sres; + } + +//================================================================================ +//============================== Insert Vertex =================================== +//================================================================================ + +SeVertex* SeTriangulator::insert_point_in_face ( SeFace* f, double x, double y ) + { + SR_TRACE2 ( "insert_point_in_face..." ); + + SrArray& stack = _buffer; + SeBase *s, *t; + + // even if the point is in an edge the degenerated triangle will be + // soon flipped by the circle test (this was the previous used solution) + stack.size(3); + s = f->se(); + t = _mesh->mev(s); + SeVertex* v = t->vtx(); // this is the return vertex + stack[0]=s; + _man->set_vertex_coordinates ( v, x, y ); + + s = s->nxt(); + t = _mesh->mef ( t, s ); //s->nxt()->nxt()->nxt() ); + stack[1]=s; + + s = s->nxt(); + _mesh->mef ( t, s ); + stack[2]=s; + + _propagate_delaunay (); + + return v; + } + +SeVertex* SeTriangulator::insert_point_in_edge ( SeEdge* e, double x, double y ) + { + SR_TRACE2 ( "insert_point_in_edge..." ); + + //return insert_point_in_face ( e, x, y ); + + SeBase *s = e->se(); + + /* We must project into the edge to ensure correctness, otherwise we may encounter + cases where the polygon of the neighboors of a vertex v does not contain v */ + double x1, y1, x2, y2, qx, qy ; + _man->get_vertex_coordinates ( s->vtx(), x1, y1 ); + _man->get_vertex_coordinates ( s->nxt()->vtx(), x2, y2 ); + if ( sr_segment_projection ( x1, y1, x2, y2, x, y, qx, qy, _epsilon ) ) + { x=qx; y=qy; + } + else + { if ( sqrt(sr_dist2(x,y,x1,y1))vtx(); else return s->nxt()->vtx(); + } + + SrArray& stack = _buffer; + SeBase *t; + stack.size(4); + + t = _mesh->mev ( s, s->rot() ); + _man->set_vertex_coordinates ( t->vtx(), x, y ); + + stack[0]=s->nxt(); + stack[1]=s->nxt()->nxt(); + stack[2]=t->nxt(); + stack[3]=t->nxt()->nxt(); + + _mesh->mef ( s, stack[1] ); + _mesh->mef ( t, stack[3] ); + + // in principle we do not need to fix this in the constrained case, where + // intersections are required to be detected before insertion + if ( _mode==ModeConstrained || _mode==ModeConforming ) + { if ( _man->is_constrained(e) ) + { SR_TRACE2 ( "Point in edge in constrained case..." ); + SrArray ids; + _man->get_constraints ( e, ids ); + _man->add_constraints ( t->edg(), ids ); // constrain the other subdivided part + _man->vertex_found_in_constrained_edge ( s->vtx() ); + } + } + + _propagate_delaunay (); + + return s->vtx(); + } + +void SeTriangulator::_propagate_delaunay () + { + SR_TRACE2 ( "propagate_delaunay..." ); + + SeBase *s, *x, *t; + + if ( _mode==ModeConforming ) // ther might be recursion, so all arrays are local + { SrArray constraints; + SrArray*> constraints_ids; + SrArray& stack = _buffer; + + constraints.capacity(32); + constraints_ids.capacity(32); + + SR_TRACE2 ( "optimizing in conforming mode..."<0 ) + { x = stack.pop(); + if ( !_man->is_delaunay(x->edg()) ) // Flip will optimize triang + { s=x->sym()->pri(); t=s->nxt()->nxt(); + if ( _man->is_constrained(x->edg()) ) + { SR_TRACE2 ( "Prepare to flip constrained edge..." ); + constraints_ids.push() = new SrArray; + _man->get_constraints ( x->edg(), *constraints_ids.top() ); + constraints.push() = x->vtx(); + constraints.push() = x->nxt()->vtx(); + _man->set_unconstrained ( x->edg() ); + } + _mesh->flip(x); stack.push(s); stack.push(t); + } + } + + while ( constraints.size()>0 ) + { SR_TRACE2 ( "Fixing Constraints..." ); + _conform_line ( constraints.pop(), constraints.pop(), *constraints_ids.top() ); + delete constraints_ids.pop(); + } + } + else if ( _mode==ModeConstrained ) // (no recursion) + { SrArray& stack = _buffer; + + SR_TRACE2 ( "optimizing in constrained mode..."<0 ) + { x = stack.pop(); + if ( !_man->is_delaunay(x->edg()) ) // Flip will optimize triang + { s=x->sym()->pri(); t=s->nxt()->nxt(); + if ( !_man->is_constrained(x->edg()) ) + { _mesh->flip(x); stack.push(s); stack.push(t); } + } + } + } + else // ModeUnconstrained (no recursion) + { SrArray& stack = _buffer; + + SR_TRACE2 ( "optimizing in unconstrained mode..."<0 ) + { x = stack.pop(); + // remember that in the delaunay case edges are always flippable; + // originating from a star polygon + if ( !_man->is_delaunay(x->edg()) ) // Flip will optimize triang + { s=x->sym()->pri(); t=s->nxt()->nxt(); + _mesh->flip(x); stack.push(s); stack.push(t); + } + } + } + SR_TRACE2 ( "Ok." ); + } + +SeVertex* SeTriangulator::insert_point ( double x, double y, const SeFace* inifac ) + { + SeBase* s; + LocateResult res; + + res = locate_point ( inifac? inifac:_mesh->first()->fac(), x, y, s ); + + if ( res==NotFound ) + { return 0; + } + else if ( res==SeTriangulator::VertexFound ) + { return s->vtx(); + } + else if ( res==SeTriangulator::EdgeFound ) + { return insert_point_in_edge ( s->edg(), x, y ); + } + else // a face was found + { return insert_point_in_face ( s->fac(), x, y ); + } + } + +bool SeTriangulator::remove_vertex ( SeVertex* v ) + { + SR_TRACE2 ( "Remove Vertex." ); + SeBase *s = _mesh->delv ( v->se() ); + return triangulate_face ( s->fac() ); + } + +//================================================================================ +//========================= Insert Line Constraint =============================== +//================================================================================ + +bool SeTriangulator::insert_line_constraint ( SeVertex *v1, SeVertex *v2, int id ) + { + SrArray& ids = _ibuffer; + ids.size(1); + ids[0]=id; + if ( v1==v2 ) return true; + switch ( _mode ) + { case ModeUnconstrained: return false; + case ModeConforming: return _conform_line ( v1, v2, ids ); + case ModeConstrained: return _constrain_line ( v1, v2, ids ); + } + return false; + } + +bool SeTriangulator::insert_segment ( double x1, double y1, double x2, double y2, int id, const SeFace* inifac ) + { + SeVertex *v1, *v2; + + v1 = insert_point ( x1, y1, inifac ); + if ( !v1 ) return false; + v2 = insert_point ( x2, y2, inifac ); + if ( !v2 ) return false; + + return insert_line_constraint ( v1, v2, id ); + } + +// conform_line() is recursive and relies on many geometrical tests. +// It has been successfully tested with many examples. +// In one case I found that, when compiling with Visual C++, optimizations +// should be set to disabled or default. +// After that I fixed a bug in kef(), that could be the cause. +// However there are still problems when several constraints intersect +// and are almost parallel +bool SeTriangulator::_conform_line ( SeVertex *v1, SeVertex *v2, + const SrArray& constraints_ids ) + { + enum Case { LineExist, VertexInEdge, NeedToSubdivide }; + Case c; + SeEdge* interedge; + SeVertex *v=0; + SeBase *x, *s; + SrArray a(0,32); + double px, py; + + SR_TRACE3 ( "Starting conform_line..." ); + + a.push()=v1; a.push()=v2; + + while ( !a.empty() ) + { SR_TRACE3 ( "Stack size:"<se(); + do { if ( x->nxt()->vtx()==v2 ) // (v1,v2) is there. + { c=LineExist; break; } + x=x->rot(); + } while ( x!=s ); + + if ( c==NeedToSubdivide ) + do { if ( _man->in_segment(v1,v2,x->nxt()->vtx(),_epsilon) ) + { c=VertexInEdge; break; } + x=x->rot(); + } while ( x!=s ); + + if ( c==NeedToSubdivide ) + do { // will subdivide, so just check if we use an intersection point + if ( _man->is_constrained(x->nxt()->edg()) && + _man->segments_intersect + ( x->nxt()->vtx(), x->pri()->vtx(), v1, v2, px, py ) ) + { interedge = x->nxt()->edg(); break; } + + x=x->rot(); + } while ( x!=s ); + + //static int i=0; se_out<300 ) while(1); + + if ( c==NeedToSubdivide ) + { SR_TRACE3 ( "NeedToSubdivide"<<(const char*)(interedge? "(with intersection)":"") ); + SeTriangulator::LocateResult res; + if ( !interedge ) _man->segment_midpoint ( v1, v2, px, py ); + res = locate_point ( s->fac(), px, py, s ); + if ( res==SeTriangulator::NotFound ) + { return false; } + else if ( res==SeTriangulator::VertexFound ) + { v=s->vtx(); } // I've already found one case here + else if ( res==SeTriangulator::EdgeFound ) + { v = insert_point_in_edge ( s->edg(), px, py ); + if ( !v ) return false; + _man->new_steiner_vertex_created ( v ); + } + else + { v = insert_point_in_face ( s->fac(), px, py ); + if ( !v ) return false; + _man->new_steiner_vertex_created ( v ); // WAS v1 + } + a.push()=v1; a.push()=v; + a.push()=v; a.push()=v2; + } + else if ( c==VertexInEdge ) + { SR_TRACE3 ( "VertexInEdge"); + v1=x->nxt()->vtx(); + _man->vertex_found_in_constrained_edge ( v1 ); + a.push()=v1; a.push()=v2; + _man->add_constraints ( x->edg(), constraints_ids ); + } + else if ( c==LineExist ) + { SR_TRACE3 ( "LineExist"); + _man->add_constraints ( x->edg(), constraints_ids ); + } + } + + return true; + } + +// handle ok if v1-v2 is already there +// To check: restrict_line doesnt seem to need to receive an array of indices. +bool SeTriangulator::_constrain_line ( SeVertex* v1, SeVertex* v2, + const SrArray& constraints_ids ) + { + SrArray totrig = _buffer; + SrArray& ea = _elem_buffer; + SeBase *s, *e, *s1, *s2; + SeVertex* v; + int i, j, iv1, iv2; + double px, py; + + SR_TRACE3 ( "Starting restrict_line..." ); + SR_TRACE3 ( "Looking for traversed elements..." ); + + // First get lists of traversed elements (ea) + v = v1; + e = 0; + s = v1->se(); + ea.size(0); + ea.push().set(v,0); + + while ( v!=v2 ) + { if ( v ) _v_next_step ( s, v1, v2, v, e ); + else _e_next_step ( s, v1, v2, v, e ); + + //static int i=0; sr_out<300 ) while(1); + + if ( e && _man->is_constrained(e->edg()) ) // intersects with another constrained edge + { SrArray ids; + s = e; + _man->segments_intersect ( v1, v2, s->vtx(), s->nxt()->vtx(), px, py ); + _man->get_constraints ( s->edg(), ids ); + s = _mesh->mev ( s, s->rot() ); + e=0; v=s->vtx(); + _man->set_vertex_coordinates ( v, px, py ); + _man->new_intersection_vertex_created ( v ); + _man->add_constraints ( s->edg(), ids ); + _mesh->mef ( s, s->nxt()->nxt() ); s=s->sym(); + _mesh->mef ( s->nxt(), s->pri() ); + SR_TRACE3 ( "Vertex added in constraint intersection" ); + } + + if ( v ) + { ea.push().set(v,0); + s=v->se(); + SR_TRACE3 ( (v==v2?"Found vertex v2.":"Crossed existing vertex.") ); + if ( v!=v2 ) _man->vertex_found_in_constrained_edge(v); + v1=v; // advance v1 + } + else + { ea.push().set(0,e); + s=e->sym(); + SR_TRACE3 ( "Crossed edge." ); + } + } + + //for ( i=0; ise(); + while ( s->nxt()->vtx()!=ea[iv2].v ) s=s->rot(); + _man->add_constraints(s->edg(),constraints_ids); + } + else // kill inbetween edges and put new constraint there + { //sr_out<<(iv1+1)<<" .. "<<(iv2-1)<<"\n"; + SR_TRACE3 ( "Killing crossed edges "<<(iv1+1)<<" to "<<(iv2-1)<<" ..." ); + s1 = ea[iv1+1].e->pri(); + for ( j=iv1+1; jkef(ea[j].e,&s); + SR_TRACE3 ( "Adding edge constraint..." ); + s = _mesh->mef ( s1, s2->nxt() ); + _man->add_constraints(s->edg(),constraints_ids); + totrig.push() = s; + totrig.push() = s->sym(); + } + iv1=iv2; + } + } + + // Finally retriangulate open regions: + SR_TRACE3 ( "Retriangulating "<0 ) + { s2 = totrig.pop(); + s1 = s2->nxt()->nxt(); + if ( s1->nxt()==s2 ) + { SR_TRACE3 ( "Region "<nxt() ) + { if ( _can_connect(s2,s) ) + { if ( s==s1 ) + { SR_TRACE3 ( "MEF case 1" ); + totrig.push() = _mesh->mef ( s2, s ); + } + else if ( s->nxt()==s2 ) + { SR_TRACE3 ( "MEF case 2" ); + totrig.push() = _mesh->mef ( s, s2->nxt() ); + } + else + { SR_TRACE3 ( "MEF case 3" ); + totrig.push() = _mesh->mef ( s, s2->nxt() ); + totrig.push() = _mesh->mef ( s2, s ); + } + break; + } + } + } + + SR_TRACE3 ( "Done." ); + return true; + } + +void SeTriangulator::_v_next_step ( SeBase* s, SeVertex* v1, SeVertex* v2, + SeVertex*& v, SeBase*& e ) + { + e=0; + SeBase* x = s; + double x1, y1, x2, y2, x3, y3, x4, y4; + + // First look if final vertex is found: + do { if ( x->nxt()->vtx()==v2 ) { v=v2; return; } + if ( x->pri()->vtx()==v2 ) { v=v2; return; } + x=x->rot(); + } while ( x!=s ); + + _man->get_vertex_coordinates ( v1, x1, y1 ); + _man->get_vertex_coordinates ( v2, x2, y2 ); + + // First test the first vertex: + v = x->nxt()->vtx(); + _man->get_vertex_coordinates ( v, x3, y3 ); + + if ( sr_in_segment(x1,y1,x2,y2,x3,y3,0) ) return; + + // Then look for intersections in all neighbours: + do { v = x->nxt()->nxt()->vtx(); + _man->get_vertex_coordinates ( v, x4, y4 ); + + if ( sr_in_segment(x1,y1,x2,y2,x4,y4,0) ) return; + if ( sr_segments_intersect(x1,y1,x2,y2,x3,y3,x4,y4) ) { v=0; e=x->nxt(); return; } + + x3=x4; y3=y4; + x=x->rot(); + } while ( x!=s ); + + //======== this point should never be reached! ======================== + + v=0; e=0; printf ( "Error in v_next_step() !\n" ); + + printf("Constr: "); PRINTV(v1); PRINTV(v2); PNL; + printf("Center: "); PRINTV(s->vtx()); PNL; + printf("Star Vertex:\n"); + + x = s; + do { v = x->nxt()->vtx(); + PRINTV(v); PNL; + x=x->rot(); + } while ( x!=s ); + + v=0; e=0; + } + +void SeTriangulator::_e_next_step ( SeBase* s, SeVertex* v1, SeVertex* v2, + SeVertex*& v, SeBase*& e ) + { + v=0; + e=0; + SeVertex *va, *vb; + va = s->nxt()->vtx(); + vb = s->nxt()->nxt()->vtx(); + + if ( vb==v2 ) { v=v2; return; } + + double x1, y1, x2, y2, x3, y3, x4, y4; + _man->get_vertex_coordinates ( v1, x1, y1 ); + _man->get_vertex_coordinates ( v2, x2, y2 ); + _man->get_vertex_coordinates ( va, x3, y3 ); + _man->get_vertex_coordinates ( vb, x4, y4 ); + + if ( sr_in_segment(x1,y1,x2,y2,x4,y4,0) ) { v=vb; } + else if ( sr_segments_intersect(x1,y1,x2,y2,x3,y3,x4,y4) ) { e=s->nxt(); } + else { e=s->nxt()->nxt(); } + } + +bool SeTriangulator::_can_connect ( SeBase* se, SeBase* sv ) + { + double x1, y1, x2, y2, x3, y3, x, y; + + _man->get_vertex_coordinates ( se->vtx(), x1, y1 ); + _man->get_vertex_coordinates ( se->nxt()->vtx(), x2, y2 ); + _man->get_vertex_coordinates ( sv->vtx(), x3, y3 ); + + SeBase* s; + for ( s=se->nxt()->nxt(); s!=se; s=s->nxt() ) + { if ( s==sv ) continue; + _man->get_vertex_coordinates ( s->vtx(), x, y ); + if ( sr_in_circle(x1,y1,x2,y2,x3,y3,x,y) ) return false; + } + + return true; + } + +//================================================================================ +//============================== search path ===================================== +//================================================================================ + +bool SeTriangulator::_blocked ( SeBase* s ) + { + SeBase *sym = s->sym(); + + // test if the triangle is already visited: + if ( _mesh->marked(sym->fac()) ) return true; + + // test if we're trying to cross an obstacle: + if ( _man->is_constrained(s->edg()) ) return true; + + // security to avoid entering the border if a given point is outside the domain: + if ( sym->nxt()->nxt()->nxt()!=sym ) return true; + + // ok, the edge is not blocked: + return false; + } + +//We should use the Euclidian distance to have better results +# define PTDIST(a,b,c,d) sqrt(sr_dist2(a,b,c,d)) + +void SeTriangulator::_ptree_init ( SeTriangulator::LocateResult res, SeBase* s, + double xi, double yi, double xg, double yg ) + { + double c, h; + double x1, y1, x2, y2, x3, y3, x, y; + + _man->get_vertex_coordinates ( s->vtx(), x1, y1 ); + _man->get_vertex_coordinates ( s->nxt()->vtx(), x2, y2 ); + _man->get_vertex_coordinates ( s->pri()->vtx(), x3, y3 ); + + _ptree->init (); + + if ( res==EdgeFound && !_blocked(s) ) + { h = PTDIST(xi,yi,xg,yg); + _ptree->add_child ( -1, s, 0, h ); + _ptree->add_child ( -1, s->sym(), 0, h ); + return; + } + + _mesh->mark ( s->fac() ); + + if ( !_blocked(s) ) + { x=(x1+x2)/2; y=(y1+y2)/2; + c = PTDIST(xi,yi,x,y); + h = PTDIST(x,y,xg,yg); + _ptree->add_child ( -1, s, c, h ); + } + + s = s->nxt(); + if ( !_blocked(s) ) + { x=(x2+x3)/2; y=(y2+y3)/2; + c = PTDIST(xi,yi,x,y); + h = PTDIST(x,y,xg,yg); + _ptree->add_child ( -1, s, c, h ); + } + + s = s->nxt(); + if ( !_blocked(s) ) + { x=(x3+x1)/2; y=(y3+y1)/2; + c = PTDIST(xi,yi,x,y); + h = PTDIST(x,y,xg,yg); + _ptree->add_child ( -1, s, c, h ); + } + } + +# define ExpansionNotFinished -1 +# define ExpansionBlocked -2 + +int SeTriangulator::_expand_lowest_cost_leaf () + { + double x1, y1, x2, y2, x3, y3, x, y, a, b; + double dcost, hcost, ncost; + int min_i; + + min_i = _ptree->lowest_cost_leaf (); + SR_TRACE5 ( "Expanding leaf: "<nodes[min_i];//leaf(min_i); + SeBase* s = n.s; + ncost = n.ncost; + + _man->get_vertex_coordinates ( s->vtx(), x1, y1 ); + _man->get_vertex_coordinates ( s->nxt()->vtx(), x2, y2 ); + _man->get_vertex_coordinates ( s->sym()->pri()->vtx(), x3, y3 ); + + // note that (p1,p2,p3) is not ccw, so we test (p2,p1,p3): + if ( sr_in_triangle(x2,y2,x1,y1,x3,y3,_xg,_yg) ) return min_i;//leafs[min_i]; // FOUND! + + s = s->sym(); + _mesh->mark ( s->fac() ); + x=(x1+x2)/2; y=(y1+y2)/2; + + s = s->nxt(); + if ( !_blocked(s) ) + { a=(x1+x3)/2; b=(y1+y3)/2; + dcost = PTDIST(x,y,a,b); + hcost = PTDIST(a,b,_xg,_yg); + _ptree->add_child ( min_i, s, ncost+dcost, hcost ); + } + + s = s->nxt(); + if ( !_blocked(s) ) + { a=(x3+x2)/2; b=(y3+y2)/2; + dcost = PTDIST(x,y,a,b); + hcost = PTDIST(a,b,_xg,_yg); + _ptree->add_child ( min_i/*_ptree->leafs[min_i]*/, s, ncost+dcost, hcost ); + } + + return ExpansionNotFinished; // continue the expansion + } + +/* - Changing of triangle according to the edge semi-plane criteria (as for locate_point), + has no correlation to the shortest path... + - If the path is created from a list of faces, when linking the centroids, the + trajectory is much worse, creating many zig-zags. + - This is the A* algorithm that takes O(nf), f is the faces in the "expansion frontier". + Without the "add distance to goal heuristic" in the cost function, this would + be the Dijkstra algorithm. */ +bool SeTriangulator::search_path ( double x1, double y1, double x2, double y2, + const SeFace* iniface, bool vistest ) + { + LocateResult res; + SeBase *s; + + SR_TRACE5 ( "Starting Find Path..." ); + + if ( !_ptree ) _ptree = new PathTree; + _path_found = false; + _channel.size(0); + _xi=x1; _yi=y1; _xg=x2; _yg=y2; + + if ( !iniface ) return _path_found=false; + + res=locate_point ( iniface, x1, y1, s ); + if ( res==NotFound ) + { SR_TRACE5 ( "Could not locate first point!" ); + return _path_found=false; + } + + // Even if p1 is coincident to a vertex or edge, locate_point should return in s + // the face which is most likely to contain p1 + if ( _man->in_triangle(s->vtx(),s->nxt()->vtx(),s->pri()->vtx(),x2,y2) ) + { SR_TRACE5 ( "Found both points in the same triangle! (0 length channel returned)" ); + return _path_found=true; + } + + // The graph used in the A* search gives and aproximate shortest path. + // If we dont want to miss shortest paths which are a direct line, + // a specific test is performed for such cases. + # define INTER(q) _man->segments_intersect(x1,y1,x2,y2,q->vtx(),q->nxt()->vtx()) + if ( vistest ) + { // Find first intersection with the triangle of p2: + SeBase *se=s; + int i=0; + while ( !INTER(se) ) + { se=se->nxt(); + if ( i++==3 ) return _path_found=true; // none intersections (seg in triangle) (already tested anyway) + } + // Continues until end: + SeBase *sen, *sep; + while ( !_man->is_constrained(se->edg()) ) + { se=se->sym(); sen=se->nxt(); sep=se->pri(); + if ( INTER(sen) ) se=sen; + else + if ( INTER(sep) ) se=sep; + else + { SR_TRACE5 ( "Path is a direct line!" ); return _path_found=true; } + } + } + # undef INTER + + SR_TRACE5 ( "Initializing A* search..." ); + _mesh->begin_marking (); + _ptree_init ( res, s, x1, y1, x2, y2 ); // remember we start from the init point (x1,y1) + + SR_TRACE5 ( "Expanding leafs..." ); + int found = ExpansionNotFinished; + while ( found==ExpansionNotFinished ) + found = _expand_lowest_cost_leaf(); + + _mesh->end_marking (); + + if ( found==ExpansionBlocked ) + { SR_TRACE5 ( "Points are not connectable!" ); + return _path_found=false; + } + + int n = found; // the starting leaf + do { _channel.push() = _ptree->nodes[n].s; + n = _ptree->nodes[n].parent; + } while ( n!=-1 ); + _channel.revert(); + + SR_TRACE5 ( "Path crosses "<<_channel.size()<<" edges." ); + + return _path_found=true; + } + +void SeTriangulator::get_channel_boundary ( SrPolygon& channel ) + { + channel.size(0); + if ( !_path_found ) return; + + int i; + double x, y; + SeVertex* v; + SeVertex* lastv=0; + + channel.push().set ( (float)_xi, (float)_yi ); + + for ( i=0; i<_channel.size(); i++ ) + { v = _channel[i]->vtx(); + if ( v==lastv ) continue; + lastv = v; + _man->get_vertex_coordinates ( v, x, y ); + channel.push().set ( (float)x, (float)y ); + } + + channel.push().set ( (float)_xg, (float)_yg ); + + for ( i=_channel.size()-1; i>=0; i-- ) + { v = _channel[i]->nxt()->vtx(); + if ( v==lastv ) continue; + lastv = v; + _man->get_vertex_coordinates ( v, x, y ); + channel.push().set ( (float)x, (float)y ); + } + } + +void SeTriangulator::get_canonical_path ( SrPolygon& path ) + { + path.open ( true ); + if ( !_path_found ) { path.size(0); return; } + + int i; + double x1, y1, x2, y2; + path.size(0); + path.push().set ( (float)_xi, (float)_yi ); + for ( i=0; i<_channel.size(); i++ ) + { _man->get_vertex_coordinates ( _channel[i]->vtx(), x1, y1 ); + _man->get_vertex_coordinates ( _channel[i]->nxt()->vtx(), x2, y2 ); + path.push().set ( (float)(x1+x2)/2, (float)(y1+y2)/2 ); + } + path.push().set ( (float)_xg, (float)_yg ); + } + +//================================================================================ +//=========================== funnel shortest path =============================== +//================================================================================ + + +static bool ordccw ( bool normal_order, SrPnt2 p1, SrPnt2 p2, float x, float y ) + { + if ( normal_order ) + return SR_CCW(p1.x,p1.y,p2.x,p2.y,x,y)>=0? true:false; + else + return SR_CCW(p2.x,p2.y,p1.x,p1.y,x,y)>=0? true:false; + } + +void SeTriangulator::_funnel_add ( bool intop, SrPolygon& path, const SrPnt2& p ) + { + FunnelDeque& dq = *_fdeque; + + dq.top_mode ( intop ); + + bool ccw; + bool order = intop; + bool newapex = false; + if ( dq.size()<=2 ) { dq.push().set(p.x,p.y); return; } + + while(1) + { SrFunnelPt& p1 = dq.get(); + SrFunnelPt& p2 = dq.get(1); + + // if the apex is passed, the orientation test changes + if ( p1.apex ) { order=!order; newapex=true; } + ccw = ordccw ( order, p1, p2, p.x, p.y ); + if ( !ccw ) break; + + dq.pop(); + if ( newapex ) path.push().set ( p2.x, p2.y ); + + if ( dq.size()==1 ) break; + } + + if ( dq.size()==1 ) + { SrFunnelPt g=dq.get(); // cannot get a reference as the deque will change + dq.get().apex=true; + dq.push().set(p.x,p.y); + dq.top_mode(!intop); + dq.push().set(g.x,g.y); + } + else + { if ( newapex ) dq.get().apex=true; + dq.push().set(p.x,p.y); + } + + SR_TRACE7 ( " psize:"<get_vertex_coordinates ( s->vtx(), x, y ); p1.set((float)x,(float)y); + _man->get_vertex_coordinates ( s->nxt()->vtx(), x, y ); p2.set((float)x,(float)y); + _funnel_add ( 0, path, p1 ); + _funnel_add ( 1, path, p2 ); + + int max = _channel.size()-1; + SeBase *s1, *s2; + for ( i=0; iget_vertex_coordinates ( s2->vtx(), x, y ); p1.set((float)x,(float)y); + _man->get_vertex_coordinates ( s2->nxt()->vtx(), x, y ); p2.set((float)x,(float)y); + + if ( s1->vtx()==s2->vtx() ) // upper vertex rotates + _funnel_add ( 1, path, p2 ); + //left + else + _funnel_add ( 0, path, p1 ); + //right + } + + // To finalize: + // 1. check where is the zone in the funnel that the goal is: + bool order=true; + dq.top_mode ( order ); + while ( dq.size()>1 ) + { if ( dq.get().apex ) { order=!order; dq.top_mode(order); } + if ( ordccw(order,dq.get(),dq.get(1),(float)_xg,(float)_yg) ) dq.pop(); + else break; + } + + // 2. add the needed portion of the funnel to the path: + for ( i=0; dq.get(i).apex==0; i++ ); + while ( --i>=0 ) path.push() = dq.get(i); + + // 3. end path: + path.push().set((float)_xg,(float)_yg); + + SR_TRACE7 ( "End..." ); + } + +//============================ End of File ================================= + + diff --git a/source/dcdt/se/se_triangulator.h b/source/dcdt/se/se_triangulator.h new file mode 100644 index 0000000000..5fb3de4fb0 --- /dev/null +++ b/source/dcdt/se/se_triangulator.h @@ -0,0 +1,366 @@ + +# ifndef SE_TRIANGULATOR_H +# define SE_TRIANGULATOR_H + +/** \file se_triangulator.h + * A triangulator based on symedges + */ + +# include "sr_polygon.h" +# include "se_mesh.h" + +/*! SeTriangulatorManager contains virtual methods that the user needs to provide + to the triangulator, allowing the triangulator to: + 1.access the coordinates of a vertex, which is a user-denided type. + 2.notify the user of some events: vertices, edges created, etc. + 3.access the constrained information of edges. This information should be + stored as an array with the constraints ids sharing the constrained edge. + However, if constraints are guaranteed to not overllap, the user can + simply maintain a boolean variable. + Other utility methods are available in the class. + Note that the required virtual methods to be implemented are only those + related to the triangulator functions being used. + */ +class SeTriangulatorManager : public SrSharedClass + { public : + + /*! Allows retrieving the coordinates of a vertex. This method is pure + virtual, so it needs to be implemented in all cases. */ + virtual void get_vertex_coordinates ( const SeVertex* v, double& x, double& y )=0; + + /*! Allows setting the coordinates of a vertex. This method is pure + virtual, so it needs to be implemented in all cases. */ + virtual void set_vertex_coordinates ( SeVertex* v, double x, double y )=0; + + /*! This method is called by SeTriangulator::triangulate_face() to notify + new edges created during the triangulation process, before the optimization + phase, which is based on edge swap. The default implementation simply does nothing.*/ + virtual void triangulate_face_created_edge ( SeEdge* e ); + + /*! This method is called by the triangulator to notify when vertices which + are intersection of two constrained edges are inserted in the triangulation. + The default implementation simply does nothing. */ + virtual void new_intersection_vertex_created ( SeVertex* v ); + + /*! This method is called by the triangulator to notify when steiner vertices are + inserted when recovering conforming line constraints. The default implementation + simply does nothing. */ + virtual void new_steiner_vertex_created ( SeVertex* v ); + + /*! This method is called to track vertices which are found when inserting + edge constraints with insert_line_constraint(). Meaning that the found + vertices v which are in the segment v1, v2 are passed here. The default + implementation simply does nothing. */ + virtual void vertex_found_in_constrained_edge ( SeVertex* v ); + + /*! Should return true if the passed edge is constrained. The default + implementation always returns false. */ + virtual bool is_constrained ( SeEdge* e ); + + /*! Requires that the edge becomes unconstrained. This is called during + manipulation of the triangulation, when edges need to be temporary + unconstrained for consistent edge flipping. The default implementation + does nothing. */ + virtual void set_unconstrained ( SeEdge* e ); + + /*! Used to see if an edge is constrained. The passed array of ids is empty. + If it is returned as empty, it means that the edge is not constrained; + and this is the case of the default implementation. */ + virtual void get_constraints ( SeEdge* e, SrArray& ids ); + + /*! This method is called when the edge needs to be set as constrained. + The passed array contains the ids to be added to the edge. So that + the user can keep track of many constraints sharing the same constrained + edge. The passed array will never be empty. The default implementation + does nothing. */ + virtual void add_constraints ( SeEdge* e, const SrArray& ids ); + + /*! Retrieves the coordinates and call sr_ccw() */ + bool ccw ( SeVertex* v1, SeVertex* v2, SeVertex* v3 ); + + /*! Retrieves the coordinates and call sr_in_triangle() */ + bool in_triangle ( SeVertex* v1, SeVertex* v2, SeVertex* v3, SeVertex* v ); + + /*! Retrieves the coordinates and call sr_in_triangle() */ + bool in_triangle ( SeVertex* v1, SeVertex* v2, SeVertex* v3, double x, double y ); + + /*! Retrieves the coordinates and call sr_in_segment() */ + bool in_segment ( SeVertex* v1, SeVertex* v2, SeVertex* v, double eps ); + + /*! Retrieves the coordinates and call sr_in_circle() */ + bool is_delaunay ( SeEdge* e ); + + /*! Uses sr_ccw() and sr_in_circle() */ + bool is_flippable_and_not_delaunay ( SeEdge* e ); + + /*! Check if (x,y) lies in the interior, edge, or vertex of (v1,v2,v3). + A SeTriangulator::LocateResult enum is returned, and s will be adjacent to + the found element, if any (sr_next() and sr_in_segment() are used). */ + int test_boundary ( SeVertex* v1, SeVertex* v2, SeVertex* v3, + double x, double y, double eps, SeBase*& s ); + + /*! Retrieves the coordinates and call sr_segments_intersect() */ + bool segments_intersect ( SeVertex* v1, SeVertex* v2, + SeVertex* v3, SeVertex* v4, double& x, double& y ); + + /*! Retrieves the coordinates and call sr_segments_intersect() */ + bool segments_intersect ( SeVertex* v1, SeVertex* v2, SeVertex* v3, SeVertex* v4 ); + + /*! Retrieves the coordinates and call sr_segments_intersect() */ + bool segments_intersect ( double x1, double y1, double x2, double y2, + SeVertex* v3, SeVertex* v4 ); + + /*! Returns the midpoint of segment (v1,v2) */ + void segment_midpoint ( SeVertex* v1, SeVertex* v2, double& x, double& y ); + }; + +/*! \class SeTriangulator sr_triangulator.h + \brief Delaunay triangulation methods + + SeTriangulator provides several methods to construct and update + triangulations, including Delaunay triangulations with support + to constrained and conforming versions. + To use it, a SeTriangulatorManager class is required in order + to tell how the triangulator can access the data in the user mesh. + The triangulator methods were made to be of flexible use for different + applications, so that they only perform the basic algorithms work, + leaving many decisions to the user. In this way it is left as user + responsability to correctly use them. For instance, the associated mesh + must be in fact a triangulation with counter-clockwise triangles, etc, + otherwise several algorithms will fail. + Most of the used techniques are described in the following paper: + M. Kallmann, H. Bieri, and D. Thalmann, "Fully Dynamic Constrained + Delaunay Triangulations", In Geometric Modelling for Scientific + Visualization, G. Brunnett, B. Hamann, H. Mueller, L. Linsen (Eds.), + ISBN 3-540-40116-4, Springer-Verlag, Heidelberg, Germany, pp. 241-257, 2003. + However, some solutions presented in the paper may not be available in the current + version of the code, which favoured to maintain simpler approaches. + All used geometric primitives are implemented in sr_geo2.cpp */ +class SeTriangulator + { public : + + /*! Note: ModeConforming is not robust when several constraints intersect */ + enum Mode { ModeUnconstrained, ModeConforming, ModeConstrained }; + + /*! Used by locate_point() method */ + enum LocateResult { NotFound, TriangleFound, EdgeFound, VertexFound }; + + private : + Mode _mode; + double _epsilon; + SeMeshBase* _mesh; + SeTriangulatorManager* _man; + SrArray _buffer; + SrArray _ibuffer; + struct ConstrElem { SeVertex* v; SeBase* e; + void set(SeVertex* a, SeBase* b) {v=a; e=b;} + }; + SrArray _elem_buffer; + class PathNode; + class PathTree; + PathTree* _ptree; + bool _path_found; + SrArray _channel; + double _xi, _yi, _xg, _yg; + class FunnelDeque; + FunnelDeque* _fdeque; + + public : + + /*! Three modes of triangulations can be created: unconstrained, conforming, + or constrained. Depending on the mode some internal methods will + behave differently. In particular, for unconstrained triangulations + it is not required to reimplement the methods of SeTriangulatorManager + dealing with is/set/get/add edge constraints. + The documentation of each method should tell which methods are used from + both the associated manager and geometric primitives. + The required manager should be allocated with operator new and can be shared. + The epsilon is used in the geometric primitives. */ + SeTriangulator ( Mode mode, SeMeshBase* m, SeTriangulatorManager* man, double epsilon ); + + /*! The destructor unreferences the associated SeTriangulatorManager, + but it does not delete the associated mesh. */ + ~SeTriangulator (); + + /*! Set a new epsilon */ + void epsilon ( double eps ) { _epsilon=eps; } + + /*! Get the currently used epsilon */ + double epsilon () const { return _epsilon; } + + /*! Returns the associated mesh pointer */ + SeMeshBase* mesh () const { return _mesh; } + + /*! Returns the associated manager pointer */ + SeTriangulatorManager* manager () const { return _man; } + + /*! Set the desired mode */ + void mode ( Mode m ) { _mode=m; } + + /*! Get the current triangulator mode */ + Mode mode () const { return _mode; } + + public : + + /*! This method destroys the associated mesh and initializes it as + a triangulated square with the given coordinates. Points must + be passed in counter clockwise order. The returned SeBase + is adjacent to vertex 1, edge (1,2) and triangle (1,2,4). + To construct a square with given maximum and minimum coordinates, + points should be set as follows (respectivelly): + xmin, ymin, xmax, ymin, xmax, ymax, xmin, ymax. + Alternatively, the user may initialize the associated mesh directly + by calling the construction operators of SeMesh (such as mev and mef). + Normally the mesh should be initialized as a triangulated convex polygon. */ + SeBase* init_as_triangulated_square + ( double x1, double y1, + double x2, double y2, + double x3, double y3, + double x4, double y4 ); + + /*! Triangulates a counter-clockwise (ccw) oriented face. It is the + user responsability to provide a face in the correct ccw orientation. + False is returned in case the face cannot be triangulated. However, + this migth only happen if the face is not simple. + It uses the so called "ear" algorithm, that has worst case complexity + of O(n^2), but has a very simple implementation. + It uses the geometric primitives sr_ccw() and sr_in_triangle() and + each time an edge is inserted during the triangulation construction, + the method new_edge_created() of the associated manager is called. + If optimization is set to true (the default), the algorithm will + flip the created edges to satisfy the Delaunay criterium, and for + this, the geometric primitive sr_in_circle() is also used. */ + bool triangulate_face ( SeFace* f, bool optimize=true ); + + /*! This method searches for the location containing the given point. + The enumerator LocateResult is returned with the result of the + query. If the point is coincident to an existing vertex, VertexFound + is returned, and result is incident to the vertex. + If the point is found on an edge, EdgeFound is returned, and result + is incident to the edge. If the point is inside a triangle, result + is incident to the found triangle. In the case that the point is + not found to be inside the triangulation, NotFound is returned. + The algorithm starts with the given iniface, and continously + skips to the neighbour triangle which shares an edge separating the + current triangle and the point to reach in two different semi spaces. + To avoid possible loops, triangle marking is used. + The "distance" of the given iniface and the point to search + dictates the performance of the algorithm. The agorithm is O(n) + (n = number of triangles). In general the pratical performance + is much better than O(n). However serious loop problems will occur + if topological inconsistencies are detected. + This algorithm may fail to find the point if the border of the + triangulation is not convex or if non triangular faces exist. + It is mainly based on the geometric primitive sr_ccw(). But it also + calls methods sr_points_are_equal() and sr_in_segment(), in order to + decide if the point is in an existing edge or vertex. */ + LocateResult locate_point ( const SeFace* iniface, + double x, double y, SeBase*& result ); + + /*! Insert a point in the given triangle, with the given coordinates, + and then continously flips its edges to ensure the Delaunay criterium. + The geometric primitive sr_in_circle() is called during this process. + The new vertex inserted is returned and will be never 0 (null). + Methods new_vertex_created() and new_edge_created() of the associated + manager are called for the new three edges and one vertex created. + If the triangulator is of type conforming, methods for managing the + constraints ids of edges are called, and to maintain the correctness + of the triangulation, new vertices might be automatically inserted + in a recursive mid-point subdivision way. + If the triangulator is of constrained type, points are inserted only + at eventual intersection points. */ + SeVertex* insert_point_in_face ( SeFace* f, double x, double y ); + + /*! Insert a point in the given edge, with the given coordinates. + It might be a good practice to project the point to the segment + defined by the edge before calling this method. + After insertion, it continously flips edges to ensure the Delaunay + criterium, as in insert_point_in_face */ + SeVertex* insert_point_in_edge ( SeEdge* e, double x, double y ); + + /*! Insert point searchs the location of (x,y) and correclty insert it in case it + is located in an edge or face. If a coincident vertex is found it is returned. + If iniface is null, the search starts from mesh()->first()->fac(). + Null is returned in case the point cannot be located. */ + SeVertex* insert_point ( double x, double y, const SeFace* inifac=0 ); + + /*! Removes a vertex from the Delaunay triangulation. It is the user responsibility + to guarantee that the vertex being removed is inside a triangulation, and not + at the border of a face which is not triangular. + This method simply calls SeMesh::delv() and then retriangulates the created + non-triangular polygon, so that no special actions are taken concerning + constrained edges. */ + bool remove_vertex ( SeVertex* v ); + + /*! Inserts a line constraint to the current Delaunay triangulation. The line + is defined by two existing vertices. This method has a different behavior + depending on the actual mode of the triangulator: + If the triangulator is in unconstrained mode, nothing is done. + If the mode is conforming, Steiner points are inserted starting with the + middle point of the missing constraint, and recursively inserted by binary + partition until the line becomes present in the triangulation. + If the triangulator is in constrained mode, Steiner points are only inserted + at the intersection of existing constraints, if there are any. Hence, + constrained edges do not respect the Delaunay criterium. + False is returned if the algorithm fails, what can occur if non ccw or non + triangular cells are found, or in unconstrained mode. + The id parameter allows the user to keep track of the created constraints, + and it is passed to the corresponding manager method. + Note that edges may be referenced by several + constrained lines in the case overllap occurs, and thats why each edge + of the triangulation should maintain an array of constraints ids. + Methods new_vertex_created() and vertex_found_in_constrained_edge() of + the manager are called to allow tracking the insertion of Steiner points. */ + bool insert_line_constraint ( SeVertex *v1, SeVertex *v2, int id ); + + /*! Inserts the two points with insert_point() and then call insert_line_constraint(). + Returns the success or failure of the operation */ + bool insert_segment ( double x1, double y1, double x2, double y2, int id, const SeFace* inifac=0 ); + + /*! Search for a sequence of triangles (e.g. a channel) connecting x1,y1 and x2,y2, + without crossing edges marked as constrained. + If true is returned, the channel and paths inside the channel can be retrieved + with methods get_channel(), get_canonical_path(), get_shortest_path(). + Note that the channel may not be the shortest one available. + Parameter iniface is required in order to feed the process of finding + the triangle containing the initial point p1. If it is already the triangle + containing p1, the search will be solved trivially. + If vistest is true (the default if false) a direct line test is performed prior + to the path search. + The A* algorithm is used, with the simple heuristic "dist(cur_node,goal_node)" */ + bool search_path ( double x1, double y1, double x2, double y2, + const SeFace* iniface, bool vistest=false ); + + /*! Returns a reference to the list with the interior edges of the last channel + determined by a sussesfull call to search_path */ + const SrArray& get_channel_interior_edges () const { return _channel; } + + /*! Returns a polygon describing the current channel, and thus, method find_path must + be succesfully called before to determine the channel to consider. */ + void get_channel_boundary ( SrPolygon& channel ); + + /*! Returns the canonical path, which is the path passing through the midpoint of + the channel interior edges. Method search_path must be succesfully called before + in order to determine the channel. The path is returned as an open polygon. */ + void get_canonical_path ( SrPolygon& path ); + + /*! Returns the shortest path inside the current channel using the funnel algorithm. */ + void get_shortest_path ( SrPolygon& path ); + + private : + void _propagate_delaunay (); + bool _conform_line ( SeVertex*, SeVertex*, const SrArray& ); + bool _constrain_line ( SeVertex*, SeVertex*, const SrArray& ); + void _v_next_step ( SeBase* s, SeVertex* v1, SeVertex* v2, SeVertex*& v, SeBase*& e ); + void _e_next_step ( SeBase* s, SeVertex* v1, SeVertex* v2, SeVertex*& v, SeBase*& e ); + bool _can_connect ( SeBase* se, SeBase* sv ); + bool _blocked ( SeBase* s ); + void _ptree_init ( LocateResult res, SeBase* s, double xi, double yi, double xg, double yg ); + int _expand_lowest_cost_leaf (); + void _funnel_add ( bool intop, SrPolygon& path, const SrPnt2& p ); + }; + +//============================ End of File ================================= + +# endif // SE_TRIANGULATOR_H + diff --git a/source/dcdt/se/sr.cpp b/source/dcdt/se/sr.cpp new file mode 100644 index 0000000000..ecb5d1a7a6 --- /dev/null +++ b/source/dcdt/se/sr.cpp @@ -0,0 +1,219 @@ +#include "precompiled.h" +# include +# include + +# include "sr.h" + +//================================ sr.cpp ============================================ + +float sr_todeg ( float radians ) + { + return 180.0f * radians / float(SR_PI); + } + +double sr_todeg ( double radians ) + { + return 180.0 * radians / double(SR_PI); + } + +float sr_torad ( float degrees ) + { + return float(SR_PI) * degrees / 180.0f; + } + +double sr_torad ( double degrees ) + { + return double(SR_PI) * degrees / 180.0; + } + +float sr_trunc ( float x ) + { + return (float) (int) (x); + } + +double sr_trunc ( double x ) + { + return (double) (int) (x); + } + +float sr_round ( float x ) + { + return (float) (int) ((x>0.0)? (x+0.5f) : (x-0.5f)); + } + +double sr_round ( double x ) + { + return (double) (int) ((x>0.0)? (x+0.5) : (x-0.5)); + } + +float sr_floor ( float x ) + { + return (float) (int) ((x>0.0)? x : (x-1.0f)); + } + +double sr_floor ( double x ) + { + return (double) (int) ((x>0.0)? x : (x-1.0)); + } + +float sr_ceil ( float x ) + { + return (float) (int) ((x>0.0)? (x+1.0f) : (x)); + } + +double sr_ceil ( double x ) + { + return (double) (int) ((x>0.0)? (x+1.0) : (x)); + } + +int sr_sqrt ( int n ) + { + register int i, s=0, t; + + for ( i=15; i>=0; i-- ) + { t = ( s | (1<1 ) m *= x; + return m; + } + +int sr_pow ( int b, int e ) + { + if ( e<=0 ) return 1; + int pow=b; + while ( --e>0 ) pow*=b; + return pow; + } + +int sr_compare ( const char *s1, const char *s2 ) + { + int c1, c2; // ANSI definition of toupper() uses int types + + while ( *s1 && *s2 ) + { c1 = SR_UPPER(*s1); + c2 = SR_UPPER(*s2); + if ( c1!=c2 ) return c1-c2; + s1++; s2++; + } + if ( !*s1 && !*s2 ) return 0; + return !*s1? -1:1; + } + +int sr_compare_cs ( const char *s1, const char *s2 ) + { + int c1, c2; // ANSI definition of toupper() uses int types + + while ( *s1 && *s2 ) + { c1 = *s1; + c2 = *s2; + if ( c1!=c2 ) return c1-c2; + s1++; s2++; + } + if ( !*s1 && !*s2 ) return 0; + return !*s1? -1:1; + } + +int sr_compare ( const char *s1, const char *s2, int n ) + { + int c1, c2; // ANSI definition of toupper() uses int types + + // printf("[%s]<>[%s] (%d)\n",s1,s2,n); + + while ( *s1 && *s2 ) + { c1 = SR_UPPER(*s1); + c2 = SR_UPPER(*s2); + if ( c1!=c2 ) return c1-c2; + s1++; s2++; n--; + if ( n==0 ) return n; // are equal + } + if ( !*s1 && !*s2 ) return 0; + return !*s1? -1:1; + } + +int sr_compare_cs ( const char *s1, const char *s2, int n ) + { + int c1, c2; // ANSI definition of toupper() uses int types + + // printf("[%s]<>[%s] (%d)\n",s1,s2,n); + + while ( *s1 && *s2 ) + { c1 = *s1; + c2 = *s2; + if ( c1!=c2 ) return c1-c2; + s1++; s2++; n--; + if ( n==0 ) return n; // are equal + } + if ( !*s1 && !*s2 ) return 0; + return !*s1? -1:1; + } + +int sr_compare ( const int *i1, const int *i2 ) + { + return *i1-*i2; + } + +int sr_compare ( const float *f1, const float *f2 ) + { + return SR_COMPARE(*f1,*f2); + } + +int sr_compare ( const double *d1, const double *d2 ) + { + return SR_COMPARE(*d1,*d2); + } + +char* sr_string_new ( const char *tocopy ) + { + if ( !tocopy ) return 0; + char *s = new char [ strlen(tocopy)+1 ]; + strcpy ( s, tocopy ); + return s; + } + +char* sr_string_set ( char*& s, const char *tocopy ) + { + delete[] s; + if ( !tocopy ) { s=0; return s; } + s = new char [ strlen(tocopy)+1 ]; + strcpy ( s, tocopy ); + return s; + } + +char* sr_string_realloc ( char*& s, int size ) + { + char *news = 0; + + if ( size>0 ) + { news = new char[size]; + news[0] = 0; + if ( s ) + { int i; + for ( i=0; i='A'&&(c)<='Z'? (c)-'A'+'a':(c) ) + +/*! Macro that puts c in upper case if c is a valid lower case letter. */ +# define SR_UPPER(c) ( (c)>='a'&&(c)<='z'? (c)-'a'+'A':(c) ) + +/*! Macro that returns 0 if a is equal to b, 1 if a>b, and -1 otherwise. */ +# define SR_COMPARE(a,b) (a==b)? 0: (a>b)? 1: -1 + +/*! Macro that swaps the value of a boolean type. */ +# define SR_SWAPB(b) b = !b // B from bool + +/*! Macro that swaps the values of a and b using three xor logical operations. */ +# define SR_SWAPX(a,b) { a^=b; b^=a; a^=b; } // x from xor + +/*! Macro that swaps the values of a and b, using tmp as temporary variable. */ +# define SR_SWAPT(a,b,tmp) { tmp=a; a=b; b=tmp; } + +/*! Macro that swaps the values of a and b, given that a temporary + variable named tmp, of the same type as a and b exists. */ +# define SR_SWAP(a,b) { tmp=a; a=b; b=tmp; } + +/*! Macro that returns a number multiple of gap, but that is greater or equal to size. */ +# define SR_SIZE_WITH_GAP(size,gap) ( (gap) * ( ((size)/(gap)) + ((size)%(gap)==0?0:1) ) ) + +/*! Macro that makes m to be x, if x is greater than m. */ +# define SR_UPDMAX(m,x) if((x)>(m)) m=x + +/*! Macro that makes m to be x, if x is smaller than m. */ +# define SR_UPDMIN(m,x) if((x)<(m)) m=x + +/*! Macro that tests if x is inside the interval [i,s]. */ +# define SR_BOUNDED(x,i,s) ((i)<=(x) && (x)<=(s)) + +/*! Macro that returns x clipped by the interval [i,s]. */ +# define SR_BOUND(x,i,s) (x)<(i)? (i): (x)>(s)? (s): (x) + +/*! Macro that forces a to be positive by negating it if it is negative. */ +# define SR_POS(a) if((a)<0) a=-(a) + +/*! Macro that forces a to be negative by negating it if it is positive. */ +# define SR_NEG(a) if((a)>0) a=-(a) + +/*! Macro that returns x, so that x = a(1-t) + bt. */ +# define SR_LERP(a,b,t) ((a)*(1-(t))+(b)*(t)) // return x = a(1-t) + bt + +/*! Macro that returns t, so that x = a(1-t) + bt. */ +# define SR_PARAM(a,b,x) ((x)-(a))/((b)-(a)) // return t : x = a(1-t) + bt + +/*! Macro that truncates x, with an int typecast. */ +# define SR_TRUNC(x) ( (int) (x) ) + +/*! Macro that returns x rounded to the nearest integer. */ +# define SR_ROUND(x) ( (int) ((x>0)? (x+0.5f):(x-0.5f)) ) + +/*! Macro that rounds x to the nearest integer, but to be + applied only when x is positive. This macro adds 0.5 + and does an int typecast. */ +# define SR_ROUNDPOS(x) ( (int) (x+0.5) ) + +/*! Macro that returns the lowest integer of x. */ +# define SR_FLOOR(x) ( int( ((x)>0)? (x):((x)-1) ) ) + +/*! Macro that returns the highest integer of x. */ +# define SR_CEIL(x) ( int( ((x)>0)? ((x)+1):(x) ) ) + +/*! Macro that returns the maximum value of the two arguments. */ +# define SR_MAX(a,b) ((a)>(b)? (a):(b)) + +/*! Macro that returns the maximum value of the three arguments. */ +# define SR_MAX3(a,b,c) ((a)>(b)? (a>c?(a):(c)):((b)>(c)?(b):(c))) + +/*! Macro that returns the minimum value of the two arguments. */ +# define SR_MIN(a,b) ((a)<(b)? (a):(b)) + +/*! Macro that returns the minimum value of the three arguments. */ +# define SR_MIN3(a,b,c) ((a)<(b)? ((a)<(c)?(a):(c)):((b)<(c)?(b):(c))) + +/*! Macro that returns the absolute value of x. */ +# define SR_ABS(x) ((x)>0? (x):-(x)) + +/*! Macro that returns |a-b|, that is the distance of two points in the line. */ +# define SR_DIST(a,b) ( (a)>(b)? ((a)-(b)):((b)-(a)) ) + +/*! Macro that tests if the distance between a and b is closer or equal to ds. */ +# define SR_NEXT(a,b,ds) ( ( (a)>(b)? ((a)-(b)):((b)-(a)) )<=(ds) ) + +/*! Macro that tests if the distance between a and 0 is closer or equal to ds. */ +# define SR_NEXTZ(a,eps) ( (a)>-(eps) && (a)<(eps) ) // z from zero + +/*! Macro that returns -1 if x is negative, 1 if x is positive and 0 if x is zero. */ +# define SR_SIGN(x) ((x)<0)? -1: ((x)>0)? 1: 0 + +/*! Returns the converted angle, from radians to degrees (float version). */ +# define SR_TODEG(r) (180.0f*float(r)/srpi) + +/*! Returns the converted angle, from degrees to radians (float version). */ +# define SR_TORAD(d) (srpi*float(d)/180.0f) + +/*! Returns the converted angle, from radians to degrees (double version). */ +# define SR_TODEGd(r) (180.0*double(r)/SR_PI) + +/*! Returns the converted angle, from degrees to radians (double version). */ +# define SR_TORADd(d) (SR_PI*double(d)/180.0) + +// ============================== Math Utilities =========================== + +/*! Returns the convertion from radians to degrees (float version). */ +float sr_todeg ( float radians ); + +/*! Returns the convertion from radians to degrees (double version). */ +double sr_todeg ( double radians ); + +/*! Returns the convertion from degrees to radians (float version). */ +float sr_torad ( float degrees ); + +/*! Returns the convertion from degrees to radians (double version). */ +double sr_torad ( double degrees ); + +/*! Returns the integer part of x by using a sequence of type casts (float version). */ +float sr_trunc ( float x ); + +/*! Returns the integer part of x by using a sequence of type casts (double version). */ +double sr_trunc ( double x ); + +/*! Returns the closest integer of x (float version). */ +float sr_round ( float x ); + +/*! Returns the closest integer of x (double version). */ +double sr_round ( double x ); + +/*! Returns the lowest rounded value of x (float version). */ +float sr_floor ( float x ); + +/*! Returns the lowest rounded value of x (double version). */ +double sr_floor ( double x ); + +/*! Returns the highest rounded value of x (float version). */ +float sr_ceil ( float x ); + +/*! Returns the highest rounded value of x (double version). */ +double sr_ceil ( double x ); + +/*! Returns the square root for integer values, with no use of floating point. */ +int sr_sqrt ( int x ); + +/*! sr_fact returns the factorial of x. */ +int sr_fact ( int x ); + +/*! returns "b^e", e must be >=0 */ +int sr_pow ( int b, int e ); + +// ============================= Compare Functions ============================ + +/*! Case insensitive comparison of strings in the C style + This function follows the C style of compare functions where 0 is returned if + s1==s2, <0 if s10 otherwise. Comparisons are case-insensitive. + s1 and s2 must be non-null pointers, otherwise unpredictable results will arise. + If two strings have the first n characters equal, where one has lenght n, and + the other has length >n, the smaller one is considered to come first. */ +int sr_compare ( const char *s1, const char *s2 ); + +/*! Case sensitive comparison of strings in the C style */ +int sr_compare_cs ( const char *s1, const char *s2 ); + +/*! Case insensitive compare strings, but compares a maximum of n characters. */ +int sr_compare ( const char *s1, const char *s2, int n ); + +/*! Case sensitive compare strings, but compares a maximum of n characters. */ +int sr_compare_cs ( const char *s1, const char *s2, int n ); + +/*! Compares two integers, returning 0 if they're equal, <0 if i10 otherwise. */ +int sr_compare ( const int *i1, const int *i2 ); + +/*! Compares two floats, returning 0 if they're equal, <0 if f10 otherwise. */ +int sr_compare ( const float *f1, const float *f2 ); + +/*! Compares two doubles, returning 0 if they're equal, <0 if d10 otherwise. */ +int sr_compare ( const double *d1, const double *d2 ); + +// ============================== C String Utilities ============================ + +/*! Allocates a string with sufficient size to copy 'tocopy' in it. + The allocation is simply done with operator new, and the allocated + memory pointer is returned. If tocopy==0, the value 0 is returned. */ +char* sr_string_new ( const char* tocopy ); + +/*! Deletes s, and reallocates s with sufficient size to copy 'tocopy' + in it. If tocopy==0, s will be a null pointer, and 0 is returned. + Otherwise the allocation is simply done with operator new, the + allocated memory pointer is returned and s is changed to point to + this new memory allocated so that the returned value will be the + same as s. */ +char* sr_string_set ( char*& s, const char *tocopy ); + +/*! Deletes s, and reallocates it with the given size also copying its + contents to the new allocated position. If size<=0, s is simply + deleted. If the new size is smaller then the original s length, + the contents will be truncated. In all cases, the new s is returned + and will be a valid string, having the ending null char. This + function is similar to the C standard realloc function, but using + C++ operators new/delete. */ +char* sr_string_realloc ( char*& s, int size ); + +// ============================== Standard IO ============================ + +/*! Redirects the C streams stdout and stderr to the text files + stdout.txt and stderr.txt in the current folder */ +void sr_stdout_to_file (); + +// ============================== Bit Operation ============================ + +/*! Tests if flg has the given bit set. */ +# define SR_FLAG_TEST(flg,bit) ((flg)&(bit)) + +/*! Sets on the given bit of the flag. */ +# define SR_FLAG_ON(flg,bit) flg|=bit + +/*! Swaps the given bit of the flag. */ +# define SR_FLAG_SWAP(flg,bit) flg^=bit + +/*! Sets off the given bit of the flag. This requires two instructions, so + that SR_FLAG_SWAP is faster. */ +# define SR_FLAG_OFF(flg,bit) if((flg)&(bit)) flg^=bit + +/*! Sets the given bit of the flag to be on or off, according to the boolean value of val. */ +# define SR_FLAG_SET(flg,bit,val) flg = (val)? (flg)|(bit) : ((flg)&(bit)? (flg)^(bit):flg) + +# define SR_BIT0 1 //!< Same as 1 +# define SR_BIT1 2 //!< Same as 2 +# define SR_BIT2 4 //!< Same as 4 +# define SR_BIT3 8 //!< Same as 8 +# define SR_BIT4 16 //!< Same as 16 +# define SR_BIT5 32 //!< Same as 32 +# define SR_BIT6 64 //!< Same as 64 +# define SR_BIT7 128 //!< Same as 128 +# define SR_ALLBITS 255 //!< Same as 255 +# define SR_NOBITS 0 //!< Same as 0 + +//============================== end of file =============================== + +# endif // SR_H diff --git a/source/dcdt/se/sr_alg.cpp b/source/dcdt/se/sr_alg.cpp new file mode 100644 index 0000000000..af48522520 --- /dev/null +++ b/source/dcdt/se/sr_alg.cpp @@ -0,0 +1,224 @@ +#include "precompiled.h" +# include + +# include "sr.h" +# include "sr_alg.h" + +/*-------------------------------------------------------------------*/ +/* Functions to solve polynomials of 2nd, 3rt and 4th degree. */ +/* Source: graphics gems */ +/*-------------------------------------------------------------------*/ + +//change kai: +//# define aMAXFLOAT 3.40282347E+38F +# define aMAXFLOAT 3.40282347E+28F +# define aEPSILON 1e-9 +# define aEPSILON2 0.00001 +# define aISZERO(x) ((x) > -aEPSILON && (x) < aEPSILON) +# define aCBRT(x) ((x) > 0.0 ? pow((double)(x), 1.0/3.0) : \ + ((x) < 0.0 ? -pow((double)-(x), 1.0/3.0) : 0.0)) + +int sr_solve_quadric_polynomial ( double c[3], double s[2] ) + { + double p, q, D; + + // normal form: x^2 + px + q = 0 + p = c[1] / (2*c[2]); + q = c[0] / c[2]; + + D = p*p - q; + + if ( aISZERO(D) ) + { s[0] = -p; + return 1; + } + else if ( D<0 ) + { return 0; + } + else // if (D > 0) + { double sqrt_D = sqrt(D); + s[0] = sqrt_D - p; + s[1] = - sqrt_D - p; + return 2; + } +} + +int sr_solve_cubic_polynomial ( double c[4], double s[3] ) + { + int i, num; + double sub; + double A, B, C; + double sq_A, p, q; + double cb_p, D; + + // normal form: x^3 + Ax^2 + Bx + C = 0 + A = c[2] / c[3]; + B = c[1] / c[3]; + C = c[0] / c[3]; + + // substitute x = y - A/3 to eliminate quadric term: + // x^3 +px + q = 0 + sq_A = A * A; + p = 1.0/3 * (- 1.0/3 * sq_A + B); + q = 1.0/2 * (A * (2.0/27 * sq_A - 1.0/3 * B) + C); + + // use Cardano's formula + cb_p = p * p * p; + D = q * q + cb_p; + + if ( aISZERO(D) ) + { if ( aISZERO(q) ) // one triple solution + { s[0] = 0; + num = 1; + } + else // one single and one double solution + { double u = aCBRT(-q); + s[0] = 2 * u; + s[1] = - u; + num = 2; + } + } + else if ( D<0 ) // Casus irreducibilis: three real solutions + { double phi = 1.0/3 * acos ( -q/sqrt(-cb_p) ); + double t = 2 * sqrt(-p); + s[0] = t * cos(phi); + s[1] = - t * cos(phi + SR_PI / 3); + s[2] = - t * cos(phi - SR_PI / 3); + num = 3; + } + else /* one real solution */ + { double sqrt_D = sqrt(D); + double u = aCBRT(sqrt_D - q); + double v = - aCBRT(sqrt_D + q); + s[0] = u + v; + num = 1; + } + + // resubstitute + sub = 1.0/3 * A; + for ( i=0; i 0) u = sqrt(u); + else return 0; + + if (aISZERO(v)) v = 0; + else if (v > 0) v = sqrt(v); + else return 0; + + coeffs[0] = z - u; + coeffs[1] = q < 0 ? -v : v; + coeffs[2] = 1; + num = sr_solve_quadric_polynomial ( coeffs, s ); + + coeffs[0]= z + u; + coeffs[1] = q < 0 ? v : -v; + coeffs[2] = 1; + num += sr_solve_quadric_polynomial ( coeffs, s+num ); + } + + // resubstitute + sub = 1.0/4 * A; + for ( i=0; i0 ? a : -a; + y = 0; + return; + } + + c [4] = y; + c [3] = f - e; + c [2] = 0; + c [1] = f + e; + c [0] = -y; + + int nb = sr_solve_quartic_polynomial ( c, s ); + + double denom, s_theta, c_theta, dist; + double test_x [4], test_y [4]; + double min_dist = aMAXFLOAT; + int i, winner=0; + + // Find the closest point + for ( i=0; i0 if it is outside */ +float sr_in_ellipse ( float x, float y, float a, float b ); + +/*! This function replaces the given point (x,y) by the closest point + on the ellipse whose semi-axes are of length a and b. The solution + is derived analyticaly, solving a quartic polynomial */ +void sr_get_closest_on_ellipse ( float a, float b, float& x, float& y ); + +//============================== end of file =============================== + +# endif // SR_ALG_H diff --git a/source/dcdt/se/sr_array.cpp b/source/dcdt/se/sr_array.cpp new file mode 100644 index 0000000000..6f96fd339d --- /dev/null +++ b/source/dcdt/se/sr_array.cpp @@ -0,0 +1,189 @@ +#include "precompiled.h" +# include +# include + +# include "sr_array.h" + +//# define SR_USE_MEM_CONTROL +# include "sr_mem_control.h" + +# define DATA(i) ((char*)_data)+(sizeofx*(i)) +# define NEWDATA(i) ((char*)newdata)+(sizeofx*(i)) + +//================================= methods ======================================================= + +SrArrayBase::SrArrayBase ( sruint sizeofx, int s, int c ) + : _size(s), _capacity(c) + { + if ( _capacity<_size ) _capacity=_size; + _data = _capacity>0? SR_MALLOC(sizeofx*_capacity) : 0; + } + +SrArrayBase::SrArrayBase ( sruint sizeofx, const SrArrayBase& a ) + : _size(a._size), _capacity(a._size) + { + if ( _capacity>0 ) + { _data = SR_MALLOC ( sizeofx*_capacity ); + if ( _size>0 ) memcpy ( _data, a._data, sizeofx*_size ); + } + else _data = 0; + } + +SrArrayBase::SrArrayBase ( void* pt, int s, int c ) + : _data(pt), _size(s), _capacity(c) + { + } + +void SrArrayBase::free_data () + { + if (_data) SR_FREE(_data); + } + +void SrArrayBase::size ( unsigned sizeofx, int ns ) + { + _size = ns; + if ( _size<0 ) _size=0; + + if ( _size>_capacity ) + { _capacity = _size; + _data = realloc ( _data, sizeofx*_capacity ); // if _data==0, realloc reacts as malloc. + } + } + +void SrArrayBase::capacity ( unsigned sizeofx, int nc ) + { + if ( nc<0 ) nc = 0; + if ( nc==_capacity ) return; + if ( nc==0 ) { if(_data)free(_data); _data=0; _capacity=_size=0; return; } + + _capacity = nc; + if ( _size>_capacity ) _size=_capacity; + + _data = realloc ( _data, sizeofx*_capacity ); // if _data==0, realloc reacts as malloc. + } + +int SrArrayBase::validate ( int index ) const + { + if ( index<0 ) index += _size * ( -index/_size + 1 ); + SR_ASSERT ( index>=0 ); + return index%_size; + } + +void SrArrayBase::compress ( sruint sizeofx ) + { + if ( _size==_capacity ) return; + + if ( !_size ) { SR_FREE ( _data ); _data=0; } + else _data = SR_REALLOC ( _data, sizeofx*_size ); + + _capacity = _size; + } + +void SrArrayBase::remove ( sruint sizeofx, int i, int dp ) + { + if ( i<_size-dp ) memmove ( DATA(i), DATA(i+dp), sizeofx*(_size-(i+dp)) ); + _size-=dp; + } + +void SrArrayBase::insert ( sruint sizeofx, int i, int dp ) + { + SR_ASSERT ( i>=0 && i<=_size); + SR_ASSERT ( dp>0 ); + + _size += dp; + + if ( _size>_capacity ) + { _capacity = _size*2; + _data = SR_REALLOC ( _data, sizeofx*_capacity ); + } + + if ( i<_size-dp ) + { memmove ( DATA(i+dp), DATA(i), sizeofx*(_size-dp-i) ); // ok with overlap + } + } + +void SrArrayBase::move ( sruint sizeofx, int dest, int src, int n ) + { + memmove ( DATA(dest), DATA(src), sizeofx*(n) ); + } + +void SrArrayBase::sort ( sruint sizeofx, srcompare cmp ) + { + ::qsort ( _data, (size_t)_size, (size_t)sizeofx, cmp ); + } + +int SrArrayBase::lsearch ( sruint sizeofx, const void *x, srcompare cmp ) const + { + char *pt = (char*)_data; + + for ( int i=0; i<_size; i++ ) { if ( cmp(pt,x)==0 ) return i; pt+=sizeofx; } + + return -1; + } + +int SrArrayBase::bsearch ( sruint sizeofx, const void *x, srcompare cmp, int *pos ) const + { + int comp; + int i, e, p; + + comp=1; i=0; e=_size; p=(e-i)/2; + + while ( comp!=0 && i!=e ) + { comp = cmp ( x, DATA(p) ); + if ( comp<0 ) e = p; + else if ( comp>0 ) i = p+1; + p = i + (e-i)/2; + } + + if (pos) *pos=p; + + return comp==0? p : -1; + } + +int SrArrayBase::insort ( sruint sizeofx, const void *x, srcompare cmp, bool allowdup ) + { + int result, pos; + result = bsearch ( sizeofx, x, cmp, &pos ); + if ( result!=-1 && !allowdup ) return -1; + insert ( sizeofx, pos, 1 ); + return pos; + } + +void SrArrayBase::copy ( sruint sizeofx, const SrArrayBase& a ) + { + if ( _data==a._data ) return; + if ( _data ) { SR_FREE(_data); _data=0; } + _capacity = a._capacity; + _size = a._size; + if ( _capacity>0 ) + { _data = SR_MALLOC ( sizeofx*_capacity ); + if ( _size>0 ) memcpy ( _data, a._data, sizeofx*_size ); + } + } + +void* SrArrayBase::leave_data () + { + void *pt = _data; + _data=0; + _size=0; + _capacity=0; + return pt; + } + +void SrArrayBase::take_data ( SrArrayBase& a ) + { + if ( _data ) SR_FREE ( _data ); + _data = a._data; a._data=0; + _size = a._size; a._size=0; + _capacity = a._capacity; a._capacity=0; + } + +void SrArrayBase::take_data ( void* pt, int s, int c ) + { + if ( _data ) SR_FREE ( _data ); + _data = pt; + _size = s; + _capacity = c; + } + +//============================== end of file =============================== diff --git a/source/dcdt/se/sr_array.h b/source/dcdt/se/sr_array.h new file mode 100644 index 0000000000..8747d3e935 --- /dev/null +++ b/source/dcdt/se/sr_array.h @@ -0,0 +1,341 @@ + +# ifndef SR_ARRAY_H +# define SR_ARRAY_H + +/** \file sr_array.h + * fast resizeable array template */ + +# include "sr.h" +# include "sr_input.h" +# include "sr_output.h" + +/*! \class SrArrayBase sr_array.h + \brief Fast resizeable array base class + + This class is to be derived, and not to be directly used. See SrArray for + a user-ready class. All memory management functions of SrArrayBase were + written using quick memory block functions. In this way, malloc() and free() + functions are used. So that void pointers are used to refer to user's data. + Most methods need to know the size of each element that is the parameter + sizeofx appearing several times. */ +class SrArrayBase + { protected : + + void* _data; //!< Array pointer used for storage + int _size; //!< Number of elements being used in array + int _capacity; //!< Number of allocated elements (>=size) + + protected : + + /*! Init with the sizeof of each element, size, and capacity. If the given + capacity is smaller than the size, capacity is set to be equal to size */ + SrArrayBase ( sruint sizeofx, int s, int c ); + + /*! Copy constructor. Allocates and copies size elements. */ + SrArrayBase ( sruint sizeofx, const SrArrayBase& a ); + + /*! Constructor from a given buffer. No checkings are done, + its the user responsability to give consistent parameters. */ + SrArrayBase ( void* pt, int s, int c ); + + /*! Will free the internal buffer if needed. The internal size and capacity + are not adjusted, so that this method should be called only by the + destructor of the derived class, as SrArrayBase has not a destructor. */ + void free_data (); + + /*! Changes the size of the array. Reallocation is done only when the size + requested is greater than the current capacity, and in this case, + capacity becomes equal to the size. */ + void size ( unsigned sizeofx, int ns ); + + /*! Changes the capacity of the array. Reallocation is done whenever a new + capacity is requested. Internal memory is freed in case of 0 capacity. + The size is always kept inside [0,nc]. Parameter nc is considered 0 + if it is negative. */ + void capacity ( unsigned sizeofx, int nc ); + + /*! Returns a valid index as if the given index references a circular + array, ie, it returns index%size() for positive numbers. Negative + numbers are also correctly mapped. */ + int validate ( int index ) const; + + /*! Makes size==capacity, freeing all extra capacity if any. */ + void compress ( sruint sizeofx ); + + /*! Removes positions starting with pos i, and length n, moving all data + correctly. The parameters must be valid, no checkings are done. No + reallocation is done. */ + void remove ( sruint sizeofx, int i, int n ); + + /*! Inserts dp positions, starting at pos i, moving all data correctly. + Parameter i can be between 0 and size(), if i==size(), n positions + are appended. If reallocation is needed, the array capacity is + reallocated to contain two times the new size (after insertion). */ + void insert ( sruint sizeofx, int i, int n ); + + /*! Copies n entries from src position to dest position. Regions are + allowed to overlap. Uses the C function memmove. */ + void move ( sruint sizeofx, int dest, int src, int n ); + + /*! Sorts the array, with the compare function cmp, by calling the + system function qsort(). */ + void sort ( sruint sizeofx, srcompare cmp ); + + /*! Linear search, returns index of the element found, or -1 if not found */ + int lsearch ( sruint sizeofx, const void *x, srcompare cmp ) const; + + /*! Binary search for sorted arrays. Returns index of the element found, + or -1 if not found. If not found and pos is not null, pos will have + the position to insert the element keeping the array sorted. Faster + than the standard C library function bsearch() for large arrays. */ + int bsearch ( sruint sizeofx, const void *x, srcompare cmp, int *pos ) const; + + /*! Returns the position of the insertion, or -1 if not inserted. The + space of sizeofx is created and the index of the position is returned, + but get attention to the fact that the contents of x are not moved to + the inserted position. In this way, x can be only a key to the data + stored in the array. In case of duplication, the insertion is not done + if parameter allowdup is given as false. To insert the element position, + insert() method is called. */ + int insort ( sruint sizeofx, const void *x, srcompare cmp, bool allowdup ); + + /*! Copy from another array. SrArrayBase will be an exact copy of the given array, + allocating the same capacity, but copying only size elements. */ + void copy ( sruint sizeofx, const SrArrayBase& a ); + + /*! Returns the internal buffer pointer that will be null or contain the address of + the memory used and that was allocated with malloc(). The user will then be + responsible to free this allocated memory with free(). After this call, the + array becomes an empty valid array. */ + void* leave_data (); + + /*! Takes the data of the given array a, that will become an empty array. + SrArrayBase will have the same data that a had before. This is done + without reallocation. */ + void take_data ( SrArrayBase& a ); + + /*! Frees the current data of SrArrayBase, and then makes SrArrayBase to control + the given buffer pt, with size and capacity as given. */ + void take_data ( void* pt, int s, int c ); + }; + +/*! \class SrArray sr_array.h + \brief Fast resizeable dynamic array + + All memory management functions of SrArray use quick memory block functions + and so be aware that constructors and destructors of class X are not called. + SrArray can be used only with classes or structs that do not have any internal + allocated data, as SrArray will not respect them when resizing. Internally, + malloc(), realloc() and free() functions are used throught SrArrayBase methods. + Note that the array size is automatically reallocated when needed (with a double + size strategey), and so take care to not reference internal memory of SrArray + that can be reallocated. For example, the following code is wrong: a.push()=a[x], + because a[x] referentiates a memory space that can be reallocated by push() */ +template +class SrArray : protected SrArrayBase + { public: + + /*! Constructs with the given size and capacity. If the given capacity + is smaller than the size, capacity is set to be equal to size. */ + SrArray ( int s=0, int c=0 ) : SrArrayBase ( sizeof(X), s, c ) {} + + /*! For compatibility with prior versions we provide a constructor from + 3 ints, the last one is simply not considered. */ + SrArray ( int s, int c, int g ) : SrArrayBase ( sizeof(X), s, c ) {} + + /*! Copy constructor. SrArray will be an exact copy of the given array, + but allocating as capacity only the size of a. + Attention: the operator= that X might have is not called ! */ + SrArray ( const SrArray& a ) : SrArrayBase ( sizeof(X), a ) {} + + /*! Constructor from a given buffer. No checkings are done, its the user + responsability to give consistent parameters. */ + SrArray ( X* pt, int s, int c ) : SrArrayBase ( (void*)pt, s, c ) {} + + /*! Destructor frees the array calling the base class free_data() method. + Attention: elements' destructors are not called ! */ + ~SrArray () { SrArrayBase::free_data(); } + + /*! Returns true if the array has no elements, ie, size()==0; and false otherwise. */ + bool empty () const { return _size==0? true:false; } + + /*! Returns the capacity of the array. Capacity is used to be able to have a + larger storage buffer than the current size used. The method capacity() + will always return a value not smaller than size(). */ + int capacity () const { return _capacity; } + + /*! Returns the current size of the array. */ + int size () const { return _size; } + + /*! Changes the size of the array. Reallocation is done only when the size + requested is greater than the current capacity, and in this case, capacity + becomes equal to the size. */ + void size ( int ns ) { SrArrayBase::size(sizeof(X),ns); } + + /*! Changes the capacity of the array. Reallocation is done whenever a new + capacity is requested. Internal memory is freed in case of 0 capacity. + The size is always kept inside [0,nc]. Parameter nc is considered 0 + if it is negative. */ + void capacity ( int nc ) { SrArrayBase::capacity(sizeof(X),nc); } + + /*! Defines a minimum capacity to use, ie, sets the capacity to be c + iff the current capacity is lower than c */ + void ensure_capacity ( int c ) { if ( capacity()& a ) + { SrArrayBase::copy ( sizeof(X), a ); } + + /*! Revert the order of the elements in the array. Copy operator of X is used. */ + void revert () + { int i, max=size()-1, mid=size()/2; X tmp; + for ( i=0; i=0 ) ((X*)_data)[pos]=x; + return pos; + } + + /*! Standard library qsort() wrapper call. The compare function is required + as argument: int sr_compare(const X*,const X*) */ + void sort ( SR_COMPARE_FUNC ) { SrArrayBase::sort ( sizeof(X), (srcompare)sr_compare_func ); } + + /*! Linear search, returns the index of the element found, or -1 if not + found. A compare function is required as argument: + int sr_compare(const X*,const X*) */ + int lsearch ( const X& x, SR_COMPARE_FUNC ) const { return SrArrayBase::lsearch ( sizeof(X), (void*)&x, (srcompare)sr_compare_func ); } + + /*! Binary search for sorted arrays. Returns index of the element found, + or -1 if not found. If not found and pos is not null, pos will have the + position to insert the element keeping the array sorted. Faster than + the standard C library bsearch() for large arrays. A compare function + is required as argument: int sr_compare(const X*,const X*) */ + int bsearch ( const X& x, SR_COMPARE_FUNC, int *pos=NULL ) const + { return SrArrayBase::bsearch ( sizeof(X), (void*)&x, (srcompare)sr_compare_func, pos ); } + + /*! Returns the internal buffer pointer that will be null or contain the address of + the memory used and that was allocated with malloc(). The user will then be + responsible to free this allocated memory with free(). After this call, the + array becomes an empty valid array. */ + X* leave_data () { return (X*) SrArrayBase::leave_data(); } + + /*! Frees the data of SrArray, and then makes SrArray be the given array a. + After this, a is set to be a valid empty array. The data is moved without + reallocation. */ + void take_data ( SrArray& a ) { SrArrayBase::take_data ( (SrArrayBase&)a ); } + + /*! Frees the data of SrArray, and then makes SrArray to control the given + buffer pt, with size and capacity as given. Its the user reponsibility to + pass correct values. Note also that the memory menagement of SrArray is + done with malloc/realloc/free functions. */ + void take_data ( X* pt, int s, int c ) { SrArrayBase::take_data ( pt, s, c ); } + + /*! Output all elements of the array. Element type X must have its ouput operator << + available. The output format is [e0 e1 ... en]. */ + friend SrOutput& operator<< ( SrOutput& o, const SrArray& a ) + { int i; + o << '['; + for ( i=0; i> ( SrInput& in, SrArray& a ) + { a.size(0); + in.get_token(); + while (true) + { in.get_token(); + if ( in.last_token()[0]==']' ) break; + in.unget_token(); + a.push(); + in >> a.top(); + } + return in; + } + }; + +//============================== end of file =============================== + +#endif // SR_ARRAY_H + diff --git a/source/dcdt/se/sr_array_pt.cpp b/source/dcdt/se/sr_array_pt.cpp new file mode 100644 index 0000000000..2d9041e86c --- /dev/null +++ b/source/dcdt/se/sr_array_pt.cpp @@ -0,0 +1,202 @@ +#include "precompiled.h" +# include "sr_array_pt.h" + +//====================== SrArrayPtBase ========================== + +SrArrayPtBase::SrArrayPtBase ( SrClassManagerBase* m ) + { + _man = m; + _man->ref(); + } + +SrArrayPtBase::SrArrayPtBase ( const SrArrayPtBase& a ) + { + _man = a._man; + _man->ref(); + *this = a; // calls copy operator + } + +SrArrayPtBase::~SrArrayPtBase () + { + init (); + _man->unref(); // must be called after init()! + } + +void SrArrayPtBase::init () + { + while ( SrArray::size()>0 ) _man->free ( SrArray::pop() ); + } + +void SrArrayPtBase::size ( int ns ) + { + int i, s = SrArray::size(); + if ( ns>s ) + { SrArray::size(ns); + for ( i=s; i::set ( i, _man->alloc() ); + } + else if ( nsfree ( SrArray::get(i) ); + SrArray::size(ns); + } + } + +void SrArrayPtBase::capacity ( int nc ) + { + int i, s = SrArray::size(); + if ( nc<0 ) nc=0; + if ( ncfree ( SrArray::get(i) ); + } + SrArray::capacity(nc); + } + +void SrArrayPtBase::compress () + { + capacity ( size() ); + } + +void SrArrayPtBase::swap ( int i, int j ) + { + void *pti = SrArray::get(i); + void *ptj = SrArray::get(j); + SrArray::set ( i, ptj ); + SrArray::set ( j, pti ); + } + +void SrArrayPtBase::set ( int i, const void* pt ) + { + SR_ASSERT ( i>=0 && ifree ( SrArray::get(i) ); + SrArray::set ( i, _man->alloc(pt) ); + } + +void* SrArrayPtBase::get ( int i ) const + { + SR_ASSERT ( i>=0 && i::get(i); + } + +const void* SrArrayPtBase::const_get ( int i ) const + { + SR_ASSERT ( i>=0 && i::const_get(i); + } + +void* SrArrayPtBase::top () const + { + if ( size()==0 ) return 0; + return SrArray::get ( size()-1 ); + } + +void SrArrayPtBase::pop () + { + if ( size()>0 ) _man->free ( SrArray::pop() ); + } + +void SrArrayPtBase::push () + { + SrArray::push() = _man->alloc (); + } + +void SrArrayPtBase::insert ( int i, int dp ) + { + SrArray::insert ( i, dp ); + int j; + for ( j=0; j::set ( i+j, _man->alloc() ); + } + +void SrArrayPtBase::remove ( int i, int dp ) + { + int j; + for ( j=0; jfree( SrArray::get(i+j) ); + SrArray::remove ( i, dp ); + } + +void* SrArrayPtBase::extract ( int i ) + { + void *pt = SrArray::get(i); + SrArray::remove ( i ); + return pt; + } + +void SrArrayPtBase::operator = ( const SrArrayPtBase& a ) + { + init (); // deletes all data + SrArray::size ( a.size() ); + SrArray::compress(); + int i; + for ( i=0; i::set ( i, _man->alloc(a[i]) ); + } + +static SrClassManagerBase* StaticManager = 0; // This is not thread safe... + +static int fcmp ( const void* pt1, const void* pt2 ) + { + typedef const int* cint; + return StaticManager->compare( (const void*)*cint(pt1), (const void*)*cint(pt2) ); + } + +int SrArrayPtBase::insort ( const void* pt, bool allowdup ) + { + int pos; + pos = SrArray::insort ( (void *const&)pt, + (int(*)(void *const *,void *const *))fcmp, + allowdup ); + if ( pos>=0 ) SrArray::get(pos) = _man->alloc(pt); + return pos; + } + +void SrArrayPtBase::sort () + { + StaticManager = _man; + SrArray::sort ( (int(*)(void *const *,void *const *))fcmp ); + } + +int SrArrayPtBase::lsearch ( const void* pt ) const + { + StaticManager = _man; + return SrArray::lsearch ( (void *const&)pt, (int(*)(void *const *,void *const *))fcmp ); + } + +int SrArrayPtBase::bsearch ( const void* pt, int *pos ) + { + StaticManager = _man; + return SrArray::bsearch ( (void *const&)pt, (int(*)(void *const *,void *const *))fcmp, pos ); + } + +void SrArrayPtBase::take_data ( SrArrayPtBase& a ) + { + size(0); + capacity(0); + SrArray::take_data(a); + } + +SrOutput& operator<< ( SrOutput& o, const SrArrayPtBase& a ) + { + int i, m; + m = a.size()-1; + o << '['; + for ( i=0; i<=m; i++ ) + { a._man->output ( o, a[i] ); + if ( i> ( SrInput& in, SrArrayPtBase& a ) + { + a.size(0); + in.get_token(); + while (true) + { in.get_token(); + if ( in.last_token()[0]==']' ) break; + in.unget_token (); + a.push (); + a._man->input ( in, a.top() ); + } + return in; + } + +//=========================== EOF =============================== diff --git a/source/dcdt/se/sr_array_pt.h b/source/dcdt/se/sr_array_pt.h new file mode 100644 index 0000000000..3c4215b3e6 --- /dev/null +++ b/source/dcdt/se/sr_array_pt.h @@ -0,0 +1,169 @@ + +# ifndef SR_ARRAY_PT_H +# define SR_ARRAY_PT_H + +/** \file sr_array_pt.h + * resizeable array of class pointers */ + +# include "sr_class_manager.h" +# include "sr_array.h" + +/*! \class SrArrayPtBase sr_array_pt.h + \brief resizeable array of class pointers + + SrArrayPtBase implements methods for managing a resizeable array + of pointers. The user should however use the template + class SrArrayPt for an implementation that includes automatic + type casts for the user types. A manager to the user + data is required, see sr_class_manager.h */ +class SrArrayPtBase : private SrArray + { private : + SrClassManagerBase* _man; + + public : + + /*! Initiates an empty array. The class manager is required. */ + SrArrayPtBase ( SrClassManagerBase* m ); + + /*! Copy constructor. The class manager of a is shared. */ + SrArrayPtBase ( const SrArrayPtBase& a ); + + /*! Destructor */ + ~SrArrayPtBase (); + + /*! Returns true if the array has no elements, and false otherwise. */ + bool empty () const { return SrArray::empty(); } + + /*! Returns the capacity of the array. */ + int capacity () const { return SrArray::capacity(); } + + /*! Returns the current size of the array. */ + int size () const { return SrArray::size(); } + + /*! Makes the array empty; equivalent to size(0) */ + void init (); + + /*! Changes the size of the array, filling new objects in the new positions. */ + void size ( int ns ); + + /*! Changes the capacity of the array. */ + void capacity ( int nc ); + + /*! Makes capacity to be equal to size. */ + void compress (); + + /*! Swaps the pointers of position i and j, that must be valid positions. */ + void swap ( int i, int j ); + + /*! Returns a valid index as if the given index references a circular + array, ie, it returns index%size() for positive numbers. Negative + numbers are also correctly mapped. */ + int validate ( int index ) const { SrArray::validate(index); } + + /*! deletes element i and reallocates a new one as a copy of pt */ + void set ( int i, const void* pt ); + + /*! returns a pointer to the object in position i. */ + void* get ( int i ) const; + + /*! Returns a const pointer to the object in position i. */ + const void* const_get ( int i ) const; + + /*! Returns a pointer to the last element or 0 if the array is empty*/ + void* top () const; + + /*! Pop and frees element size-1 if the array is not empty */ + void pop (); + + /*! Allocates and appends one empty element */ + void push (); + + /*! Inserts n positions, starting at pos i, and putting a new element in + each new position created. */ + void insert ( int i, int n=1 ); + + /*! Removes n positions starting from pos i */ + void remove ( int i, int n=1 ); + + /*! Extract (without deletion) and returns the pointer at position i */ + void* extract ( int i ); + + /*! Copy operator */ + void operator = ( const SrArrayPtBase& a ); + + /*! Inserts one object, considering the array is sorted. Returns the + inserted position, or -1 if duplication occurs and allowdup is false. + (Note: all methods using sr_compare functions are not thread safe, as + they use a static pointer to set the current comparison function) */ + int insort ( const void* pt, bool allowdup=true ); + + /*! Sort array */ + void sort (); + + /*! Linear search. Returns index of the element found, + or -1 if not found. */ + int lsearch ( const void* pt ) const; + + /*! Binary search for sorted arrays. Returns index of the element found, + or -1 if not found. If not found and pos is not 0, pos will have the + position to insert the element keeping the array sorted. */ + int bsearch ( const void* pt, int *pos=NULL ); + + /*! Frees all data, and then makes SrArrayPtBase be the given array a. + After this, a is set to be a valid empty array. The data is moved without + reallocation. */ + void take_data ( SrArrayPtBase& a ); + + /*! Outputs all elements of the array in format is [e0 e1 ... en]. */ + friend SrOutput& operator<< ( SrOutput& o, const SrArrayPtBase& a ); + + /*! Inputs elements in format is [e0 e1 ... en]. */ + friend SrInput& operator>> ( SrInput& in, SrArrayPtBase& a ); + }; + +/*! \class SrArrayPt sr_array_pt.h + \brief resizeable array of class pointers + + SrArrayPtBase implements methods for managing a resizeable array + of pointers, which are managed by SrClassManager object. */ +template +class SrArrayPt : public SrArrayPtBase + { public : + /*! Default constructor that automatically creates a SrClassManager. */ + SrArrayPt () : SrArrayPtBase ( new SrClassManager ) {} + + /*! Constructor with a given class manager. */ + SrArrayPt ( SrClassManagerBase* m ) : SrArrayPtBase ( m ) {} + + /*! Copy constructor sharing class manager. */ + SrArrayPt ( const SrArrayPt& a ) : SrArrayPtBase ( a ) {} + + void set ( int i, const X& x ) { SrArrayPtBase::set(i,(const void*)&x); } + + X* get ( int i ) { return (X*)SrArrayPtBase::get(i); } + const X* const_get ( int i ) const { return (const X*)SrArrayPtBase::const_get(i); } + + X* operator[] ( int i ) { return get(i); } + + /*! Returns a pointer to the last element or 0 if the array is empty*/ + X* top () const { return (X*)SrArrayPtBase::top(); } + + /*! Pop and frees element size-1 if the array is not empty */ + void pop () { SrArrayPtBase::pop(); } + + /*! Allocates and appends one empty element */ + void push () { SrArrayPtBase::push(); } + + /*! Allocates and appends one element using copy operator */ + void push ( const X& x ) { push(); *top()=x; } + + /*! Allocates and insert one element using copy operator */ + void insert ( int i, const X& x ) { insert(i,1); *get(i)=x; } + + /*! Extract (without deletion) and returns the pointer at position i */ + X* extract ( int i ) { return (X*) SrArrayPtBase::extract(i); } + }; + +//============================== end of file =============================== + +#endif // SR_ARRAY_PT_H diff --git a/source/dcdt/se/sr_box.cpp b/source/dcdt/se/sr_box.cpp new file mode 100644 index 0000000000..7be61f199c --- /dev/null +++ b/source/dcdt/se/sr_box.cpp @@ -0,0 +1,193 @@ +#include "precompiled.h" + +# include "sr_box.h" +# include "sr_mat.h" + +//======================================== SrBox ======================================= + +const char* SrBox::class_name = "Box"; + +SrBox::SrBox ( const SrBox& x, const SrBox& y ) + : a ( SR_MIN(x.a.x,y.a.x), SR_MIN(x.a.y,y.a.y), SR_MIN(x.a.z,y.a.z) ), + b ( SR_MAX(x.b.x,y.b.x), SR_MAX(x.b.y,y.b.y), SR_MAX(x.b.z,y.b.z) ) + { + } + +bool SrBox::empty () const + { + return a.x>b.x || a.y>b.y || a.z>b.z ? true:false; + } + +float SrBox::volume () const + { + return (b.x-a.x) * (b.y-a.y) * (b.z-a.z); + } + +SrVec SrBox::center () const + { + return (a+b)/2.0f; // == a + (b-a)/2 == a + b/2 - a/2 + } + +void SrBox::center ( const SrVec& p ) + { + (*this) += p-center(); + } + +void SrBox::size ( const SrVec& v ) + { + b = a+v; + } + +SrVec SrBox::size () const + { + return b-a; + } + +float SrBox::max_size () const + { + SrVec s = b-a; + return SR_MAX3(s.x,s.y,s.z); + } + +float SrBox::min_size () const + { + SrVec s = b-a; + return SR_MIN3(s.x,s.y,s.z); + } + +void SrBox::extend ( const SrPnt &p ) + { + if ( empty() ) { a=p; b=p; } + SR_UPDMIN ( a.x, p.x ); SR_UPDMAX ( b.x, p.x ); + SR_UPDMIN ( a.y, p.y ); SR_UPDMAX ( b.y, p.y ); + SR_UPDMIN ( a.z, p.z ); SR_UPDMAX ( b.z, p.z ); + } + +void SrBox::extend ( const SrBox &box ) + { + if ( empty() ) *this=box; + if ( box.empty() ) return; + SR_UPDMIN ( a.x, box.a.x ); SR_UPDMAX ( b.x, box.b.x ); + SR_UPDMIN ( a.y, box.a.y ); SR_UPDMAX ( b.y, box.b.y ); + SR_UPDMIN ( a.z, box.a.z ); SR_UPDMAX ( b.z, box.b.z ); + } + +void SrBox::grows ( float dx, float dy, float dz ) + { + a.x-=dx; a.y-=dy; a.z-=dz; + b.x+=dx; b.y+=dy; b.z+=dz; + } + +bool SrBox::contains ( const SrVec& p ) const + { + return p.xb.x || p.y>b.y || p.z>b.z ? false : true; + } + +bool SrBox::intersects ( const SrBox& box ) const + { + if ( box.contains(a) ) return true; + if ( box.contains(b) ) return true; + SrVec x(a.x,a.y,b.z); if ( box.contains(x) ) return true; + x.set (a.x,b.y,a.z); if ( box.contains(x) ) return true; + x.set (b.x,a.y,a.z); if ( box.contains(x) ) return true; + x.set (b.x,b.y,a.z); if ( box.contains(x) ) return true; + x.set (b.x,a.y,b.z); if ( box.contains(x) ) return true; + x.set (a.x,b.y,b.z); if ( box.contains(x) ) return true; + return false; + } + +void SrBox::get_side ( SrPnt& p1, SrPnt& p2, SrPnt& p3, SrPnt& p4, int s ) const + { + switch (s) + { case 0 : p1.set ( a.x, a.y, a.z ); + p2.set ( a.x, a.y, b.z ); + p3.set ( a.x, b.y, b.z ); + p4.set ( a.x, b.y, a.z ); + break; + case 1 : p1.set ( b.x, a.y, a.z ); + p2.set ( b.x, b.y, a.z ); + p3.set ( b.x, b.y, b.z ); + p4.set ( b.x, a.y, b.z ); + break; + case 2 : p1.set ( a.x, a.y, a.z ); + p2.set ( b.x, a.y, a.z ); + p3.set ( b.x, a.y, b.z ); + p4.set ( a.x, a.y, b.z ); + break; + case 3 : p1.set ( a.x, b.y, a.z ); + p2.set ( a.x, b.y, b.z ); + p3.set ( b.x, b.y, b.z ); + p4.set ( b.x, b.y, a.z ); + break; + case 4 : p1.set ( a.x, a.y, a.z ); + p2.set ( a.x, b.y, a.z ); + p3.set ( b.x, b.y, a.z ); + p4.set ( b.x, a.y, a.z ); + break; + case 5 : p1.set ( a.x, a.y, b.z ); + p2.set ( b.x, a.y, b.z ); + p3.set ( b.x, b.y, b.z ); + p4.set ( a.x, b.y, b.z ); + break; + } + } + +void SrBox::operator += ( const SrVec& v ) + { + a += v; + b += v; + } + +void SrBox::operator *= ( float s ) + { + a *= s; + b *= s; + } + +//============================== friends ======================================== + +SrBox operator * ( const SrBox& b, const SrMat& m ) + { + SrBox x; // init as an empty box + + if ( b.empty() ) return x; + + SrVec v(b.a); x.extend(v*m); + v.x=b.b.x; x.extend(v*m); + v.y=b.b.y; x.extend(v*m); + v.x=b.a.x; x.extend(v*m); + v.z=b.b.z; x.extend(v*m); + v.x=b.b.x; x.extend(v*m); + v.y=b.a.y; x.extend(v*m); + v.x=b.a.x; x.extend(v*m); + return x; + } + +SrBox operator * ( const SrMat& m, const SrBox& b ) + { + SrBox x; // init as an empty box + + if ( b.empty() ) return x; + + SrVec v(b.a); x.extend(m*v); + v.x=b.b.x; x.extend(m*v); + v.y=b.b.y; x.extend(m*v); + v.x=b.a.x; x.extend(m*v); + v.z=b.b.z; x.extend(m*v); + v.x=b.b.x; x.extend(m*v); + v.y=b.a.y; x.extend(m*v); + v.x=b.a.x; x.extend(m*v); + return x; + } + +SrOutput& operator<< ( SrOutput& o, const SrBox& box ) + { + return o << box.a << ' ' << box.b; + } + +SrInput& operator>> ( SrInput& in, SrBox& box ) + { + return in >> box.a >> box.b; + } + +//================================ End of File ================================================= diff --git a/source/dcdt/se/sr_box.h b/source/dcdt/se/sr_box.h new file mode 100644 index 0000000000..301bd0d797 --- /dev/null +++ b/source/dcdt/se/sr_box.h @@ -0,0 +1,132 @@ + +# ifndef SR_BOX_H +# define SR_BOX_H + +/** \file sr_box.h + * 3d axis-aligned box + */ + +# include "sr_vec.h" + +class SrMat; + +/*! \class SrBox sr_box.h + \brief 3d axis-aligned box + + SrBox describes a 3d axis-aligned box. The box is described by + two 3d vertices a,b; one with the minimum coordinates(a), and the + other with the maximum coordinates(b). It is used to describe + bounding boxes, which have their sides parallel to the axes. + If any of the coordinates of a are greater than any coordinates + of b, the box is said to be empty, ie, not valid. */ +class SrBox + { public : + SrPnt a; //!< Contains the minimum coordinates of the box + SrPnt b; //!< Contains the maximum coordinates of the box + static const char* class_name; + public : + + /*! Default constructor initializes the box as the empty box (1,1,1)(0,0,0). */ + SrBox () : a(SrPnt::one), b(SrPnt::null) {} + + /*! Constructs a box with all vertices the same. This degenerated + box is identical to a single point and is not considered an + empty box. */ + SrBox ( const SrPnt& p ) : a(p), b(p) {} + + /*! Constructs the box from the given min and max points. */ + SrBox ( const SrPnt& min, const SrPnt& max ) : a(min), b(max) {} + + /*! Copy constructor. */ + SrBox ( const SrBox& box ) : a(box.a), b(box.b) {} + + /* Constructs SrBox containing the two given boxes. */ + SrBox ( const SrBox& x, const SrBox& y ); + + /*! Init the box as (0,0,0)(0,0,0). */ + void set_null () { a=SrPnt::null; b=SrPnt::null; } + + /*! Sets the minimum and maximum vertices of the box. */ + void set ( const SrPnt& min, const SrPnt& max ) { a=min; b=max; } + + /*! Sets the box to be empty, ie, invalid, just by putting + the x coordinate of the minimum vertex (a) greater than + the x coordinate of the maximum vertex (b). */ + void set_empty () { a.x=1.0; b.x=0.0; } + + /*! Returns true if the box is empty (or invalid), ie, when + some coordinate of a is greater than b. */ + bool empty () const; + + /*! Returns the volume of the box. */ + float volume () const; + + /*! Returns the center point of the box (b+a)/2. */ + SrPnt center () const; + + /*! Translates SrBox to have its center in p. */ + void center ( const SrPnt& p ); + + /*! Changes the position of the maximum vertex b of the box in order to + achieve the desired dimensions given in v (b=a+v). */ + void size ( const SrVec& v ); + + /*! Returns the dimensions in each axis (b-a). */ + SrVec size () const; + + /*! Returns the maximum dimension of the box. */ + float max_size () const; + + /*! Returns the minimum dimension of the box. */ + float min_size () const; + + /*! Extends SrBox (if needed) to contain the given point. If SrBox + is empty, SrBox min and max vertices become the given point. */ + void extend ( const SrPnt& p ); + + /*! Extends SrBox (if needed) to contain the given box, if the given + box is not empty(). If SrBox is empty, SrBox becomes the given box. */ + void extend ( const SrBox& box ); + + /*! Adds (dx,dy,dz) to b, and diminish it from a. */ + void grows ( float dx, float dy, float dz ); + + /*! Returns true if SrBox contains the given point. */ + bool contains ( const SrPnt& p ) const; + + /*! Returns true if SrBox intersects with the given box. */ + bool intersects ( const SrBox& box ) const; + + /*! Returns the four corners of side s={0,..,5} of the box. + Side 0 has all x coordinates equal to a.x, side 1 equal to b.x. + Side 2 has all y coordinates equal to a.y, side 3 equal to b.y. + Side 4 has all z coordinates equal to a.z, side 5 equal to b.z. + Order is ccw, starting with the point with more SrBox::a coordinates */ + void get_side ( SrPnt& p1, SrPnt& p2, SrPnt& p3, SrPnt& p4, int s ) const; + + /*! The bounding box is identical to SrBox (needed by SrSceneShapeTpl). */ + void get_bounding_box ( SrBox &box ) const { box=*this; } + + /* Translates SrBox by v. */ + void operator += ( const SrVec& v ); + + /* Scales SrBox by the factor s. */ + void operator *= ( float s ); + + /* Returns the bounding box of the transformed vertices vM of b. */ + friend SrBox operator * ( const SrBox& b, const SrMat& m ); + + /* Returns the bounding box of the transformed vertices Mv of b. */ + friend SrBox operator * ( const SrMat& m, const SrBox& b ); + + /*! Outputs in format: "x y z a b c". */ + friend SrOutput& operator<< ( SrOutput& o, const SrBox& box ); + + /*! Inputs from format: "x y z a b c". */ + friend SrInput& operator>> ( SrInput& in, SrBox& box ); + }; + +//================================ End of File ================================================= + +# endif // SR_BOX_H + diff --git a/source/dcdt/se/sr_buffer.cpp b/source/dcdt/se/sr_buffer.cpp new file mode 100644 index 0000000000..43f19311e1 --- /dev/null +++ b/source/dcdt/se/sr_buffer.cpp @@ -0,0 +1,63 @@ +#include "precompiled.h" +# include +# include + +# include "sr_buffer.h" + +//=== SrBuffer ======================================================================= + +# define BUF(i) ((char*)buffer)+(sizeofx*(i)) + +void* sr_buffer_size ( void* buffer, int sizeofx, int& size, int newsize ) + { + if ( size==newsize ) return buffer; + size = newsize; + if ( size==0 ) + { if (buffer) { free(buffer); buffer=0; } } + else + { buffer = realloc ( buffer, (size_t)(sizeofx*size) ); } + return buffer; + } + +void* sr_buffer_insert ( void* buffer, int sizeofx, int& size, int i, int dp ) + { + size += dp; + buffer = realloc ( buffer, (size_t)(sizeofx*size) ); + if ( i +class SrBuffer + { private: + void* _data; + int _size; + public: + + /*! Default constructor. */ + SrBuffer () : _data(0), _size(0) {} + + /*! Copy constructor. */ + SrBuffer ( const SrBuffer& b ) : _data(0), _size(0) + { _data=sr_buffer_copy(_data,sizeof(X),_size,b._data,b._size); } + + /*! Constructor with a given size. */ + SrBuffer ( int s ) : _data(0), _size(0) { _data=sr_buffer_size(_data,sizeof(X),_size,s); } + + /*! Constructor from a user allocated buffer. See also leave_data(). */ + SrBuffer ( X* pt, int s ) : _data(pt), _size(s) {} + + /*! Destructor frees the buffer. Elements' destructors are not called ! */ + ~SrBuffer () { sr_buffer_size ( _data, sizeof(X), _size, 0 ); } + + /*! Returns true if size()==0; and false otherwise. */ + bool empty () const { return _size==0? true:false; } + + /*! Returns the current size of the Buffer. */ + int size () const { return _size; } + + /*! Allows to change the size of the buffer. */ + void size ( int ns ) + { _data=sr_buffer_size(_data,sizeof(X),_size,ns); } + + /*! Sets all elements as x, copying each element using operator = */ + void setall ( const X& x ) + { for ( int i=0; i<_size; i++ ) ((X*)_data)[i]=x; } + + /*! Get a const reference to the element of index i. Indices start from 0 + and must be smaller than size(). No checkings are done to ensure that + i is in a valid range. */ + const X& const_get ( int i ) const { return ((X*)_data)[i]; } + + /*! Get a reference to the element of index i. Indices start from 0 + and must be smaller than size(). No checkings are done to ensure that + i is in a valid range. */ + X& get ( int i ) { return ((X*)_data)[i]; } + + /*! Sets an element. Operator = is used here. Indices start from 0 and must + be smaller than size(). No checkings are done to ensure that i is valid. */ + void set ( int i, const X& x ) { ((X*)_data)[i]=x; } + + /*! Operator version of X& get(int i), but returning a non const reference. + No checkings are done to ensure that i is valid. */ + X& operator[] ( int i ) { return ((X*)_data)[i]; } + + /*! Returns a const pointer of the internal buffer. The internal buffer + will always contain a contigous storage space of size() elements. + See also take_data() and leave_data() methods. */ + operator const X* () const { return (X*)_data; } + + /*! Returns a reference to the last element, ie, with index size()-1. + The Buffer must not be empty when calling this method. */ + X& top () { return ((X*)_data)[_size-1]; } + + /* Reduces the size of the buffer by one. */ + void pop () { _data=sr_buffer_size(_data,sizeof(X),_size,_size-1); } + + /* Increases the size of the buffer by one. */ + void push ( int dp=1 ) { _data=sr_buffer_size(_data,sizeof(X),_size,_size+1); } + + /*! Inserts dp positions, starting at pos i, moving all data correctly. + Parameter i can be between 0 and size(), if i==size(), dp positions are + appended. */ + void insert ( int i, int dp=1 ) { _data=sr_buffer_insert(_data,sizeof(X),_size,i,dp); } + + /*! Removes dp positions starting from pos i, moving all data correctly; + dp has a default value of 1. Attention: elements' destructors are not + called ! */ + void remove ( int i, int dp=1 ) { _data=sr_buffer_remove(_data,sizeof(X),_size,i,dp); } + + /*! Copies all internal data of a to SrBuffer, with fast memcpy() functions, + so that the operator=() that X might have is not used. This method has no + effect if a "self copy" is called. */ + void operator = ( const SrBuffer& b ) + { _data=sr_buffer_copy(_data,sizeof(X),_size,b._data,b._size); } + + /*! Makes the given xpt pointer to point to the internal buffer, without + reallocation; xpt will then be null or contain the address of the memory + used and that was allocated with malloc(). The user will then be responsible + to free this allocated memory with free(). After this call, SrBuffer will + become an empty but valid buffer. */ + void leave_data ( X*& xpt, int& size ) + { xpt=_data; size=_size; _data=0; _size=0; } + + /*! Frees the data of SrBuffer, and then makes SrBuffer be the given buffer b. + After this, b is set to be a valid empty buffer. The data is moved without + reallocation. */ + void take_data ( SrBuffer& b ) { size(0); b.leave_data(_data,_size); } + + /*! Outputs all elements of the Buffer. Element type X must have its ouput operator << + available. This is the only method of SrBuffer that is not inline. */ + friend SrOutput& operator<< ( SrOutput& o, const SrBuffer& b ) + { o << '['; + int i; + for ( i=0; i> ( SrInput& in, SrBuffer& b ) + { int s=0; + b.size(128); + in.get_token(); + + while (true) + { in.get_token(); + if ( in.last_token()[0]==']' ) break; + in.unget_token(); + in >> b[s]; + s++; + if ( s>=b.size() ) b.size ( b.size()+128 ); + } + + b.size ( s ); + + return in; + } + }; + +//============================== end of file =============================== + +#endif // SR_BUFFER_H + diff --git a/source/dcdt/se/sr_bv.cpp b/source/dcdt/se/sr_bv.cpp new file mode 100644 index 0000000000..8af8c94293 --- /dev/null +++ b/source/dcdt/se/sr_bv.cpp @@ -0,0 +1,1176 @@ +#include "precompiled.h" +/* Note: this code was adapted from the PQP library; + their copyright notice can be found at the end of this file. */ + +# include + +# include "sr_bv_tree.h" + +//=========================== +//===== static MaxOfTwo ===== +//=========================== +static inline srbvreal MaxOfTwo ( srbvreal a, srbvreal b ) +{ + if (a > b) return a; + return b; +} + +//=========================== +//======= SrBv::size ======== +//=========================== +srbvreal SrBv::size() const + { + return sqrt(l[0]*l[0] + l[1]*l[1]) + srbvreal(2)*r; + } + +//=========================== +//======= SrBv::fit ========= +//=========================== +void SrBv::fit ( srbvmat O, SrBvTri* tris, int num_tris ) + { + using namespace SrBvMath; + + // store orientation + McM(R,O); + + // project points of tris to R coordinates + int num_points = 3*num_tris; + srbvreal (*P)[3] = new srbvreal[num_points][3]; + int point = 0; + int i; + for (i = 0; i < num_tris; i++) + { + MTxV(P[point],R,tris[i].p1); + point++; + + MTxV(P[point],R,tris[i].p2); + point++; + + MTxV(P[point],R,tris[i].p3); + point++; + } + + srbvreal minx, maxx, miny, maxy, minz, maxz, c[3]; + + // OBB + minx = maxx = P[0][0]; + miny = maxy = P[0][1]; + minz = maxz = P[0][2]; + for (i = 1; i < num_points; i++) + { + if (P[i][0] < minx) minx = P[i][0]; + else if (P[i][0] > maxx) maxx = P[i][0]; + if (P[i][1] < miny) miny = P[i][1]; + else if (P[i][1] > maxy) maxy = P[i][1]; + if (P[i][2] < minz) minz = P[i][2]; + else if (P[i][2] > maxz) maxz = P[i][2]; + } + c[0] = (srbvreal)0.5*(maxx + minx); + c[1] = (srbvreal)0.5*(maxy + miny); + c[2] = (srbvreal)0.5*(maxz + minz); + MxV(To,R,c); + + d[0] = (srbvreal)0.5*(maxx - minx); + d[1] = (srbvreal)0.5*(maxy - miny); + d[2] = (srbvreal)0.5*(maxz - minz); + + // RSS + // compute thickness, which determines radius, and z of rectangle corner + srbvreal cz,radsqr; + minz = maxz = P[0][2]; + for (i = 1; i < num_points; i++) + { + if (P[i][2] < minz) minz = P[i][2]; + else if (P[i][2] > maxz) maxz = P[i][2]; + } + r = (srbvreal)0.5*(maxz - minz); + radsqr = r*r; + cz = (srbvreal)0.5*(maxz + minz); + + // compute an initial length of rectangle along x direction + // find minx and maxx as starting points + int minindex, maxindex; + minindex = maxindex = 0; + for (i = 1; i < num_points; i++) + { + if (P[i][0] < P[minindex][0]) minindex = i; + else if (P[i][0] > P[maxindex][0]) maxindex = i; + } + srbvreal x, dz; + dz = P[minindex][2] - cz; + minx = P[minindex][0] + sqrt(MaxOfTwo(radsqr - dz*dz,0)); + dz = P[maxindex][2] - cz; + maxx = P[maxindex][0] - sqrt(MaxOfTwo(radsqr - dz*dz,0)); + + // grow minx + for (i = 0; i < num_points; i++) + { + if (P[i][0] < minx) + { + dz = P[i][2] - cz; + x = P[i][0] + sqrt(MaxOfTwo(radsqr - dz*dz,0)); + if (x < minx) minx = x; + } + } + + // grow maxx + for (i = 0; i < num_points; i++) + { + if (P[i][0] > maxx) + { + dz = P[i][2] - cz; + x = P[i][0] - sqrt(MaxOfTwo(radsqr - dz*dz,0)); + if (x > maxx) maxx = x; + } + } + + // compute an initial length of rectangle along y direction + // find miny and maxy as starting points + minindex = maxindex = 0; + for (i = 1; i < num_points; i++) + { + if (P[i][1] < P[minindex][1]) minindex = i; + else if (P[i][1] > P[maxindex][1]) maxindex = i; + } + srbvreal y; + dz = P[minindex][2] - cz; + miny = P[minindex][1] + sqrt(MaxOfTwo(radsqr - dz*dz,0)); + dz = P[maxindex][2] - cz; + maxy = P[maxindex][1] - sqrt(MaxOfTwo(radsqr - dz*dz,0)); + + // grow miny + for (i = 0; i < num_points; i++) + { + if (P[i][1] < miny) + { + dz = P[i][2] - cz; + y = P[i][1] + sqrt(MaxOfTwo(radsqr - dz*dz,0)); + if (y < miny) miny = y; + } + } + + // grow maxy + for (i = 0; i < num_points; i++) + { + if (P[i][1] > maxy) + { + dz = P[i][2] - cz; + y = P[i][1] - sqrt(MaxOfTwo(radsqr - dz*dz,0)); + if (y > maxy) maxy = y; + } + } + + // corners may have some points which are not covered - grow lengths if + // necessary + srbvreal dx, dy, u, t; + srbvreal a = sqrt((srbvreal)0.5); + for (i = 0; i < num_points; i++) + { + if (P[i][0] > maxx) + { + if (P[i][1] > maxy) + { + dx = P[i][0] - maxx; + dy = P[i][1] - maxy; + u = dx*a + dy*a; + t = (a*u - dx)*(a*u - dx) + + (a*u - dy)*(a*u - dy) + + (cz - P[i][2])*(cz - P[i][2]); + u = u - sqrt(MaxOfTwo(radsqr - t,0)); + if (u > 0) + { + maxx += u*a; + maxy += u*a; + } + } + else if (P[i][1] < miny) + { + dx = P[i][0] - maxx; + dy = P[i][1] - miny; + u = dx*a - dy*a; + t = (a*u - dx)*(a*u - dx) + + (-a*u - dy)*(-a*u - dy) + + (cz - P[i][2])*(cz - P[i][2]); + u = u - sqrt(MaxOfTwo(radsqr - t,0)); + if (u > 0) + { + maxx += u*a; + miny -= u*a; + } + } + } + else if (P[i][0] < minx) + { + if (P[i][1] > maxy) + { + dx = P[i][0] - minx; + dy = P[i][1] - maxy; + u = dy*a - dx*a; + t = (-a*u - dx)*(-a*u - dx) + + (a*u - dy)*(a*u - dy) + + (cz - P[i][2])*(cz - P[i][2]); + u = u - sqrt(MaxOfTwo(radsqr - t,0)); + if (u > 0) + { + minx -= u*a; + maxy += u*a; + } + } + else if (P[i][1] < miny) + { + dx = P[i][0] - minx; + dy = P[i][1] - miny; + u = -dx*a - dy*a; + t = (-a*u - dx)*(-a*u - dx) + + (-a*u - dy)*(-a*u - dy) + + (cz - P[i][2])*(cz - P[i][2]); + u = u - sqrt(MaxOfTwo(radsqr - t,0)); + if (u > 0) + { + minx -= u*a; + miny -= u*a; + } + } + } + } + + c[0] = minx; + c[1] = miny; + c[2] = cz; + MxV(Tr,R,c); + + l[0] = maxx - minx; + if (l[0] < 0) l[0] = 0; + l[1] = maxy - miny; + if (l[1] < 0) l[1] = 0; + + delete [] P; +} + +// int +// obb_disjoint(srbvreal B[3][3], srbvreal T[3], srbvreal a[3], srbvreal b[3]); +// +// This is a test between two boxes, box A and box B. It is assumed that +// the coordinate system is aligned and centered on box A. The 3x3 +// matrix B specifies box B's orientation with respect to box A. +// Specifically, the columns of B are the basis vectors (axis vectors) of +// box B. The center of box B is located at the vector T. The +// dimensions of box B are given in the array b. The orientation and +// placement of box A, in this coordinate system, are the identity matrix +// and zero vector, respectively, so they need not be specified. The +// dimensions of box A are given in array a. + +inline int +obb_disjoint(srbvreal B[3][3], srbvreal T[3], const srbvvec a, const srbvvec b ) +{ + register srbvreal t, s; + register int r; + srbvreal Bf[3][3]; + const srbvreal reps = (srbvreal)1e-6; + + // Bf = fabs(B) + Bf[0][0] = SR_ABS(B[0][0]); Bf[0][0] += reps; + Bf[0][1] = SR_ABS(B[0][1]); Bf[0][1] += reps; + Bf[0][2] = SR_ABS(B[0][2]); Bf[0][2] += reps; + Bf[1][0] = SR_ABS(B[1][0]); Bf[1][0] += reps; + Bf[1][1] = SR_ABS(B[1][1]); Bf[1][1] += reps; + Bf[1][2] = SR_ABS(B[1][2]); Bf[1][2] += reps; + Bf[2][0] = SR_ABS(B[2][0]); Bf[2][0] += reps; + Bf[2][1] = SR_ABS(B[2][1]); Bf[2][1] += reps; + Bf[2][2] = SR_ABS(B[2][2]); Bf[2][2] += reps; + + // if any of these tests are one-sided, then the polyhedra are disjoint + r = 1; + + // A1 x A2 = A0 + t = SR_ABS(T[0]); + + r &= (t <= + (a[0] + b[0] * Bf[0][0] + b[1] * Bf[0][1] + b[2] * Bf[0][2])); + if (!r) return 1; + + // B1 x B2 = B0 + s = T[0]*B[0][0] + T[1]*B[1][0] + T[2]*B[2][0]; + t = SR_ABS(s); + + r &= ( t <= + (b[0] + a[0] * Bf[0][0] + a[1] * Bf[1][0] + a[2] * Bf[2][0])); + if (!r) return 2; + + // A2 x A0 = A1 + t = SR_ABS(T[1]); + + r &= ( t <= + (a[1] + b[0] * Bf[1][0] + b[1] * Bf[1][1] + b[2] * Bf[1][2])); + if (!r) return 3; + + // A0 x A1 = A2 + t = SR_ABS(T[2]); + + r &= ( t <= + (a[2] + b[0] * Bf[2][0] + b[1] * Bf[2][1] + b[2] * Bf[2][2])); + if (!r) return 4; + + // B2 x B0 = B1 + s = T[0]*B[0][1] + T[1]*B[1][1] + T[2]*B[2][1]; + t = SR_ABS(s); + + r &= ( t <= + (b[1] + a[0] * Bf[0][1] + a[1] * Bf[1][1] + a[2] * Bf[2][1])); + if (!r) return 5; + + // B0 x B1 = B2 + s = T[0]*B[0][2] + T[1]*B[1][2] + T[2]*B[2][2]; + t = SR_ABS(s); + + r &= ( t <= + (b[2] + a[0] * Bf[0][2] + a[1] * Bf[1][2] + a[2] * Bf[2][2])); + if (!r) return 6; + + // A0 x B0 + s = T[2] * B[1][0] - T[1] * B[2][0]; + t = SR_ABS(s); + + r &= ( t <= + (a[1] * Bf[2][0] + a[2] * Bf[1][0] + + b[1] * Bf[0][2] + b[2] * Bf[0][1])); + if (!r) return 7; + + // A0 x B1 + s = T[2] * B[1][1] - T[1] * B[2][1]; + t = SR_ABS(s); + + r &= ( t <= + (a[1] * Bf[2][1] + a[2] * Bf[1][1] + + b[0] * Bf[0][2] + b[2] * Bf[0][0])); + if (!r) return 8; + + // A0 x B2 + s = T[2] * B[1][2] - T[1] * B[2][2]; + t = SR_ABS(s); + + r &= ( t <= + (a[1] * Bf[2][2] + a[2] * Bf[1][2] + + b[0] * Bf[0][1] + b[1] * Bf[0][0])); + if (!r) return 9; + + // A1 x B0 + s = T[0] * B[2][0] - T[2] * B[0][0]; + t = SR_ABS(s); + + r &= ( t <= + (a[0] * Bf[2][0] + a[2] * Bf[0][0] + + b[1] * Bf[1][2] + b[2] * Bf[1][1])); + if (!r) return 10; + + // A1 x B1 + s = T[0] * B[2][1] - T[2] * B[0][1]; + t = SR_ABS(s); + + r &= ( t <= + (a[0] * Bf[2][1] + a[2] * Bf[0][1] + + b[0] * Bf[1][2] + b[2] * Bf[1][0])); + if (!r) return 11; + + // A1 x B2 + s = T[0] * B[2][2] - T[2] * B[0][2]; + t = SR_ABS(s); + + r &= (t <= + (a[0] * Bf[2][2] + a[2] * Bf[0][2] + + b[0] * Bf[1][1] + b[1] * Bf[1][0])); + if (!r) return 12; + + // A2 x B0 + s = T[1] * B[0][0] - T[0] * B[1][0]; + t = SR_ABS(s); + + r &= (t <= + (a[0] * Bf[1][0] + a[1] * Bf[0][0] + + b[1] * Bf[2][2] + b[2] * Bf[2][1])); + if (!r) return 13; + + // A2 x B1 + s = T[1] * B[0][1] - T[0] * B[1][1]; + t = SR_ABS(s); + + r &= ( t <= + (a[0] * Bf[1][1] + a[1] * Bf[0][1] + + b[0] * Bf[2][2] + b[2] * Bf[2][0])); + if (!r) return 14; + + // A2 x B2 + s = T[1] * B[0][2] - T[0] * B[1][2]; + t = SR_ABS(s); + + r &= ( t <= + (a[0] * Bf[1][2] + a[1] * Bf[0][2] + + b[0] * Bf[2][1] + b[1] * Bf[2][0])); + if (!r) return 15; + + return 0; // should equal 0 +} + +//=============================== +//======= SrBv::overlap ========= +//=============================== +bool SrBv::overlap ( srbvmat R, srbvvec T, const SrBv* b1, const SrBv* b2 ) +{ + return obb_disjoint(R,T,b1->d,b2->d)==0? true:false; +} + +// ClipToRange +// +// clips val between a and b + +inline +void +ClipToRange(srbvreal &val, const srbvreal &a, const srbvreal &b) +{ + if (val < a) val = a; + else if (val > b) val = b; +} + +// SegCoords +// +// finds the parameters t & u corresponding to the two closest points +// on a pair of line segments +// +// The first segment is defined as +// +// Pa + A*t, 0 <= t <= a, +// +// where "Pa" is one endpoint of the segment, "A" is a unit vector +// pointing to the other endpoint, and t is a scalar that produces +// all the points between the two endpoints. Since "A" is a unit +// vector, "a" is the segment's length. +// +// The second segment is +// +// Pb + B*u, 0 <= u <= b +// +// In my application, many of the terms needed by the algorithm +// are already computed for other purposes, so I pass these terms to +// the function instead of complete specifications of each segment. +// "T" in the dot products is the vector between Pa and Pb. +// +// The algorithm is from +// +// Vladimir J. Lumelsky, +// On fast computation of distance between line segments. +// In Information Processing Letters, no. 21, pages 55-61, 1985. + +inline +void +SegCoords(srbvreal& t, srbvreal& u, + const srbvreal& a, const srbvreal& b, + const srbvreal& A_dot_B, + const srbvreal& A_dot_T, + const srbvreal& B_dot_T) +{ + srbvreal denom = 1 - (A_dot_B)*(A_dot_B); + + if (denom == 0) t = 0; + else + { + t = (A_dot_T - B_dot_T*A_dot_B)/denom; + ClipToRange(t,0,a); + } + + u = t*A_dot_B - B_dot_T; + if (u < 0) + { + u = 0; + t = A_dot_T; + ClipToRange(t,0,a); + } + else if (u > b) + { + u = b; + t = u*A_dot_B + A_dot_T; + ClipToRange(t,0,a); + } +} + +// InVoronoi +// +// returns whether the nearest point on rectangle edge +// Pb + B*u, 0 <= u <= b, to the rectangle edge, +// Pa + A*t, 0 <= t <= a, is within the half space +// determined by the point Pa and the direction Anorm. +// +// A,B, and Anorm are unit vectors. +// T is the vector between Pa and Pb. + +inline +int +InVoronoi(const srbvreal &a, + const srbvreal &b, + const srbvreal &Anorm_dot_B, + const srbvreal &Anorm_dot_T, + const srbvreal &A_dot_B, + const srbvreal &A_dot_T, + const srbvreal &B_dot_T) +{ + if (SR_ABS(Anorm_dot_B) < 1e-7) return 0; + + srbvreal t, u, v; + + u = -Anorm_dot_T / Anorm_dot_B; + ClipToRange(u,0,b); + + t = u*A_dot_B + A_dot_T; + ClipToRange(t,0,a); + + v = t*A_dot_B - B_dot_T; + + if (Anorm_dot_B > 0) + { + if (v > (u + 1e-7)) return 1; + } + else + { + if (v < (u - 1e-7)) return 1; + } + return 0; +} + + +// RectDist +// +// Finds the distance between two rectangles A and B. A is assumed +// to have its corner on the origin, one side aligned with +// x, the other side aligned with y, and its normal aligned with z. +// +// [Rab,Tab] gives the orientation and corner position of rectangle B +// +// a[2] are the side lengths of A, b[2] are the side lengths of B + +inline +srbvreal +RectDist(srbvreal Rab[3][3], srbvreal Tab[3], + const srbvreal a[2], const srbvreal b[2]) +{ + using namespace SrBvMath; + + srbvreal A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1; + + A0_dot_B0 = Rab[0][0]; + A0_dot_B1 = Rab[0][1]; + A1_dot_B0 = Rab[1][0]; + A1_dot_B1 = Rab[1][1]; + + srbvreal aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1; + srbvreal bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1; + + aA0_dot_B0 = a[0]*A0_dot_B0; + aA0_dot_B1 = a[0]*A0_dot_B1; + aA1_dot_B0 = a[1]*A1_dot_B0; + aA1_dot_B1 = a[1]*A1_dot_B1; + bA0_dot_B0 = b[0]*A0_dot_B0; + bA1_dot_B0 = b[0]*A1_dot_B0; + bA0_dot_B1 = b[1]*A0_dot_B1; + bA1_dot_B1 = b[1]*A1_dot_B1; + + srbvreal Tba[3]; + MTxV(Tba,Rab,Tab); + + srbvreal S[3], t, u; + + // determine if any edge pair contains the closest points + + srbvreal ALL_x, ALU_x, AUL_x, AUU_x; + srbvreal BLL_x, BLU_x, BUL_x, BUU_x; + srbvreal LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux; + + ALL_x = -Tba[0]; + ALU_x = ALL_x + aA1_dot_B0; + AUL_x = ALL_x + aA0_dot_B0; + AUU_x = ALU_x + aA0_dot_B0; + + if (ALL_x < ALU_x) + { + LA1_lx = ALL_x; + LA1_ux = ALU_x; + UA1_lx = AUL_x; + UA1_ux = AUU_x; + } + else + { + LA1_lx = ALU_x; + LA1_ux = ALL_x; + UA1_lx = AUU_x; + UA1_ux = AUL_x; + } + + BLL_x = Tab[0]; + BLU_x = BLL_x + bA0_dot_B1; + BUL_x = BLL_x + bA0_dot_B0; + BUU_x = BLU_x + bA0_dot_B0; + + if (BLL_x < BLU_x) + { + LB1_lx = BLL_x; + LB1_ux = BLU_x; + UB1_lx = BUL_x; + UB1_ux = BUU_x; + } + else + { + LB1_lx = BLU_x; + LB1_ux = BLL_x; + UB1_lx = BUU_x; + UB1_ux = BUL_x; + } + + // UA1, UB1 + + if ((UA1_ux > b[0]) && (UB1_ux > a[0])) + { + if (((UA1_lx > b[0]) || + InVoronoi(b[1],a[1],A1_dot_B0,aA0_dot_B0 - b[0] - Tba[0], + A1_dot_B1, aA0_dot_B1 - Tba[1], + -Tab[1] - bA1_dot_B0)) + && + + ((UB1_lx > a[0]) || + InVoronoi(a[1],b[1],A0_dot_B1,Tab[0] + bA0_dot_B0 - a[0], + A1_dot_B1,Tab[1] + bA1_dot_B0,Tba[1] - aA0_dot_B1))) + { + SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1] + bA1_dot_B0, + Tba[1] - aA0_dot_B1); + + S[0] = Tab[0] + Rab[0][0]*b[0] + Rab[0][1]*u - a[0] ; + S[1] = Tab[1] + Rab[1][0]*b[0] + Rab[1][1]*u - t; + S[2] = Tab[2] + Rab[2][0]*b[0] + Rab[2][1]*u; + return sqrt(VdotV(S,S)); + } + } + + + // UA1, LB1 + + if ((UA1_lx < 0) && (LB1_ux > a[0])) + { + if (((UA1_ux < 0) || + InVoronoi(b[1],a[1],-A1_dot_B0,Tba[0] - aA0_dot_B0, + A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1])) + && + + ((LB1_lx > a[0]) || + InVoronoi(a[1],b[1],A0_dot_B1,Tab[0] - a[0], + A1_dot_B1,Tab[1],Tba[1] - aA0_dot_B1))) + { + SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1],Tba[1] - aA0_dot_B1); + + S[0] = Tab[0] + Rab[0][1]*u - a[0]; + S[1] = Tab[1] + Rab[1][1]*u - t; + S[2] = Tab[2] + Rab[2][1]*u; + return sqrt(VdotV(S,S)); + } + } + + // LA1, UB1 + + if ((LA1_ux > b[0]) && (UB1_lx < 0)) + { + if (((LA1_lx > b[0]) || + InVoronoi(b[1],a[1],A1_dot_B0,-Tba[0] - b[0], + A1_dot_B1,-Tba[1], -Tab[1] - bA1_dot_B0)) + && + + ((UB1_ux < 0) || + InVoronoi(a[1],b[1],-A0_dot_B1, -Tab[0] - bA0_dot_B0, + A1_dot_B1, Tab[1] + bA1_dot_B0,Tba[1]))) + { + + SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1] + bA1_dot_B0,Tba[1]); + + S[0] = Tab[0] + Rab[0][0]*b[0] + Rab[0][1]*u; + S[1] = Tab[1] + Rab[1][0]*b[0] + Rab[1][1]*u - t; + S[2] = Tab[2] + Rab[2][0]*b[0] + Rab[2][1]*u; + return sqrt(VdotV(S,S)); + } + } + + // LA1, LB1 + + if ((LA1_lx < 0) && (LB1_lx < 0)) + { + if (((LA1_ux < 0) || + InVoronoi(b[1],a[1],-A1_dot_B0,Tba[0],A1_dot_B1, + -Tba[1],-Tab[1])) + && + + ((LB1_ux < 0) || + InVoronoi(a[1],b[1],-A0_dot_B1,-Tab[0],A1_dot_B1, + Tab[1], Tba[1]))) + { + SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1],Tba[1]); + + S[0] = Tab[0] + Rab[0][1]*u; + S[1] = Tab[1] + Rab[1][1]*u - t; + S[2] = Tab[2] + Rab[2][1]*u; + return sqrt(VdotV(S,S)); + } + } + + srbvreal ALL_y, ALU_y, AUL_y, AUU_y; + + ALL_y = -Tba[1]; + ALU_y = ALL_y + aA1_dot_B1; + AUL_y = ALL_y + aA0_dot_B1; + AUU_y = ALU_y + aA0_dot_B1; + + srbvreal LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux; + + if (ALL_y < ALU_y) + { + LA1_ly = ALL_y; + LA1_uy = ALU_y; + UA1_ly = AUL_y; + UA1_uy = AUU_y; + } + else + { + LA1_ly = ALU_y; + LA1_uy = ALL_y; + UA1_ly = AUU_y; + UA1_uy = AUL_y; + } + + if (BLL_x < BUL_x) + { + LB0_lx = BLL_x; + LB0_ux = BUL_x; + UB0_lx = BLU_x; + UB0_ux = BUU_x; + } + else + { + LB0_lx = BUL_x; + LB0_ux = BLL_x; + UB0_lx = BUU_x; + UB0_ux = BLU_x; + } + + // UA1, UB0 + + if ((UA1_uy > b[1]) && (UB0_ux > a[0])) + { + if (((UA1_ly > b[1]) || + InVoronoi(b[0],a[1],A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1], + A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1)) + && + + ((UB0_lx > a[0]) || + InVoronoi(a[1],b[0],A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1, + A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) + { + SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1] + bA1_dot_B1, + Tba[0] - aA0_dot_B0); + + S[0] = Tab[0] + Rab[0][1]*b[1] + Rab[0][0]*u - a[0] ; + S[1] = Tab[1] + Rab[1][1]*b[1] + Rab[1][0]*u - t; + S[2] = Tab[2] + Rab[2][1]*b[1] + Rab[2][0]*u; + return sqrt(VdotV(S,S)); + } + } + + // UA1, LB0 + + if ((UA1_ly < 0) && (LB0_ux > a[0])) + { + if (((UA1_uy < 0) || + InVoronoi(b[0],a[1],-A1_dot_B1, Tba[1] - aA0_dot_B1,A1_dot_B0, + aA0_dot_B0 - Tba[0], -Tab[1])) + && + + ((LB0_lx > a[0]) || + InVoronoi(a[1],b[0],A0_dot_B0,Tab[0] - a[0], + A1_dot_B0,Tab[1],Tba[0] - aA0_dot_B0))) + { + SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1],Tba[0] - aA0_dot_B0); + + S[0] = Tab[0] + Rab[0][0]*u - a[0]; + S[1] = Tab[1] + Rab[1][0]*u - t; + S[2] = Tab[2] + Rab[2][0]*u; + return sqrt(VdotV(S,S)); + } + } + + // LA1, UB0 + + if ((LA1_uy > b[1]) && (UB0_lx < 0)) + { + if (((LA1_ly > b[1]) || + InVoronoi(b[0],a[1],A1_dot_B1,-Tba[1] - b[1], + A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1)) + && + + ((UB0_ux < 0) || + InVoronoi(a[1],b[0],-A0_dot_B0, -Tab[0] - bA0_dot_B1,A1_dot_B0, + Tab[1] + bA1_dot_B1,Tba[0]))) + { + SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1] + bA1_dot_B1,Tba[0]); + + S[0] = Tab[0] + Rab[0][1]*b[1] + Rab[0][0]*u; + S[1] = Tab[1] + Rab[1][1]*b[1] + Rab[1][0]*u - t; + S[2] = Tab[2] + Rab[2][1]*b[1] + Rab[2][0]*u; + return sqrt(VdotV(S,S)); + } + } + + // LA1, LB0 + + if ((LA1_ly < 0) && (LB0_lx < 0)) + { + if (((LA1_uy < 0) || + InVoronoi(b[0],a[1],-A1_dot_B1,Tba[1],A1_dot_B0, + -Tba[0],-Tab[1])) + && + + ((LB0_ux < 0) || + InVoronoi(a[1],b[0],-A0_dot_B0,-Tab[0],A1_dot_B0, + Tab[1],Tba[0]))) + { + SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1],Tba[0]); + + S[0] = Tab[0] + Rab[0][0]*u; + S[1] = Tab[1] + Rab[1][0]*u - t; + S[2] = Tab[2] + Rab[2][0]*u; + return sqrt(VdotV(S,S)); + } + } + + srbvreal BLL_y, BLU_y, BUL_y, BUU_y; + + BLL_y = Tab[1]; + BLU_y = BLL_y + bA1_dot_B1; + BUL_y = BLL_y + bA1_dot_B0; + BUU_y = BLU_y + bA1_dot_B0; + + srbvreal LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy; + + if (ALL_x < AUL_x) + { + LA0_lx = ALL_x; + LA0_ux = AUL_x; + UA0_lx = ALU_x; + UA0_ux = AUU_x; + } + else + { + LA0_lx = AUL_x; + LA0_ux = ALL_x; + UA0_lx = AUU_x; + UA0_ux = ALU_x; + } + + if (BLL_y < BLU_y) + { + LB1_ly = BLL_y; + LB1_uy = BLU_y; + UB1_ly = BUL_y; + UB1_uy = BUU_y; + } + else + { + LB1_ly = BLU_y; + LB1_uy = BLL_y; + UB1_ly = BUU_y; + UB1_uy = BUL_y; + } + + // UA0, UB1 + + if ((UA0_ux > b[0]) && (UB1_uy > a[1])) + { + if (((UA0_lx > b[0]) || + InVoronoi(b[1],a[0],A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], + A0_dot_B1,aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0)) + && + + ((UB1_ly > a[1]) || + InVoronoi(a[0],b[1],A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0, + A0_dot_B1,Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) + { + SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0] + bA0_dot_B0, + Tba[1] - aA1_dot_B1); + + S[0] = Tab[0] + Rab[0][0]*b[0] + Rab[0][1]*u - t; + S[1] = Tab[1] + Rab[1][0]*b[0] + Rab[1][1]*u - a[1]; + S[2] = Tab[2] + Rab[2][0]*b[0] + Rab[2][1]*u; + return sqrt(VdotV(S,S)); + } + } + + // UA0, LB1 + + if ((UA0_lx < 0) && (LB1_uy > a[1])) + { + if (((UA0_ux < 0) || + InVoronoi(b[1],a[0],-A0_dot_B0, Tba[0] - aA1_dot_B0,A0_dot_B1, + aA1_dot_B1 - Tba[1],-Tab[0])) + && + + ((LB1_ly > a[1]) || + InVoronoi(a[0],b[1],A1_dot_B1,Tab[1] - a[1],A0_dot_B1,Tab[0], + Tba[1] - aA1_dot_B1))) + { + SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0],Tba[1] - aA1_dot_B1); + + S[0] = Tab[0] + Rab[0][1]*u - t; + S[1] = Tab[1] + Rab[1][1]*u - a[1]; + S[2] = Tab[2] + Rab[2][1]*u; + return sqrt(VdotV(S,S)); + } + } + + // LA0, UB1 + + if ((LA0_ux > b[0]) && (UB1_ly < 0)) + { + if (((LA0_lx > b[0]) || + InVoronoi(b[1],a[0],A0_dot_B0,-b[0] - Tba[0],A0_dot_B1,-Tba[1], + -bA0_dot_B0 - Tab[0])) + && + + ((UB1_uy < 0) || + InVoronoi(a[0],b[1],-A1_dot_B1, -Tab[1] - bA1_dot_B0,A0_dot_B1, + Tab[0] + bA0_dot_B0,Tba[1]))) + { + SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0] + bA0_dot_B0,Tba[1]); + + S[0] = Tab[0] + Rab[0][0]*b[0] + Rab[0][1]*u - t; + S[1] = Tab[1] + Rab[1][0]*b[0] + Rab[1][1]*u; + S[2] = Tab[2] + Rab[2][0]*b[0] + Rab[2][1]*u; + return sqrt(VdotV(S,S)); + } + } + + // LA0, LB1 + + if ((LA0_lx < 0) && (LB1_ly < 0)) + { + if (((LA0_ux < 0) || + InVoronoi(b[1],a[0],-A0_dot_B0,Tba[0],A0_dot_B1,-Tba[1], + -Tab[0])) + && + + ((LB1_uy < 0) || + InVoronoi(a[0],b[1],-A1_dot_B1,-Tab[1],A0_dot_B1, + Tab[0],Tba[1]))) + { + SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0],Tba[1]); + + S[0] = Tab[0] + Rab[0][1]*u - t; + S[1] = Tab[1] + Rab[1][1]*u; + S[2] = Tab[2] + Rab[2][1]*u; + return sqrt(VdotV(S,S)); + } + } + + srbvreal LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy; + + if (ALL_y < AUL_y) + { + LA0_ly = ALL_y; + LA0_uy = AUL_y; + UA0_ly = ALU_y; + UA0_uy = AUU_y; + } + else + { + LA0_ly = AUL_y; + LA0_uy = ALL_y; + UA0_ly = AUU_y; + UA0_uy = ALU_y; + } + + if (BLL_y < BUL_y) + { + LB0_ly = BLL_y; + LB0_uy = BUL_y; + UB0_ly = BLU_y; + UB0_uy = BUU_y; + } + else + { + LB0_ly = BUL_y; + LB0_uy = BLL_y; + UB0_ly = BUU_y; + UB0_uy = BLU_y; + } + + // UA0, UB0 + + if ((UA0_uy > b[1]) && (UB0_uy > a[1])) + { + if (((UA0_ly > b[1]) || + InVoronoi(b[0],a[0],A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], + A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1)) + && + + ((UB0_ly > a[1]) || + InVoronoi(a[0],b[0],A1_dot_B0,Tab[1] - a[1] + bA1_dot_B1,A0_dot_B0, + Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) + { + SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0] + bA0_dot_B1, + Tba[0] - aA1_dot_B0); + + S[0] = Tab[0] + Rab[0][1]*b[1] + Rab[0][0]*u - t; + S[1] = Tab[1] + Rab[1][1]*b[1] + Rab[1][0]*u - a[1]; + S[2] = Tab[2] + Rab[2][1]*b[1] + Rab[2][0]*u; + return sqrt(VdotV(S,S)); + } + } + + // UA0, LB0 + + if ((UA0_ly < 0) && (LB0_uy > a[1])) + { + if (((UA0_uy < 0) || + InVoronoi(b[0],a[0],-A0_dot_B1,Tba[1] - aA1_dot_B1,A0_dot_B0, + aA1_dot_B0 - Tba[0],-Tab[0])) + && + + ((LB0_ly > a[1]) || + InVoronoi(a[0],b[0],A1_dot_B0,Tab[1] - a[1], + A0_dot_B0,Tab[0],Tba[0] - aA1_dot_B0))) + { + SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0],Tba[0] - aA1_dot_B0); + + S[0] = Tab[0] + Rab[0][0]*u - t; + S[1] = Tab[1] + Rab[1][0]*u - a[1]; + S[2] = Tab[2] + Rab[2][0]*u; + return sqrt(VdotV(S,S)); + } + } + + // LA0, UB0 + + if ((LA0_uy > b[1]) && (UB0_ly < 0)) + { + if (((LA0_ly > b[1]) || + InVoronoi(b[0],a[0],A0_dot_B1,-Tba[1] - b[1], A0_dot_B0,-Tba[0], + -Tab[0] - bA0_dot_B1)) + && + + ((UB0_uy < 0) || + InVoronoi(a[0],b[0],-A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0, + Tab[0] + bA0_dot_B1,Tba[0]))) + { + SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0] + bA0_dot_B1,Tba[0]); + + S[0] = Tab[0] + Rab[0][1]*b[1] + Rab[0][0]*u - t; + S[1] = Tab[1] + Rab[1][1]*b[1] + Rab[1][0]*u; + S[2] = Tab[2] + Rab[2][1]*b[1] + Rab[2][0]*u; + return sqrt(VdotV(S,S)); + } + } + + // LA0, LB0 + + if ((LA0_ly < 0) && (LB0_ly < 0)) + { + if (((LA0_uy < 0) || + InVoronoi(b[0],a[0],-A0_dot_B1,Tba[1],A0_dot_B0, + -Tba[0],-Tab[0])) + && + + ((LB0_uy < 0) || + InVoronoi(a[0],b[0],-A1_dot_B0,-Tab[1],A0_dot_B0, + Tab[0],Tba[0]))) + { + SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0],Tba[0]); + + S[0] = Tab[0] + Rab[0][0]*u - t; + S[1] = Tab[1] + Rab[1][0]*u; + S[2] = Tab[2] + Rab[2][0]*u; + return sqrt(VdotV(S,S)); + } + } + + // no edges passed, take max separation along face normals + + srbvreal sep1, sep2; + + if (Tab[2] > 0.0) + { + sep1 = Tab[2]; + if (Rab[2][0] < 0.0) sep1 += b[0]*Rab[2][0]; + if (Rab[2][1] < 0.0) sep1 += b[1]*Rab[2][1]; + } + else + { + sep1 = -Tab[2]; + if (Rab[2][0] > 0.0) sep1 -= b[0]*Rab[2][0]; + if (Rab[2][1] > 0.0) sep1 -= b[1]*Rab[2][1]; + } + + if (Tba[2] < 0) + { + sep2 = -Tba[2]; + if (Rab[0][2] < 0.0) sep2 += a[0]*Rab[0][2]; + if (Rab[1][2] < 0.0) sep2 += a[1]*Rab[1][2]; + } + else + { + sep2 = Tba[2]; + if (Rab[0][2] > 0.0) sep2 -= a[0]*Rab[0][2]; + if (Rab[1][2] > 0.0) sep2 -= a[1]*Rab[1][2]; + } + + srbvreal sep = (sep1 > sep2? sep1 : sep2); + return (sep > 0? sep : 0); +} + +//================================ +//======= SrBv::distance ========= +//================================ +srbvreal SrBv::distance ( srbvmat R, srbvvec T, const SrBv* b1, const SrBv* b2 ) +{ + srbvreal dist = RectDist(R,T,b1->l,b2->l); + dist -= (b1->r + b2->r); + return (dist < (srbvreal)0.0)? (srbvreal)0.0 : dist; +} + + + + +/*************************************************************************\ + Copyright 1999 The University of North Carolina at Chapel Hill. + All Rights Reserved. + + Permission to use, copy, modify and distribute this software and its + documentation for educational, research and non-profit purposes, without + fee, and without a written agreement is hereby granted, provided that the + above copyright notice and the following three paragraphs appear in all + copies. + + IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE + LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE + USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY + OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + DAMAGES. + + THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE + PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF + NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, + UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + + The authors may be contacted via: + + US Mail: S. Gottschalk, E. Larsen + Department of Computer Science + Sitterson Hall, CB #3175 + University of N. Carolina + Chapel Hill, NC 27599-3175 + Phone: (919)962-1749 + EMail: geom@cs.unc.edu +\**************************************************************************/ + diff --git a/source/dcdt/se/sr_bv_coldet.cpp b/source/dcdt/se/sr_bv_coldet.cpp new file mode 100644 index 0000000000..9710f34974 --- /dev/null +++ b/source/dcdt/se/sr_bv_coldet.cpp @@ -0,0 +1,230 @@ +#include "precompiled.h" +# include "sr_bv_coldet.h" + +//============================= SrBvColdet::Object =========================== + +/*! Contains required data to be stored in each BvTree. */ +class SrBvColdet::Object + { public : + bool active; + srbvmat R; + srbvvec T; + SrBvTree tree; + public : + Object () + { SrBvMath::Midentity ( R ); // set to id matrix + SrBvMath::Videntity ( T ); // set to a null vector + active = true; + } + + ~Object () {}; + + void set_transformation ( const SrMat& srm ) + { + # define SRM(i) (srbvreal)srm.get(i) + // here we pass from column-major to line-major order: + R[0][0]=SRM(0); R[1][0]=SRM(1); R[2][0]=SRM(2); + R[0][1]=SRM(4); R[1][1]=SRM(5); R[2][1]=SRM(6); + R[0][2]=SRM(8); R[1][2]=SRM(9); R[2][2]=SRM(10); + T[0]=SRM(12); T[1]=SRM(13); T[2]=SRM(14); + # undef SRM + } + + void make_tree ( const SrModel& model ) + { tree.make ( model ); + } + }; + +class SrBvColdetObjectManager : public SrClassManagerBase + { public : + virtual void* alloc () { return (void*) new SrBvColdet::Object; } + virtual void* alloc ( const void* obj ) { return (void*) 0; } // copy not used/supported + virtual void free ( void* obj ) { delete (SrBvColdet::Object*) obj; } + }; + +//================================ SrBvColdet ===================================== + +SrBvColdet::SrBvColdet () + { + _set = new SrSet ( new SrBvColdetObjectManager ); + } + +SrBvColdet::~SrBvColdet () + { + delete _set; + } + +void SrBvColdet::init () + { + _set->init (); + _nbody.init (); + _disabled.init (); + _pairs.size (0); + } + +int SrBvColdet::insert_object ( const SrModel& m ) + { + int id = _set->insert (); + Object* o = _set->get ( id ); + o->make_tree ( m ); + _nbody.insert_object ( id, m ); + return id; + } + +void SrBvColdet::remove_object ( int id ) + { + Object* o = _getobj ( id ); + if ( !o ) return; + _set->remove(id); // will also delete o + _disabled.del_pairs_with_id ( id ); + _nbody.remove_object ( id ); + } + +void SrBvColdet::update_transformation ( int id, const SrMat& m ) + { + Object* o = _getobj ( id ); + if ( !o ) return; + o->set_transformation ( m ); + _nbody.update_transformation ( id, m ); + } + +void SrBvColdet::activate_object ( int id ) + { + Object* o = _getobj ( id ); + if ( !o ) return; + o->active = true; + } + +void SrBvColdet::deactivate_object ( int id ) + { + Object* o = _getobj ( id ); + if ( !o ) return; + o->active = false; + } + +void SrBvColdet::activate_pair ( int id1, int id2 ) + { + _disabled.del_pair ( id1, id2 ); + } + +void SrBvColdet::deactivate_pair ( int id1, int id2 ) + { + _disabled.add_pair ( id1, id2 ); + } + +//============================= private method ================================ + +bool SrBvColdet::_collide ( SrBvTreeQuery::CollideFlag flag ) + { + _pairs.size ( 0 ); + + const SrArray& overlap = _nbody.overlapping_pairs.elements(); + const SrArray& disabled = _disabled.elements(); + int osize = overlap.size(); + int dsize = disabled.size(); + + bool call_collide; + Object* o1; + Object* o2; + const SrBvIdPairs::Elem* curr_overlap; + const SrBvIdPairs::Elem* curr_disabled; + + // Simultaneously traverse overlap and disabled, + // and make collision queries only when required. + int i; + for ( i=0; iget(i); //overlap[i]->id); + if ( !o1 ) continue; + if ( !o1->active ) continue; // this objects is not active + + curr_disabled = iget(curr_overlap->id); + + // update curr_disabled position: + while ( curr_disabled ) + { if ( curr_disabled->idid ) + curr_disabled = curr_disabled->next; + else break; + } + + call_collide = false; + if ( !curr_disabled ) call_collide=true; + else + if ( curr_disabled->id>curr_overlap->id ) call_collide=true; + else + curr_disabled=curr_disabled->next; + + //sr_out<<"COLLIDING "<id<active ) + { + _query.collide ( o1->R, o1->T, &(o1->tree), o2->R, o2->T, &(o2->tree), flag ); + _query.colliding_pairs().size(); + if ( _query.colliding_pairs().size()>0 ) // had collisions + { _pairs.push()=i; + _pairs.push()=curr_overlap->id; + if ( flag==SrBvTreeQuery::CollideFirstContact ) return true; + } + } + curr_overlap = curr_overlap->next; + } + } + return _pairs.size()>0? true:false; + } + +bool SrBvColdet::_collide_tolerance ( float toler ) + { + const SrArray& overlap = _nbody.overlapping_pairs.elements(); + const SrArray& disabled = _disabled.elements(); + int osize = overlap.size(); + int dsize = disabled.size(); + + bool call_tolerance; + Object* o1; + Object* o2; + const SrBvIdPairs::Elem* curr_overlap; + const SrBvIdPairs::Elem* curr_disabled; + + // Simultaneously traverse overlap and disabled, + // and make collision queries only when required. + int i; + for ( i=0; iget(i); //overlap[i]->id); + if ( !o1 ) continue; + if ( !o1->active ) continue; // this objects is not active + + curr_disabled = iget(curr_overlap->id); + + // update curr_disabled position: + while ( curr_disabled ) + { if ( curr_disabled->idid ) + curr_disabled = curr_disabled->next; + else break; + } + + call_tolerance = false; + if ( !curr_disabled ) call_tolerance=true; + else + if ( curr_disabled->id>curr_overlap->id ) call_tolerance=true; + else + curr_disabled=curr_disabled->next; + + if ( call_tolerance && o2->active ) + { if ( _query.tolerance( o1->R, o1->T, &(o1->tree), o2->R, o2->T, &(o2->tree), toler ) ) + return true; + } + curr_overlap = curr_overlap->next; + } + } + return false; + } + +//=================================== EOF ===================================== + diff --git a/source/dcdt/se/sr_bv_coldet.h b/source/dcdt/se/sr_bv_coldet.h new file mode 100644 index 0000000000..273e399ccd --- /dev/null +++ b/source/dcdt/se/sr_bv_coldet.h @@ -0,0 +1,95 @@ +/** \file sr_bv_coldet.h + * multi model collision test */ + +# ifndef SR_BV_COLDET_H +# define SR_BV_COLDET_H + + +# include "precompiled.h" +# include "sr_set.h" +# include "sr_bv_nbody.h" +# include "sr_bv_tree_query.h" + +/* SrBvColdet uses SrBvNBody in order to efficiently determine + collisions between several dynamic objects. + Adapted from the VCollide package. Their copyright is in the source file. */ +class SrBvColdet + { private: + class Object; + friend class SrBvColdetObjectManager; + SrSet* _set; + SrBvNBody _nbody; + SrBvIdPairs _disabled; + SrBvTreeQuery _query; + SrArray _pairs; + + public: + /*! Constructor */ + SrBvColdet (); + + /*! Destructor (calls init()) */ + ~SrBvColdet (); + + /*! Clears everything */ + void init (); + + /*! Create a new object and returns its id. */ + int insert_object ( const SrModel& m ); + + /*! Removes (and deletes) the object from the database. */ + void remove_object ( int id ); + + /*! Returns true if id represents a valid id in the database, and false otherwise */ + bool id_valid ( int id ) { return _getobj(id)? true:false; } + + /*! Update the transformation applied to an object. + Only rotations and translations are supported. */ + void update_transformation ( int id, const SrMat& m ); + + /*! Turn on collision detection for an object. */ + void activate_object ( int id ); + + /*! Turn off collision detection for an object. */ + void deactivate_object ( int id ); + + /*! Turn on collision detection between a specific pair of objects. */ + void activate_pair ( int id1, int id2 ); + + /*! Turn off collision detection between a specific pair of objects. */ + void deactivate_pair ( int id1, int id2 ); + + /*! Returns true if the given pair of objects is deactivated and + false otherwise (-1 ids return false). */ + bool pair_deactivated ( int id1, int id2 ) { return _disabled.pair_exists(id1,id2); } + + /*! Counts and returns the number of deactivated pairs */ + int count_deactivated_pairs () { return _disabled.count_pairs(); } + + /*! Returns the array with the ids of the colliding objects in + the last collide_all() or has_collisions() query. The id + pairs are sequentially stored in the array. Therefore, the number + of collisions = colliding_pairs.size()/2. */ + const SrArray& colliding_pairs () const { return _pairs; } + + /*! Perform collision detection only untill finding a first collision. + True is returned if a collision was found; false is returned otherwise. */ + bool collide () + { return _collide ( SrBvTreeQuery::CollideFirstContact ); } + + /*! Perform collision detection among all pairs of objects. The results can + be retrieved with colliding_pairs() */ + bool collide_all () + { return _collide ( SrBvTreeQuery::CollideAllContacts ); } + + /*! Returns true as soon as the distance between one pair of models + is found to be smaller than the given tolerance. + False is returned when all pairs respects the minimum clearance. */ + bool collide_tolerance ( float toler ) { return _collide_tolerance(toler); } + + private : + bool _collide ( SrBvTreeQuery::CollideFlag flag ); + bool _collide_tolerance ( float toler ); + Object* _getobj ( int id ) { return ( id<0 || id>_set->maxid() )? 0:_set->get(id); } + }; + +#endif // SR_BV_COLDET_H diff --git a/source/dcdt/se/sr_bv_id_pairs.cpp b/source/dcdt/se/sr_bv_id_pairs.cpp new file mode 100644 index 0000000000..98d91ecb43 --- /dev/null +++ b/source/dcdt/se/sr_bv_id_pairs.cpp @@ -0,0 +1,216 @@ +# include "precompiled.h" +# include "sr_bv_id_pairs.h" + +# define DEFAULT_SIZE 10 + +//============================== SrBvIdPairs ==================================== + +# define ORDER_IDS(id1,id2) { if (id1>id2) { SR_SWAPX(id1,id2); } } // make id1=_arr.size() ) // increase the size of "_arr" + { _arr.push() = 0; // reallocation is efficiently done when needed + } + + Elem *current = _arr[id1]; //select the right list from "_arr". + + if ( !current ) //if the list is empty, insert the element in the front. + { current = new Elem; + current->id = id2; + current->next = NULL; + _arr[id1] = current; + } + else if (current->id > id2) //if the list is not empty but all + { //elements are greater than id2, then + current = new Elem; //insert id2 in the front. + current->id = id2; + current->next = _arr[id1]; + _arr[id1] = current; + } + else + { while (current->next != NULL) // otherwise, find the correct location + { // in the sorted list (ascending order) + if (current->next->id > id2) //and insert id2 there. + break; + current = current->next; + } + if (current->id == id2) // already there + { return; + } + else + { Elem *temp = new Elem; + temp->id = id2; + temp->next = current->next; + current->next = temp; + } + } +} + +void SrBvIdPairs::del_pair ( int id1, int id2 ) //delete a pair from the set. +{ + ORDER_IDS(id1, id2); //order the ids. + + if ( id1<0 || id1>=_arr.size() ) return; //the pair does not exist in the set, so return + + Elem *current = _arr[id1]; //otherwise, select the correct list. + + if ( !current ) return; //if this list is empty, the pair doesn't exist, so return + + if (current->id == id2) //otherwise, if id2 is the first element, delete it + { _arr[id1] = current->next; + delete current; + return; + } + else + { while (current->next != NULL) //if id2 is not the first element, + { //start traversing the sorted list. + if (current->next->id > id2) //if you have moved too far away + { //without hitting id2, then the pair + return; //pair doesn't exist. So, return. + } + else if (current->next->id == id2) //otherwise, delete id2. + { Elem *temp = current->next; + current->next = current->next->next; + delete temp; + return; + } + current = current->next; + } + } +} + +void SrBvIdPairs:: del_pairs_with_id ( int id ) //delete all pairs containing id. +{ + int i; + Elem *t; + Elem *temp; + + if ( id<_arr.size() ) + { temp = _arr[id]; + while ( temp ) + { t = temp; + temp = temp->next; + delete t; + } + _arr[id] = 0; + + for ( i=0; inext; + delete current; + } + } + + _arr.size(0); + }; + +bool SrBvIdPairs::pair_exists ( int id1, int id2 ) +{ + ORDER_IDS ( id1, id2 ); + + if ( id1>=_arr.size() || id1<0 || id2<0 ) return false; // the pair cannot exist + + Elem *current = _arr[id1]; //otherwise, get the correct list and look for id2 + while ( current ) + { if ( current->id==id2) return true; + if ( current->id>id2) return false; + current = current->next; + } + return false; +} + +int SrBvIdPairs::count_pairs () +{ + int i, n = 0; + + Elem *current; + for ( i=0; i<_arr.size(); i++ ) + { current = _arr[i]; + while ( current ) + { n++; current = current->next; } + } + + return n; +} + +/************************************************************************\ + + Copyright 1997 The University of North Carolina at Chapel Hill. + All Rights Reserved. + + Permission to use, copy, modify and distribute this software + and its documentation for educational, research and non-profit + purposes, without fee, and without a written agreement is + hereby granted, provided that the above copyright notice and + the following three paragraphs appear in all copies. + + IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL + HILL BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, + INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, + ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, + EVEN IF THE UNIVERSITY OF NORTH CAROLINA HAVE BEEN ADVISED OF + THE POSSIBILITY OF SUCH DAMAGES. + + Permission to use, copy, modify and distribute this software + and its documentation for educational, research and non-profit + purposes, without fee, and without a written agreement is + hereby granted, provided that the above copyright notice and + the following three paragraphs appear in all copies. + + THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" + BASIS, AND THE UNIVERSITY OF NORTH CAROLINA HAS NO OBLIGATION + TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR + MODIFICATIONS. + + + --------------------------------- + |Please send all BUG REPORTS to: | + | | + | geom@cs.unc.edu | + | | + --------------------------------- + + + The authors may be contacted via: + + US Mail: A. Pattekar/J. Cohen/T. Hudson/S. Gottschalk/M. Lin/D. Manocha + Department of Computer Science + Sitterson Hall, CB #3175 + University of N. Carolina + Chapel Hill, NC 27599-3175 + + Phone: (919)962-1749 + + EMail: geom@cs.unc.edu + +\************************************************************************/ diff --git a/source/dcdt/se/sr_bv_id_pairs.h b/source/dcdt/se/sr_bv_id_pairs.h new file mode 100644 index 0000000000..635afdc7b1 --- /dev/null +++ b/source/dcdt/se/sr_bv_id_pairs.h @@ -0,0 +1,55 @@ +/** \file sr_bv_id_pairs.h + * maintains a set of id pairs */ + +# ifndef SR_BV_ID_PAIRS_H +# define SR_BV_ID_PAIRS_H + +# include "sr_array.h" + +/*! Stores a set of pairs of integers. It is assumed that the data is sparse + (i.e. the size of this set is small as compared to the n*(n-1)/2 possible pairs). + A pair (id1, id2) exists <==> max(id1, id2) exists in the linked list pointed + to by elements[min(id1, id2)]. + Adapted from the VCollide package. Their copyright is in the source file. */ +class SrBvIdPairs + { public: + /*! The node of the linked list containing the 2nd ids of all pairs + starting with the same id. It is a single connected list, with + the last pointer as null */ + struct Elem { int id; Elem *next; }; + + private : + SrArray _arr; + + public : + /*! Constructor */ + SrBvIdPairs (); + + /*! Destructor */ + ~SrBvIdPairs (); + + /*! Returns the array of elements. Each element[i] points to the sorted list + with all pairs (i,x) in the structure, i& elements() const { return _arr; } + + /*! Add a pair of ids to the set */ + void add_pair ( int id1, int id2 ); + + /*! Delete a pair from the set */ + void del_pair ( int id1, int id2 ); + + /*! Delete all pairs containing id */ + void del_pairs_with_id ( int id ); + + /*! Empty the set */ + void init (); + + /*! Check if a pair of ids exists (-1 ids return false) */ + bool pair_exists ( int id1, int id2 ); + + /*! Counts and returns the number of existing pairs */ + int count_pairs (); +}; + +#endif // SR_BV_ID_PAIRS_H diff --git a/source/dcdt/se/sr_bv_math.cpp b/source/dcdt/se/sr_bv_math.cpp new file mode 100644 index 0000000000..a6891fa1d9 --- /dev/null +++ b/source/dcdt/se/sr_bv_math.cpp @@ -0,0 +1,902 @@ +#include "precompiled.h"/* Note: this code was adapted from the PQP library; + their copyright notice can be found at the end of this file. */ + +# include +# include + +# include "sr_bv_math.h" + +//====================== namespace SrBvMath ======================= + +/* void Mprint ( const srbvmat M ); +void SrBvMath::Mprint ( const srbvmat M ) +{ + printf("%g %g %g\n%g %g %g\n%g %g %g\n", + M[0][0], M[0][1], M[0][2], + M[1][0], M[1][1], M[1][2], + M[2][0], M[2][1], M[2][2]); +}*/ + +/* void Vprint ( const srbvvec V ); +void SrBvMath::Vprint ( const srbvvec V ) +{ + printf("%g %g %g\n", V[0], V[1], V[2]); +}*/ + +/* void Vset ( srbvvec V, srbvreal x, srbvreal y, srbvreal z ); +void SrBvMath::Vset ( srbvvec V, srbvreal x, srbvreal y, srbvreal z ) +{ + V[0]=x; V[1]=y; V[2]=z; +}*/ + +void SrBvMath::Vset ( srbvvec V, float* fp ) +{ + V[0]=fp[0]; V[1]=fp[1]; V[2]=fp[2]; +} + +void SrBvMath::Midentity ( srbvmat M ) +{ + M[0][0] = M[1][1] = M[2][2] = 1; + M[0][1] = M[1][2] = M[2][0] = 0; + M[0][2] = M[1][0] = M[2][1] = 0; +} + +void SrBvMath::Videntity ( srbvvec T ) +{ + T[0] = T[1] = T[2] = 0.0; +} + +void SrBvMath::McM ( srbvmat Mr, const srbvmat M ) +{ + Mr[0][0] = M[0][0]; Mr[0][1] = M[0][1]; Mr[0][2] = M[0][2]; + Mr[1][0] = M[1][0]; Mr[1][1] = M[1][1]; Mr[1][2] = M[1][2]; + Mr[2][0] = M[2][0]; Mr[2][1] = M[2][1]; Mr[2][2] = M[2][2]; +} + +/*void MTcM ( srbvmat Mr, const srbvmat M ); // Mr=M.transpose() +void SrBvMath::MTcM ( srbvmat Mr, const srbvmat M ) +{ + Mr[0][0] = M[0][0]; Mr[1][0] = M[0][1]; Mr[2][0] = M[0][2]; + Mr[0][1] = M[1][0]; Mr[1][1] = M[1][1]; Mr[2][1] = M[1][2]; + Mr[0][2] = M[2][0]; Mr[1][2] = M[2][1]; Mr[2][2] = M[2][2]; +}*/ + +void SrBvMath::VcV ( srbvvec Vr, const srbvvec V ) +{ + Vr[0] = V[0]; Vr[1] = V[1]; Vr[2] = V[2]; +} + +void SrBvMath::McolcV ( srbvvec Vr, const srbvmat M, int c ) +{ + Vr[0] = M[0][c]; + Vr[1] = M[1][c]; + Vr[2] = M[2][c]; +} + +void SrBvMath::McolcMcol ( srbvmat Mr, int cr, const srbvmat M, int c ) +{ + Mr[0][cr] = M[0][c]; + Mr[1][cr] = M[1][c]; + Mr[2][cr] = M[2][c]; +} +/* +void MxMpV ( srbvmat Mr, const srbvmat M1, const srbvmat M2, const srbvvec T ); // Mr=M1*M2+T + + void SrBvMath::MxMpV ( srbvmat Mr, const srbvmat M1, const srbvmat M2, const srbvvec T ) +{ + Mr[0][0] = (M1[0][0] * M2[0][0] + + M1[0][1] * M2[1][0] + + M1[0][2] * M2[2][0] + + T[0]); + Mr[1][0] = (M1[1][0] * M2[0][0] + + M1[1][1] * M2[1][0] + + M1[1][2] * M2[2][0] + + T[1]); + Mr[2][0] = (M1[2][0] * M2[0][0] + + M1[2][1] * M2[1][0] + + M1[2][2] * M2[2][0] + + T[2]); + Mr[0][1] = (M1[0][0] * M2[0][1] + + M1[0][1] * M2[1][1] + + M1[0][2] * M2[2][1] + + T[0]); + Mr[1][1] = (M1[1][0] * M2[0][1] + + M1[1][1] * M2[1][1] + + M1[1][2] * M2[2][1] + + T[1]); + Mr[2][1] = (M1[2][0] * M2[0][1] + + M1[2][1] * M2[1][1] + + M1[2][2] * M2[2][1] + + T[2]); + Mr[0][2] = (M1[0][0] * M2[0][2] + + M1[0][1] * M2[1][2] + + M1[0][2] * M2[2][2] + + T[0]); + Mr[1][2] = (M1[1][0] * M2[0][2] + + M1[1][1] * M2[1][2] + + M1[1][2] * M2[2][2] + + T[1]); + Mr[2][2] = (M1[2][0] * M2[0][2] + + M1[2][1] * M2[1][2] + + M1[2][2] * M2[2][2] + + T[2]); +}*/ + +void SrBvMath::MxM ( srbvmat Mr, const srbvmat M1, const srbvmat M2 ) +{ + Mr[0][0] = (M1[0][0] * M2[0][0] + + M1[0][1] * M2[1][0] + + M1[0][2] * M2[2][0]); + Mr[1][0] = (M1[1][0] * M2[0][0] + + M1[1][1] * M2[1][0] + + M1[1][2] * M2[2][0]); + Mr[2][0] = (M1[2][0] * M2[0][0] + + M1[2][1] * M2[1][0] + + M1[2][2] * M2[2][0]); + Mr[0][1] = (M1[0][0] * M2[0][1] + + M1[0][1] * M2[1][1] + + M1[0][2] * M2[2][1]); + Mr[1][1] = (M1[1][0] * M2[0][1] + + M1[1][1] * M2[1][1] + + M1[1][2] * M2[2][1]); + Mr[2][1] = (M1[2][0] * M2[0][1] + + M1[2][1] * M2[1][1] + + M1[2][2] * M2[2][1]); + Mr[0][2] = (M1[0][0] * M2[0][2] + + M1[0][1] * M2[1][2] + + M1[0][2] * M2[2][2]); + Mr[1][2] = (M1[1][0] * M2[0][2] + + M1[1][1] * M2[1][2] + + M1[1][2] * M2[2][2]); + Mr[2][2] = (M1[2][0] * M2[0][2] + + M1[2][1] * M2[1][2] + + M1[2][2] * M2[2][2]); +} + +/* void MxMT ( srbvmat Mr, const srbvmat M1, const srbvmat M2 ); // Mr=M1*M2.transpose() +void SrBvMath::MxMT ( srbvmat Mr, const srbvmat M1, const srbvmat M2 ) +{ + Mr[0][0] = (M1[0][0] * M2[0][0] + + M1[0][1] * M2[0][1] + + M1[0][2] * M2[0][2]); + Mr[1][0] = (M1[1][0] * M2[0][0] + + M1[1][1] * M2[0][1] + + M1[1][2] * M2[0][2]); + Mr[2][0] = (M1[2][0] * M2[0][0] + + M1[2][1] * M2[0][1] + + M1[2][2] * M2[0][2]); + Mr[0][1] = (M1[0][0] * M2[1][0] + + M1[0][1] * M2[1][1] + + M1[0][2] * M2[1][2]); + Mr[1][1] = (M1[1][0] * M2[1][0] + + M1[1][1] * M2[1][1] + + M1[1][2] * M2[1][2]); + Mr[2][1] = (M1[2][0] * M2[1][0] + + M1[2][1] * M2[1][1] + + M1[2][2] * M2[1][2]); + Mr[0][2] = (M1[0][0] * M2[2][0] + + M1[0][1] * M2[2][1] + + M1[0][2] * M2[2][2]); + Mr[1][2] = (M1[1][0] * M2[2][0] + + M1[1][1] * M2[2][1] + + M1[1][2] * M2[2][2]); + Mr[2][2] = (M1[2][0] * M2[2][0] + + M1[2][1] * M2[2][1] + + M1[2][2] * M2[2][2]); +}*/ + +void SrBvMath::MTxM ( srbvmat Mr, const srbvmat M1, const srbvmat M2 ) +{ + Mr[0][0] = (M1[0][0] * M2[0][0] + + M1[1][0] * M2[1][0] + + M1[2][0] * M2[2][0]); + Mr[1][0] = (M1[0][1] * M2[0][0] + + M1[1][1] * M2[1][0] + + M1[2][1] * M2[2][0]); + Mr[2][0] = (M1[0][2] * M2[0][0] + + M1[1][2] * M2[1][0] + + M1[2][2] * M2[2][0]); + Mr[0][1] = (M1[0][0] * M2[0][1] + + M1[1][0] * M2[1][1] + + M1[2][0] * M2[2][1]); + Mr[1][1] = (M1[0][1] * M2[0][1] + + M1[1][1] * M2[1][1] + + M1[2][1] * M2[2][1]); + Mr[2][1] = (M1[0][2] * M2[0][1] + + M1[1][2] * M2[1][1] + + M1[2][2] * M2[2][1]); + Mr[0][2] = (M1[0][0] * M2[0][2] + + M1[1][0] * M2[1][2] + + M1[2][0] * M2[2][2]); + Mr[1][2] = (M1[0][1] * M2[0][2] + + M1[1][1] * M2[1][2] + + M1[2][1] * M2[2][2]); + Mr[2][2] = (M1[0][2] * M2[0][2] + + M1[1][2] * M2[1][2] + + M1[2][2] * M2[2][2]); +} + +void SrBvMath::MxV ( srbvvec Vr, const srbvmat M1, const srbvvec V1 ) +{ + Vr[0] = (M1[0][0] * V1[0] + + M1[0][1] * V1[1] + + M1[0][2] * V1[2]); + Vr[1] = (M1[1][0] * V1[0] + + M1[1][1] * V1[1] + + M1[1][2] * V1[2]); + Vr[2] = (M1[2][0] * V1[0] + + M1[2][1] * V1[1] + + M1[2][2] * V1[2]); +} + +void SrBvMath::MxVpV ( srbvvec Vr, const srbvmat M1, const srbvvec V1, const srbvvec V2) +{ + Vr[0] = (M1[0][0] * V1[0] + + M1[0][1] * V1[1] + + M1[0][2] * V1[2] + + V2[0]); + Vr[1] = (M1[1][0] * V1[0] + + M1[1][1] * V1[1] + + M1[1][2] * V1[2] + + V2[1]); + Vr[2] = (M1[2][0] * V1[0] + + M1[2][1] * V1[1] + + M1[2][2] * V1[2] + + V2[2]); +} + +/* void sMxVpV ( srbvvec Vr, srbvreal s1, const srbvmat M1, const srbvvec V1, const srbvvec V2 ); +void SrBvMath::sMxVpV ( srbvvec Vr, srbvreal s1, const srbvmat M1, const srbvvec V1, const srbvvec V2 ) +{ + Vr[0] = s1 * (M1[0][0] * V1[0] + + M1[0][1] * V1[1] + + M1[0][2] * V1[2]) + + V2[0]; + Vr[1] = s1 * (M1[1][0] * V1[0] + + M1[1][1] * V1[1] + + M1[1][2] * V1[2]) + + V2[1]; + Vr[2] = s1 * (M1[2][0] * V1[0] + + M1[2][1] * V1[1] + + M1[2][2] * V1[2]) + + V2[2]; +}*/ + +void SrBvMath::MTxV ( srbvvec Vr, const srbvmat M1, const srbvvec V1 ) +{ + Vr[0] = (M1[0][0] * V1[0] + + M1[1][0] * V1[1] + + M1[2][0] * V1[2]); + Vr[1] = (M1[0][1] * V1[0] + + M1[1][1] * V1[1] + + M1[2][1] * V1[2]); + Vr[2] = (M1[0][2] * V1[0] + + M1[1][2] * V1[1] + + M1[2][2] * V1[2]); +} + +/* void sMTxV ( srbvvec Vr, srbvreal s1, const srbvmat M1, const srbvvec V1 ); +void SrBvMath::sMTxV ( srbvvec Vr, srbvreal s1, const srbvmat M1, const srbvvec V1 ) +{ + Vr[0] = s1*(M1[0][0] * V1[0] + + M1[1][0] * V1[1] + + M1[2][0] * V1[2]); + Vr[1] = s1*(M1[0][1] * V1[0] + + M1[1][1] * V1[1] + + M1[2][1] * V1[2]); + Vr[2] = s1*(M1[0][2] * V1[0] + + M1[1][2] * V1[1] + + M1[2][2] * V1[2]); +}*/ + +/* void sMxV ( srbvvec Vr, srbvreal s1, const srbvmat M1, const srbvvec V1 ); +void SrBvMath::sMxV ( srbvvec Vr, srbvreal s1, const srbvmat M1, const srbvvec V1 ) +{ + Vr[0] = s1*(M1[0][0] * V1[0] + + M1[0][1] * V1[1] + + M1[0][2] * V1[2]); + Vr[1] = s1*(M1[1][0] * V1[0] + + M1[1][1] * V1[1] + + M1[1][2] * V1[2]); + Vr[2] = s1*(M1[2][0] * V1[0] + + M1[2][1] * V1[1] + + M1[2][2] * V1[2]); +}*/ + +void SrBvMath::VmV ( srbvvec Vr, const srbvvec V1, const srbvvec V2 ) +{ + Vr[0] = V1[0] - V2[0]; + Vr[1] = V1[1] - V2[1]; + Vr[2] = V1[2] - V2[2]; +} + +void SrBvMath::VpV ( srbvvec Vr, const srbvvec V1, const srbvvec V2 ) +{ + Vr[0] = V1[0] + V2[0]; + Vr[1] = V1[1] + V2[1]; + Vr[2] = V1[2] + V2[2]; +} + +void SrBvMath::VpVxS ( srbvvec Vr, const srbvvec V1, const srbvvec V2, srbvreal s ) +{ + Vr[0] = V1[0] + V2[0] * s; + Vr[1] = V1[1] + V2[1] * s; + Vr[2] = V1[2] + V2[2] * s; +} + +/* void MskewV ( srbvmat M, const srbvvec v ); +void SrBvMath::MskewV ( srbvmat M, const srbvvec v ) +{ + M[0][0] = M[1][1] = M[2][2] = 0.0; + M[1][0] = v[2]; + M[0][1] = -v[2]; + M[0][2] = v[1]; + M[2][0] = -v[1]; + M[1][2] = -v[0]; + M[2][1] = v[0]; +}*/ + +void SrBvMath::VcrossV ( srbvvec Vr, const srbvvec V1, const srbvvec V2 ) +{ + Vr[0] = V1[1]*V2[2] - V1[2]*V2[1]; + Vr[1] = V1[2]*V2[0] - V1[0]*V2[2]; + Vr[2] = V1[0]*V2[1] - V1[1]*V2[0]; +} + +/* srbvreal Vlength ( srbvvec V ); +srbvreal SrBvMath::Vlength ( srbvvec V ) +{ + return sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]); +}*/ + +/* void Vnormalize ( srbvvec V ); +void SrBvMath::Vnormalize ( srbvvec V ) +{ + srbvreal d = srbvreal(1.0) / sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]); + V[0] *= d; + V[1] *= d; + V[2] *= d; +}*/ + +srbvreal SrBvMath::VdotV ( const srbvvec V1, const srbvvec V2 ) +{ + return (V1[0]*V2[0] + V1[1]*V2[1] + V1[2]*V2[2]); +} + + +srbvreal SrBvMath::VdistV2 ( const srbvvec V1, const srbvvec V2 ) +{ + return ( (V1[0]-V2[0]) * (V1[0]-V2[0]) + + (V1[1]-V2[1]) * (V1[1]-V2[1]) + + (V1[2]-V2[2]) * (V1[2]-V2[2])); +} + + +void SrBvMath::VxS ( srbvvec Vr, const srbvvec V, srbvreal s ) +{ + Vr[0] = V[0] * s; + Vr[1] = V[1] * s; + Vr[2] = V[2] * s; +} + +/* void MRotZ ( srbvmat Mr, srbvreal t ); + void MRotX ( srbvmat Mr, srbvreal t ); + void MRotY ( srbvmat Mr, srbvreal t ); + void Mqinverse ( srbvmat Mr, srbvmat M );*/ +/* +void SrBvMath::MRotZ ( srbvmat Mr, srbvreal t) +{ + Mr[0][0] = cos(t); + Mr[1][0] = sin(t); + Mr[0][1] = -Mr[1][0]; + Mr[1][1] = Mr[0][0]; + Mr[2][0] = Mr[2][1] = 0.0; + Mr[0][2] = Mr[1][2] = 0.0; + Mr[2][2] = 1.0; +} + +void SrBvMath::MRotX ( srbvmat Mr, srbvreal t ) +{ + Mr[1][1] = cos(t); + Mr[2][1] = sin(t); + Mr[1][2] = -Mr[2][1]; + Mr[2][2] = Mr[1][1]; + Mr[0][1] = Mr[0][2] = 0.0; + Mr[1][0] = Mr[2][0] = 0.0; + Mr[0][0] = 1.0; +} + +void SrBvMath::MRotY ( srbvmat Mr, srbvreal t ) +{ + Mr[2][2] = cos(t); + Mr[0][2] = sin(t); + Mr[2][0] = -Mr[0][2]; + Mr[0][0] = Mr[2][2]; + Mr[1][2] = Mr[1][0] = 0.0; + Mr[2][1] = Mr[0][1] = 0.0; + Mr[1][1] = 1.0; +} + +void SrBvMath::Mqinverse ( srbvmat Mr, srbvmat M ) +{ + int i,j; + + for(i=0; i<3; i++) + for(j=0; j<3; j++) + { + int i1 = (i+1)%3; + int i2 = (i+2)%3; + int j1 = (j+1)%3; + int j2 = (j+2)%3; + Mr[i][j] = (M[j1][i1]*M[j2][i2] - M[j1][i2]*M[j2][i1]); + } +}*/ + +// Meigen from Numerical Recipes in C + +#define ROTATE(a,i,j,k,l) g=a[i][j]; h=a[k][l]; a[i][j]=g-s*(h+g*tau); a[k][l]=h+s*(g-h*tau) + +void SrBvMath::Meigen ( srbvmat vout, srbvvec dout, srbvmat a ) +{ + int n = 3; + int j,iq,ip,i; + srbvreal tresh,theta,tau,t,sm,s,h,g,c; + int nrot; + srbvvec b, z, d; + srbvmat v; + + Midentity(v); + for(ip=0; ip3 && + fabs(d[ip])+g==fabs(d[ip]) && + fabs(d[iq])+g==fabs(d[iq])) + { a[ip][iq]=0.0; } + else if (fabs(a[ip][iq])>tresh) + { h = d[iq]-d[ip]; + if (fabs(h)+g == fabs(h)) t=(a[ip][iq])/h; + else + { theta=(srbvreal)0.5*h/(a[ip][iq]); + t=(srbvreal)(1.0/(fabs(theta)+sqrt(1.0+theta*theta))); + if (theta < 0.0) t = -t; + } + c=(srbvreal)1.0/sqrt(1+t*t); + s=t*c; + tau=s/((srbvreal)1.0+c); + h=t*a[ip][iq]; + z[ip] -= h; + z[iq] += h; + d[ip] -= h; + d[iq] += h; + a[ip][iq]=0.0; + for(j=0;j +#define isnan _isnan +#endif + +//-------------------------------------------------------------------------- +// SegPoints() +// +// Returns closest points between an segment pair. +// Implemented from an algorithm described in +// +// Vladimir J. Lumelsky, +// On fast computation of distance between line segments. +// In Information Processing Letters, no. 21, pages 55-61, 1985. +//-------------------------------------------------------------------------- +void SrBvMath::SegPoints ( srbvvec VEC, srbvvec X, srbvvec Y, + const srbvvec P, const srbvvec A, + const srbvvec Q, const srbvvec B ) +{ + srbvreal T[3], A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; + srbvreal TMP[3]; + + VmV(T,Q,P); + A_dot_A = VdotV(A,A); + B_dot_B = VdotV(B,B); + A_dot_B = VdotV(A,B); + A_dot_T = VdotV(A,T); + B_dot_T = VdotV(B,T); + + // t parameterizes ray P,A + // u parameterizes ray Q,B + + srbvreal t,u; + + // compute t for the closest point on ray P,A to + // ray Q,B + + srbvreal denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B; + + t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom; + + // clamp result so t is on the segment P,A + + if ((t < 0) || isnan(t)) t = 0; else if (t > 1) t = 1; + + // find u for point on ray Q,B closest to point at t + + u = (t*A_dot_B - B_dot_T) / B_dot_B; + + // if u is on segment Q,B, t and u correspond to + // closest points, otherwise, clamp u, recompute and + // clamp t + + if ((u <= 0) || isnan(u)) { + + VcV(Y, Q); + + t = A_dot_T / A_dot_A; + + if ((t <= 0) || isnan(t)) { + VcV(X, P); + VmV(VEC, Q, P); + } + else if (t >= 1) { + VpV(X, P, A); + VmV(VEC, Q, X); + } + else { + VpVxS(X, P, A, t); + VcrossV(TMP, T, A); + VcrossV(VEC, A, TMP); + } + } + else if (u >= 1) { + + VpV(Y, Q, B); + + t = (A_dot_B + A_dot_T) / A_dot_A; + + if ((t <= 0) || isnan(t)) { + VcV(X, P); + VmV(VEC, Y, P); + } + else if (t >= 1) { + VpV(X, P, A); + VmV(VEC, Y, X); + } + else { + VpVxS(X, P, A, t); + VmV(T, Y, P); + VcrossV(TMP, T, A); + VcrossV(VEC, A, TMP); + } + } + else { + + VpVxS(Y, Q, B, u); + + if ((t <= 0) || isnan(t)) { + VcV(X, P); + VcrossV(TMP, T, B); + VcrossV(VEC, B, TMP); + } + else if (t >= 1) { + VpV(X, P, A); + VmV(T, Q, X); + VcrossV(TMP, T, B); + VcrossV(VEC, B, TMP); + } + else { + VpVxS(X, P, A, t); + VcrossV(VEC, A, B); + if (VdotV(VEC, T) < 0) { + VxS(VEC, VEC, -1); + } + } + } +} + +//-------------------------------------------------------------------------- +// TriDist() +// +// Computes the closest points on two triangles, and returns the +// distance between them. +// +// S and T are the triangles, stored tri[point][dimension]. +// +// If the triangles are disjoint, P and Q give the closest points of +// S and T respectively. However, if the triangles overlap, P and Q +// are basically a random pair of points from the triangles, not +// coincident points on the intersection of the triangles, as might +// be expected. +//-------------------------------------------------------------------------- +srbvreal +SrBvMath::TriDist( srbvvec P, srbvvec Q, + const srbvmat S, const srbvmat T ) +{ + // Compute vectors along the 6 sides + + srbvreal Sv[3][3], Tv[3][3]; + srbvreal VEC[3]; + + VmV(Sv[0],S[1],S[0]); + VmV(Sv[1],S[2],S[1]); + VmV(Sv[2],S[0],S[2]); + + VmV(Tv[0],T[1],T[0]); + VmV(Tv[1],T[2],T[1]); + VmV(Tv[2],T[0],T[2]); + + // For each edge pair, the vector connecting the closest points + // of the edges defines a slab (parallel planes at head and tail + // enclose the slab). If we can show that the off-edge vertex of + // each triangle is outside of the slab, then the closest points + // of the edges are the closest points for the triangles. + // Even if these tests fail, it may be helpful to know the closest + // points found, and whether the triangles were shown disjoint + + srbvreal V[3]; + srbvreal Z[3]; + srbvreal minP[3], minQ[3], mindd; + int shown_disjoint = 0; + + mindd = VdistV2(S[0],T[0]) + 1; // Set first minimum safely high + + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + // Find closest points on edges i & j, plus the + // vector (and distance squared) between these points + + SegPoints(VEC,P,Q,S[i],Sv[i],T[j],Tv[j]); + + VmV(V,Q,P); + srbvreal dd = VdotV(V,V); + + // Verify this closest point pair only if the distance + // squared is less than the minimum found thus far. + + if (dd <= mindd) + { + VcV(minP,P); + VcV(minQ,Q); + mindd = dd; + + VmV(Z,S[(i+2)%3],P); + srbvreal a = VdotV(Z,VEC); + VmV(Z,T[(j+2)%3],Q); + srbvreal b = VdotV(Z,VEC); + + if ((a <= 0) && (b >= 0)) return sqrt(dd); + + srbvreal p = VdotV(V, VEC); + + if (a < 0) a = 0; + if (b > 0) b = 0; + if ((p - a + b) > 0) shown_disjoint = 1; + } + } + } + + // No edge pairs contained the closest points. + // either: + // 1. one of the closest points is a vertex, and the + // other point is interior to a face. + // 2. the triangles are overlapping. + // 3. an edge of one triangle is parallel to the other's face. If + // cases 1 and 2 are not true, then the closest points from the 9 + // edge pairs checks above can be taken as closest points for the + // triangles. + // 4. possibly, the triangles were degenerate. When the + // triangle points are nearly colinear or coincident, one + // of above tests might fail even though the edges tested + // contain the closest points. + + // First check for case 1 + + srbvreal Sn[3], Snl; + VcrossV(Sn,Sv[0],Sv[1]); // Compute normal to S triangle + Snl = VdotV(Sn,Sn); // Compute square of length of normal + + // If cross product is long enough, + + if (Snl > 1e-15) + { + // Get projection lengths of T points + + srbvreal Tp[3]; + + VmV(V,S[0],T[0]); + Tp[0] = VdotV(V,Sn); + + VmV(V,S[0],T[1]); + Tp[1] = VdotV(V,Sn); + + VmV(V,S[0],T[2]); + Tp[2] = VdotV(V,Sn); + + // If Sn is a separating direction, + // find point with smallest projection + + int point = -1; + if ((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) + { + if (Tp[0] < Tp[1]) point = 0; else point = 1; + if (Tp[2] < Tp[point]) point = 2; + } + else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) + { + if (Tp[0] > Tp[1]) point = 0; else point = 1; + if (Tp[2] > Tp[point]) point = 2; + } + + // If Sn is a separating direction, + + if (point >= 0) + { + shown_disjoint = 1; + + // Test whether the point found, when projected onto the + // other triangle, lies within the face. + + VmV(V,T[point],S[0]); + VcrossV(Z,Sn,Sv[0]); + if (VdotV(V,Z) > 0) + { + VmV(V,T[point],S[1]); + VcrossV(Z,Sn,Sv[1]); + if (VdotV(V,Z) > 0) + { + VmV(V,T[point],S[2]); + VcrossV(Z,Sn,Sv[2]); + if (VdotV(V,Z) > 0) + { + // T[point] passed the test - it's a closest point for + // the T triangle; the other point is on the face of S + + VpVxS(P,T[point],Sn,Tp[point]/Snl); + VcV(Q,T[point]); + return sqrt(VdistV2(P,Q)); + } + } + } + } + } + + srbvreal Tn[3], Tnl; + VcrossV(Tn,Tv[0],Tv[1]); + Tnl = VdotV(Tn,Tn); + + if (Tnl > 1e-15) + { + srbvreal Sp[3]; + + VmV(V,T[0],S[0]); + Sp[0] = VdotV(V,Tn); + + VmV(V,T[0],S[1]); + Sp[1] = VdotV(V,Tn); + + VmV(V,T[0],S[2]); + Sp[2] = VdotV(V,Tn); + + int point = -1; + if ((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) + { + if (Sp[0] < Sp[1]) point = 0; else point = 1; + if (Sp[2] < Sp[point]) point = 2; + } + else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) + { + if (Sp[0] > Sp[1]) point = 0; else point = 1; + if (Sp[2] > Sp[point]) point = 2; + } + + if (point >= 0) + { + shown_disjoint = 1; + + VmV(V,S[point],T[0]); + VcrossV(Z,Tn,Tv[0]); + if (VdotV(V,Z) > 0) + { + VmV(V,S[point],T[1]); + VcrossV(Z,Tn,Tv[1]); + if (VdotV(V,Z) > 0) + { + VmV(V,S[point],T[2]); + VcrossV(Z,Tn,Tv[2]); + if (VdotV(V,Z) > 0) + { + VcV(P,S[point]); + VpVxS(Q,S[point],Tn,Sp[point]/Tnl); + return sqrt(VdistV2(P,Q)); + } + } + } + } + } + + // Case 1 can't be shown. + // If one of these tests showed the triangles disjoint, + // we assume case 3 or 4, otherwise we conclude case 2, + // that the triangles overlap. + + if (shown_disjoint) + { + VcV(P,minP); + VcV(Q,minQ); + return sqrt(mindd); + } + else return 0; +} + +/*************************************************************************\ + Copyright 1999 The University of North Carolina at Chapel Hill. + All Rights Reserved. + + Permission to use, copy, modify and distribute this software and its + documentation for educational, research and non-profit purposes, without + fee, and without a written agreement is hereby granted, provided that the + above copyright notice and the following three paragraphs appear in all + copies. + + IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE + LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE + USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY + OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + DAMAGES. + + THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE + PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF + NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, + UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + + The authors may be contacted via: + + US Mail: S. Gottschalk, E. Larsen + Department of Computer Science + Sitterson Hall, CB #3175 + University of N. Carolina + Chapel Hill, NC 27599-3175 + Phone: (919)962-1749 + EMail: geom@cs.unc.edu +\**************************************************************************/ + diff --git a/source/dcdt/se/sr_bv_math.h b/source/dcdt/se/sr_bv_math.h new file mode 100644 index 0000000000..a85281b24d --- /dev/null +++ b/source/dcdt/se/sr_bv_math.h @@ -0,0 +1,72 @@ +/** \file sr_bv_math.h + * BV tree math routines */ + +# ifndef SR_BV_MATH_H +# define SR_BV_MATH_H + +# include "sr.h" + +/*! if the following macro is defined, will use floats instead of doubles + Default is doubles (macro not defined) */ +# ifdef SR_BV_MATH_FLOAT + typedef float srbvreal; +# else + typedef double srbvreal; +# endif + +typedef srbvreal srbvmat[3][3]; +typedef srbvreal srbvvec[3]; + +const srbvreal srbvpi = (srbvreal)SR_PI; + +//============================== SrBvMath =============================== + +/*! This namespace encapsulates a (row-major) matrix/vector code + adapted from the PQP package. Their copyright notice + is displayed in the source file */ +namespace SrBvMath + { + void Vset ( srbvvec V, float* fp ); + void Midentity ( srbvmat M ); // set to id matrix + void Videntity ( srbvvec T ); // set to a null vector + void McM ( srbvmat Mr, const srbvmat M ); // Mr=M + void VcV ( srbvvec Vr, const srbvvec V ); // Vr=V + void McolcV ( srbvvec Vr, const srbvmat M, int c ); // Vr=M.column(c) + void McolcMcol ( srbvmat Mr, int cr, const srbvmat M, int c ); // Vr.column(cr)=M.column(c) + void MxM ( srbvmat Mr, const srbvmat M1, const srbvmat M2 ); // Mr=M1*M2 + void MTxM ( srbvmat Mr, const srbvmat M1, const srbvmat M2 ); // Mr=M1.transpose()*M2 + void MxV ( srbvvec Vr, const srbvmat M1, const srbvvec V1 ); // Vr=M1*V1 + void MxVpV ( srbvvec Vr, const srbvmat M1, const srbvvec V1, const srbvvec V2 ); + void MTxV ( srbvvec Vr, const srbvmat M1, const srbvvec V1 ); + void VmV ( srbvvec Vr, const srbvvec V1, const srbvvec V2 ); + void VpV ( srbvvec Vr, const srbvvec V1, const srbvvec V2 ); + void VpVxS ( srbvvec Vr, const srbvvec V1, const srbvvec V2, srbvreal s ); + void VcrossV ( srbvvec Vr, const srbvvec V1, const srbvvec V2 ); + srbvreal VdotV ( const srbvvec V1, const srbvvec V2 ); + srbvreal VdistV2 ( const srbvvec V1, const srbvvec V2 ); + void VxS ( srbvvec Vr, const srbvvec V, srbvreal s ); + void Meigen ( srbvmat vout, srbvvec dout, srbvmat a ); + + /*! SegPoints returns closest points between a segment pair. + Points x and y are the found closest points. + Parameters p and a are segment 1 origin and vector. + Parameters q and b are segment 2 origin and vector. */ + void SegPoints( srbvvec vec, srbvvec x, srbvvec y, + const srbvvec p, const srbvvec a, + const srbvvec q, const srbvvec b ); + + /*! TriDist computes the closest points on two triangles, and + returns the distance between them. + s and t are the triangles, stored tri[point][dimension]. + If the triangles are disjoint, p and q give the closest + points of s and t respectively. However, if the triangles + overlap, p and q are basically a random pair of points from + the triangles, not coincident points on the intersection of + the triangles, as might be expected. */ + srbvreal TriDist ( srbvvec p, srbvvec q, + const srbvmat s, const srbvmat t ); + } + +//============================== end of file =============================== + +# endif // SR_BV_MATH_H diff --git a/source/dcdt/se/sr_bv_nbody.cpp b/source/dcdt/se/sr_bv_nbody.cpp new file mode 100644 index 0000000000..e39290a231 --- /dev/null +++ b/source/dcdt/se/sr_bv_nbody.cpp @@ -0,0 +1,471 @@ +#include "precompiled.h" +# include +# include "sr_bv_nbody.h" +# include "sr_model.h" +# include "sr_box.h" +# include "sr_mat.h" +# include "sr_trace.h" + +# define SR_USE_TRACE1 // function calls trace + +//============================== SrBvNBody::EndPoint ==================================== + +/* EndPoint stores either the (min_x, min_y, min_z) + or the (max_x, max_y, max_z) end-point of an AABB. + Each instance of EndPoint appears simultaneously in three sorted linked + lists, one for each dimension, as required by the sweep and prune algorithm. + A "min" and the correspoding "max" end-point give us three intervals, one + for each axis, which are actually the projections of the AABB on each of the + three co-ordinate axii. */ +enum MinMax { MIN=1, MAX=2 }; +struct SrBvNBody::EndPoint +{ char minmax; //whether it represents a "MIN" or a "MAX" end-point + srbvreal val[3]; //the coordinates of the EndPoint. + EndPoint* prev[3]; //for maintaining the three linked + EndPoint* next[3]; //lists. + AABB *aabb; //back pointer to the parent AABB. +}; + +//============================== SrBvNBody::AABB ==================================== + +/* Class AABB is used to store information about an axis aligned bounding box. + The AABB here has the same "radius" along all the three axii. As a result + of this, even if the object rotates about some arbitrary point and axis, + the radius of the enclosing AABB need not be changed. + Computing the center and radius of the AABBs: + Let P1, P2, ... , Pn be the position vectors of the end-points of the given + object. Then, the center of the enclosing AABB is the centroid of the object: + center = (P1 + P2 + ... + Pn) / n + The radius is given by: radius = max_i (dist(Pi, center)) */ +struct SrBvNBody::AABB +{ int id; // the id of the enclosed object + srbvreal center[3]; // center of the AABB + srbvreal radius; // radius of the AABB + EndPoint *lo; // the (min_x, min_y, min_z) and + EndPoint *hi; // (max_x, max_y, max_z) corners of the AABB. These point to + // the corresponding nodes in the NBody linked lists. + + AABB ( EndPoint* l, EndPoint* h ) + { lo = l; + lo->minmax = MIN; + lo->aabb = this; + hi = h; + hi->minmax = MAX; + hi->aabb = this; + } + + AABB () + { lo = new EndPoint; + lo->minmax = MIN; + lo->aabb = this; + hi = new EndPoint; + hi->minmax = MAX; + hi->aabb = this; + } + + ~AABB () + { delete lo; + delete hi; + } + + /* min/max holds the min/max endpoints of the box, but considering the + maximum radius as the size in all directions; this allows rotation + of the models w/o the need to recompute the boxes */ + void compute_min_max () + { lo->val[0] = center[0] - radius; + lo->val[1] = center[1] - radius; + lo->val[2] = center[2] - radius; + hi->val[0] = center[0] + radius; + hi->val[1] = center[1] + radius; + hi->val[2] = center[2] + radius; + } + + friend bool overlaps ( AABB* obj1, AABB* obj2 )//check it the two AABBs overlap + { + int coord; + for (coord=0; coord<3; coord++) + { if (obj1->lo->val[coord] < obj2->lo->val[coord]) + { if (obj2->lo->val[coord] > obj1->hi->val[coord]) return false; } + else + { if (obj1->lo->val[coord] > obj2->hi->val[coord]) return false; } + } + return true; + } +}; + +//============================== SrBvNBody ==================================== + +const int REVERSE = 1; //whether the direction of movement of the interval +const int FORWARD = 2; //along the list is forward (FORWARD), reverse (REVERSE) +const int NOCHANGE = 3; //or there is no movement at all (NOCHANGE). + +SrBvNBody::SrBvNBody() //constructor. +{ + _elist[0] = NULL; + _elist[1] = NULL; + _elist[2] = NULL; +} + +SrBvNBody::~SrBvNBody() //destructor. +{ + init (); +} + +void SrBvNBody::init() +{ + while ( _aabb.size()>0 ) + { delete _aabb.pop(); + } + + _elist[0] = NULL; + _elist[1] = NULL; + _elist[2] = NULL; + + overlapping_pairs.init (); +} + +bool SrBvNBody::insert_object ( int id, const SrModel& m ) +{ + SR_TRACE1 ( "insert_object" ); + + // Increase the dynamic array if necessary, and store the new aabb + while ( id>=_aabb.size() ) _aabb.push()=0; + if ( _aabb[id] ) return false; // error: id already exists, so just return + AABB* aabb = new AABB; + _aabb[id] = aabb; + + // Set the id to the given value + aabb->id = id; + + // Get the center of the AABB, instead of the centroid as vcollide does + SrBox box; + m.get_bounding_box(box); + SrPnt boxc = box.center(); + aabb->center[0] = boxc.x; + aabb->center[1] = boxc.y; + aabb->center[2] = boxc.z; + + // The "radius" of the AABB is computed as the maximum distance of the AABB + // center from any of the vertices of the object. + float dist2, distmax=0; + int i, mvsize=m.V.size(); + for ( i=0; idistmax ) distmax=dist2; + } + aabb->radius = (srbvreal) sqrt(double(distmax)); + aabb->radius *= (srbvreal)1.0001; //add a 0.01% buffer. + aabb->compute_min_max (); + + // Now, check the overlap of this AABB with with all other AABBs and + // add the pair to the set of overlapping pairs if reqd. + int size = _aabb.size(); + for ( i=0; iid, i ); + } + + // Now, for each of the three co-ordinates, insert the interval + // in the correspoding list. + int coord; + for ( coord=0; coord<3; coord++ ) + { //first insert the "hi" endpoint. + if ( _elist[coord]==NULL ) // if the list is empty, insert in front. + { _elist[coord] = aabb->hi; + aabb->hi->prev[coord] = aabb->hi->next[coord] = NULL; + } + else //otherwise insert in the correct location (the list is sorted) + { _list_insert ( coord, aabb->hi ); } + + // Now, insert the "lo" endpoint. The list cannot be empty since we + // have already inserted the "hi" endpoint + _list_insert ( coord, aabb->lo ); + } + + SR_TRACE1 ( "insert_object end" ); + return true; +} + +bool SrBvNBody::update_transformation ( int id, const SrMat& m ) +{ + SR_TRACE1 ( "update_transformation" ); + + if ( id>=_aabb.size() ) return false; + if ( !_aabb[id] ) return false; + + AABB *aabb = _aabb[id]; // the given object exists + + // compute the new position of the AABB center + SrPnt c ( aabb->center ); + srbvvec new_center; + new_center[0] = m.get(0)*c.x + m.get(4)*c.y + m.get(8)*c.z + m.get(12); + new_center[1] = m.get(1)*c.x + m.get(5)*c.y + m.get(9)*c.z + m.get(13); + new_center[2] = m.get(2)*c.x + m.get(6)*c.y + m.get(10)*c.z + m.get(14); + + // vcollide version: + //double new_center[3]; // multiply: [trans] * [center] + //new_center[0] = aabb->center[0] * trans[0][0] + aabb->center[1] * trans[0][1] + aabb->center[2] * trans[0][2] + trans[0][3]; + //new_center[1] = aabb->center[0] * trans[1][0] + aabb->center[1] * trans[1][1] + aabb->center[2] * trans[1][2] + trans[1][3]; + //new_center[2] = aabb->center[0] * trans[2][0] + aabb->center[1] * trans[2][1] + aabb->center[2] * trans[2][2] + trans[2][3]; + + // compute the new min and max endpoints + // (aabb min/max values are updated later on) + srbvreal min[3], max[3]; + min[0] = new_center[0] - aabb->radius; + min[1] = new_center[1] - aabb->radius; + min[2] = new_center[2] - aabb->radius; + max[0] = new_center[0] + aabb->radius; + max[1] = new_center[1] + aabb->radius; + max[2] = new_center[2] + aabb->radius; + + // we need these so that we can use the same function overlaps(AABB *, AABB *) + // to check overlap of the newly transformed object with other objects + EndPoint lo, hi; + AABB dummy(&lo,&hi); + lo.val[0]=min[0]; lo.val[1]=min[1]; lo.val[2]=min[2]; + hi.val[0]=max[0]; hi.val[1]=max[1]; hi.val[2]=max[2]; + + //update all the three lists by moving the endpoint to correct position. + EndPoint *temp; + int direction; + int coord; + for ( coord=0; coord<3; coord++ ) + { // set the direction of motion of the endpoint along the list + if ( aabb->lo->val[coord] > min[coord] ) + direction = REVERSE; + else if ( aabb->lo->val[coord] < min[coord] ) + direction = FORWARD; + else + direction = NOCHANGE; + + if ( direction==REVERSE ) // backward motion + { // first update the "lo" endpoint of the interval + if ( aabb->lo->prev[coord]!=NULL ) + { temp = aabb->lo; + while ( (temp != NULL) && (temp->val[coord] > min[coord])) + { if (temp->minmax == MAX) + if (overlaps(temp->aabb, &dummy)) + _add_pair(temp->aabb->id, aabb->id); + temp = temp->prev[coord]; + } + if (temp == NULL) + { aabb->lo->prev[coord]->next[coord] = aabb->lo->next[coord]; + aabb->lo->next[coord]->prev[coord] = aabb->lo->prev[coord]; + aabb->lo->prev[coord] = NULL; + aabb->lo->next[coord] = _elist[coord]; + _elist[coord]->prev[coord] = aabb->lo; + _elist[coord] = aabb->lo; + } + else + { aabb->lo->prev[coord]->next[coord] = aabb->lo->next[coord]; + aabb->lo->next[coord]->prev[coord] = aabb->lo->prev[coord]; + aabb->lo->prev[coord] = temp; + aabb->lo->next[coord] = temp->next[coord]; + temp->next[coord]->prev[coord] = aabb->lo; + temp->next[coord] = aabb->lo; + } + } + aabb->lo->val[coord] = min[coord]; + //then update the "hi" endpoint of the interval. + if (aabb->hi->val[coord] != max[coord]) + { temp = aabb->hi; + while (temp->val[coord] > max[coord]) + { if ( (temp->minmax == MIN) && (overlaps(temp->aabb, aabb)) ) + _del_pair(temp->aabb->id, aabb->id); + temp = temp->prev[coord]; + } + aabb->hi->prev[coord]->next[coord] = aabb->hi->next[coord]; + if (aabb->hi->next[coord] != NULL) + aabb->hi->next[coord]->prev[coord] = aabb->hi->prev[coord]; + aabb->hi->prev[coord] = temp; + aabb->hi->next[coord] = temp->next[coord]; + if (temp->next[coord] != NULL) + temp->next[coord]->prev[coord] = aabb->hi; + temp->next[coord] = aabb->hi; + aabb->hi->val[coord] = max[coord]; + } + } + else if (direction == FORWARD) // forward motion + { // here, we first update the "hi" endpoint + if ( aabb->hi->next[coord] != NULL ) + { temp = aabb->hi; + while ( (temp->next[coord] != NULL) && (temp->val[coord] < max[coord]) ) + { if (temp->minmax == MIN) + if (overlaps(temp->aabb, &dummy)) + _add_pair(temp->aabb->id, aabb->id); + temp = temp->next[coord]; + } + if (temp->val[coord] < max[coord]) + { aabb->hi->prev[coord]->next[coord] = aabb->hi->next[coord]; + aabb->hi->next[coord]->prev[coord] = aabb->hi->prev[coord]; + aabb->hi->prev[coord] = temp; + aabb->hi->next[coord] = NULL; + temp->next[coord] = aabb->hi; + } + else if (aabb->hi->val[coord] != max[coord]) + { aabb->hi->prev[coord]->next[coord] = aabb->hi->next[coord]; + aabb->hi->next[coord]->prev[coord] = aabb->hi->prev[coord]; + aabb->hi->prev[coord] = temp->prev[coord]; + aabb->hi->next[coord] = temp; + temp->prev[coord]->next[coord] = aabb->hi; + temp->prev[coord] = aabb->hi; + } + } + aabb->hi->val[coord] = max[coord]; + //then, update the "lo" endpoint of the interval. + temp = aabb->lo; + while (temp->val[coord] < min[coord]) + { if ( (temp->minmax == MAX) && (overlaps(temp->aabb, aabb)) ) + _del_pair(temp->aabb->id, aabb->id); + temp = temp->next[coord]; + } + if (aabb->lo->prev[coord] != NULL) + { aabb->lo->prev[coord]->next[coord] = aabb->lo->next[coord]; } + else + { _elist[coord] = aabb->lo->next[coord]; } + aabb->lo->next[coord]->prev[coord] = aabb->lo->prev[coord]; + aabb->lo->prev[coord] = temp->prev[coord]; + aabb->lo->next[coord] = temp; + if (temp->prev[coord] != NULL) + { temp->prev[coord]->next[coord] = aabb->lo; } + else + { _elist[coord] = aabb->lo; } + temp->prev[coord] = aabb->lo; + aabb->lo->val[coord] = min[coord]; + } + } + + // make sure AABB destructor does not delete the used static vars: + dummy.lo = 0; + dummy.hi = 0; + + SR_TRACE1 ( "update_transformation end" ); + + return true; +} + +bool SrBvNBody::remove_object ( int id ) +{ + SR_TRACE1 ( "remove_object" ); + + if ( id>=_aabb.size() ) return false; + if ( !_aabb[id] ) return false; + + // 1st get the AABB to be deleted + AABB *aabb = _aabb[id]; + + // Now "remove it" from the AABB array + if ( id+1==_aabb.size() ) + _aabb.pop(); + else + _aabb[id] = NULL; + + // Now we delete all the three intervals from the corresponding lists + int coord; + for ( coord=0; coord<3; coord++ ) + { // first delete the "lo" endpoint of the interval + if ( aabb->lo->prev[coord]==NULL ) + _elist[coord] = aabb->lo->next[coord]; + else + aabb->lo->prev[coord]->next[coord] = aabb->lo->next[coord]; + aabb->lo->next[coord]->prev[coord] = aabb->lo->prev[coord]; + // then, delete the "hi" endpoint + if ( aabb->hi->prev[coord]==NULL ) + _elist[coord] = aabb->hi->next[coord]; + else + aabb->hi->prev[coord]->next[coord] = aabb->hi->next[coord]; + if ( aabb->hi->next[coord]!=NULL ) + aabb->hi->next[coord]->prev[coord] = aabb->hi->prev[coord]; + } + + // Delete all entries involving this id from the set of overlapping pairs + overlapping_pairs.del_pairs_with_id ( id ); + + // de-allocate the memory + delete aabb; + + SR_TRACE1 ( "remove_object end" ); + + return true; +} + +//=========================== private methods =================================== + +void SrBvNBody::_list_insert ( int coord, EndPoint* newpt ) + { + // Find the correct location in the list and insert there (the list is sorted) + EndPoint* current = _elist[coord]; + while ( current->next[coord] && current->val[coord]val[coord] ) + current = current->next[coord]; + if ( current->val[coord]>=newpt->val[coord] ) // insert before + { newpt->prev[coord] = current->prev[coord]; + newpt->next[coord] = current; + if ( !current->prev[coord] ) + _elist[coord] = newpt; + else + current->prev[coord]->next[coord] = newpt; + current->prev[coord] = newpt; + } + else // insert at the end + { newpt->prev[coord] = current; + newpt->next[coord] = NULL; + current->next[coord] = newpt; + } + } + +/************************************************************************\ + + Copyright 1997 The University of North Carolina at Chapel Hill. + All Rights Reserved. + + Permission to use, copy, modify and distribute this software + and its documentation for educational, research and non-profit + purposes, without fee, and without a written agreement is + hereby granted, provided that the above copyright notice and + the following three paragraphs appear in all copies. + + IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL + HILL BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, + INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, + ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, + EVEN IF THE UNIVERSITY OF NORTH CAROLINA HAVE BEEN ADVISED OF + THE POSSIBILITY OF SUCH DAMAGES. + + + Permission to use, copy, modify and distribute this software + and its documentation for educational, research and non-profit + purposes, without fee, and without a written agreement is + hereby granted, provided that the above copyright notice and + the following three paragraphs appear in all copies. + + THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" + BASIS, AND THE UNIVERSITY OF NORTH CAROLINA HAS NO OBLIGATION + TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR + MODIFICATIONS. + + + --------------------------------- + |Please send all BUG REPORTS to: | + | | + | geom@cs.unc.edu | + | | + --------------------------------- + + + The authors may be contacted via: + + US Mail: A. Pattekar/J. Cohen/T. Hudson/S. Gottschalk/M. Lin/D. Manocha + Department of Computer Science + Sitterson Hall, CB #3175 + University of N. Carolina + Chapel Hill, NC 27599-3175 + + Phone: (919)962-1749 + + EMail: geom@cs.unc.edu + +\************************************************************************/ diff --git a/source/dcdt/se/sr_bv_nbody.h b/source/dcdt/se/sr_bv_nbody.h new file mode 100644 index 0000000000..dd9c35a6a5 --- /dev/null +++ b/source/dcdt/se/sr_bv_nbody.h @@ -0,0 +1,65 @@ +/** \file sr_bv_nbody.h + * multibody AABB seep and prune */ + +# ifndef SR_BV_NBODY_H +# define SR_BV_NBODY_H + +# include "sr_bv_math.h" +# include "sr_bv_id_pairs.h" + +class SrModel; +class SrMat; + +/*! Maintains a set of three linked lists and keeps updating them using the + sweep and prune algorithm. At any point in execution, the class maintains + a set of pairs of objects whose AABBs overlap. In general, this set would + be very small compared to the n(n-1)/2 possible pairs. + Adapted from the VCollide package. Their copyright is in the source file. */ +class SrBvNBody +{ private: + struct AABB; + struct EndPoint; + EndPoint *_elist[3]; //Pointers to the three linked lists, one per axis + SrArray _aabb; //Element "i" points to the AABB of the object with id=i. If + // this object doesn't exist, then the pointer is NULL. + public: + /*! at any instance during execution, overlapping_pairs contains the set + of pairs of ids whose AABBs overlap. */ + SrBvIdPairs overlapping_pairs; + + public: + /*! Constructor */ + SrBvNBody(); + + /*! Destructor (calls init()) */ + ~SrBvNBody(); + + /*! Clears everything */ + void init (); + + /*! Add an object with the given id to the NBody data structure. + If the id already exists, nothing is done and false is returned. + Otherwise, the object is added and true is returned. + Note: to change a model associated to an existing id, delete_object() + must be called prior to add_object() */ + bool insert_object ( int id, const SrModel& m ); + + /*! Update position and orientation of the associated object, and + the 3 per-axis linked lists maintaining the ordered AABB endpoints. + False is returned if the id is invalid; true is returned otherwise. */ + bool update_transformation ( int id, const SrMat& m ); + + /*! Delete the object from the data structure. Returns true if the + object could be deleted, and false otherwise. False may only + occur if the id is invalid or represents an already deleted object */ + bool remove_object ( int id ); + + private: + void _list_insert ( int coord, EndPoint* newpt ); + void _add_pair(int id1, int id2) + { if (id1 != id2) overlapping_pairs.add_pair(id1, id2); } + void _del_pair(int id1, int id2) + { if (id1 != id2) overlapping_pairs.del_pair(id1, id2); } +}; + +#endif // SR_BV_NBODY_H diff --git a/source/dcdt/se/sr_bv_tree.cpp b/source/dcdt/se/sr_bv_tree.cpp new file mode 100644 index 0000000000..e8cf4f0c01 --- /dev/null +++ b/source/dcdt/se/sr_bv_tree.cpp @@ -0,0 +1,331 @@ +#include "precompiled.h"/* Note: this code was adapted from the PQP library; + their copyright notice can be found at the end of this file. */ + +# include "sr_bv_tree.h" +# include "sr_model.h" + +//==================================== +//======= SrBvTree::SrBvTree ========= +//==================================== + +SrBvTree::SrBvTree () + { + _last_tri=0; + } + +//===================================== +//======= SrBvTree::~SrBvTree ========= +//===================================== +SrBvTree::~SrBvTree () + { + } + +//================================ +//======= SrBvTree::init ========= +//================================ +void SrBvTree::init () + { + _tris.size(0); _tris.compress(); + _bvs.size(0); _bvs.compress(); + _last_tri=0; + } + +//==================================== +//======= SrBvTree::make ========= +//==================================== +void SrBvTree::make ( const SrModel& m ) + { + using namespace SrBvMath; + + if ( m.F.size()==0 ) { init(); return; } + + // get the geometry of the model: + _tris.size ( m.F.size() ); + int i; + for ( i=0; i<_tris.size(); i++ ) + { Vset ( _tris[i].p1, m.V[m.F[i].a] ); + Vset ( _tris[i].p2, m.V[m.F[i].b] ); + Vset ( _tris[i].p3, m.V[m.F[i].c] ); + _tris[i].id = i; + } + _last_tri = &_tris[0]; + + // create an array of BVs for the model: + _bvs.ensure_capacity ( 2*_tris.size()-1 ); + _bvs.size ( 1 ); + + // build recursively the tree: + _build_recurse ( 0/*node*/, 0/*first*/, _tris.size()/*num*/ ); + + // change BV orientations from world-relative to parent-relative: + srbvmat R; srbvvec T; + Midentity(R); + Videntity(T); + _make_parent_relative(0,R,T,T); + + // make sure the sizes were well defined: + _tris.compress(); + _bvs.compress(); + } + +static void get_centroid_triverts ( srbvvec c, SrBvTri *tris, int num_tris ) +{ + int i; + + c[0] = c[1] = c[2] = 0.0; + + // get center of mass + for(i=0; ichild(bn) to the num_tris triangles starting at first_tri +// Then, if num_tris is greater than one, partitions the tris into two +// sets, and recursively builds two children of m->child(bn) +void SrBvTree::_build_recurse ( int bn, int first_tri, int num_tris ) +{ + using namespace SrBvMath; + + SrBv* b = child ( bn ); + + // compute a rotation matrix + + srbvreal C[3][3], E[3][3], R[3][3], s[3], axis[3], mean[3], coord; + + get_covariance_triverts(C,&_tris[first_tri],num_tris); + + Meigen(E, s, C); + + // place axes of E in order of increasing s + + int min, mid, max; + if (s[0] > s[1]) { max = 0; min = 1; } + else { min = 0; max = 1; } + if (s[2] < s[min]) { mid = min; min = 2; } + else if (s[2] > s[max]) { mid = max; max = 2; } + else { mid = 2; } + McolcMcol(R,0,E,max); + McolcMcol(R,1,E,mid); + R[0][2] = E[1][max]*E[2][mid] - E[1][mid]*E[2][max]; + R[1][2] = E[0][mid]*E[2][max] - E[0][max]*E[2][mid]; + R[2][2] = E[0][max]*E[1][mid] - E[0][mid]*E[1][max]; + + // fit the BV + b->fit ( R, &_tris[first_tri], num_tris ); + + if (num_tris == 1) + { + // BV is a leaf BV - first_child will index a triangle + b->first_child = -(first_tri + 1); + } + else if (num_tris > 1) + { + // BV not a leaf - first_child will index a BV + b->first_child = _bvs.size(); + _bvs.push(); + _bvs.push(); + + // choose splitting axis and splitting coord + McolcV(axis,R,0); + + get_centroid_triverts(mean,&_tris[first_tri],num_tris); + coord = VdotV(axis, mean); + + // now split + int num_first_half = split_tris(&_tris[first_tri], num_tris, + axis, coord); + + // recursively build the children + _build_recurse( child(bn)->first_child, first_tri, num_first_half); + _build_recurse( child(bn)->first_child + 1, + first_tri + num_first_half, num_tris - num_first_half); + } +} + +// this descends the hierarchy, converting world-relative +// transforms to parent-relative transforms +void SrBvTree::_make_parent_relative ( int bn, + const srbvmat parentR, + const srbvvec parentTr, + const srbvvec parentTo ) +{ + srbvmat Rpc; + srbvvec Tpc; + + if ( !child(bn)->leaf() ) + { // make children parent-relative + _make_parent_relative ( child(bn)->first_child, + child(bn)->R, + child(bn)->Tr, + child(bn)->To + ); + _make_parent_relative ( child(bn)->first_child+1, + child(bn)->R, + child(bn)->Tr, + child(bn)->To + ); + } + + // make self parent relative + using namespace SrBvMath; + MTxM(Rpc,parentR,child(bn)->R); + McM(child(bn)->R,Rpc); + VmV(Tpc,child(bn)->Tr,parentTr); + MTxV(child(bn)->Tr,parentR,Tpc); + VmV(Tpc,child(bn)->To,parentTo); + MTxV(child(bn)->To,parentR,Tpc); +} + +/*************************************************************************\ + Copyright 1999 The University of North Carolina at Chapel Hill. + All Rights Reserved. + + Permission to use, copy, modify and distribute this software and its + documentation for educational, research and non-profit purposes, without + fee, and without a written agreement is hereby granted, provided that the + above copyright notice and the following three paragraphs appear in all + copies. + + IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE + LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE + USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY + OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + DAMAGES. + + THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE + PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF + NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, + UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + + The authors may be contacted via: + + US Mail: S. Gottschalk, E. Larsen + Department of Computer Science + Sitterson Hall, CB #3175 + University of N. Carolina + Chapel Hill, NC 27599-3175 + Phone: (919)962-1749 + EMail: geom@cs.unc.edu +\**************************************************************************/ diff --git a/source/dcdt/se/sr_bv_tree.h b/source/dcdt/se/sr_bv_tree.h new file mode 100644 index 0000000000..063b540d90 --- /dev/null +++ b/source/dcdt/se/sr_bv_tree.h @@ -0,0 +1,87 @@ +/** \file sr_bv_tree.h + * bounding volume tree */ + +# ifndef SR_BV_TREE_H +# define SR_BV_TREE_H + +# include "sr_array.h" +# include "sr_bv_math.h" +# include "sr_class_manager.h" + +class SrModel; +class SrBvTreeQuery; + +/*! Encapsulates the coordinates of a triangle. */ +struct SrBvTri + { srbvvec p1, p2, p3; // the three coordinates + int id; // the id of the face of the original SrModel + }; + +/*! Encapsulates a bounding volume node. + Adapted from PQP, see the copyright notice in the source file. */ +struct SrBv + { srbvmat R; // orientation of RSS & OBB + srbvvec Tr; // RSS: position of rectangle + srbvreal l[2]; // RSS: side lengths of rectangle + srbvreal r; // RSS: radius of sphere summed with rectangle to form RSS + srbvvec To; // OBB: position of obb + srbvvec d; // OBB: (half) dimensions of obb + int first_child; // positive value is index of first_child bv + // negative value is -(index + 1) of triangle + SrBv() { first_child=0; } + bool leaf() const { return first_child<0? true:false; } + srbvreal size() const; + void fit ( srbvmat O, SrBvTri* tris, int num_tris ); + static bool overlap ( srbvmat R, srbvvec T, const SrBv* b1, const SrBv* b2 ); + static srbvreal distance ( srbvmat R, srbvvec T, const SrBv* b1, const SrBv* b2 ); + }; + +/*! \class SrBvTree sr_bv_tree.h + \brief bounding volume tree + Manages a hierarchy of bounding volumes, both OBB and RSS are + maintained. OBBs are used for collision detection and RSSs are + used for distance computation. + Adapted from PQP, see the copyright notice in the source file. */ +class SrBvTree + { private: + SrArray _tris; + SrArray _bvs; + SrBvTri* _last_tri; // closest tri on this model in last distance test + friend class SrBvTreeQuery; + + public : + /*! Constructor */ + SrBvTree (); + + /*! Destructor */ + ~SrBvTree (); + + /*! Returns the number of bounding volumes in the current tree */ + int nodes () const { return _bvs.size(); } + + /*! Returns the triangle with index i */ + const SrBvTri* tri ( int i ) const { return &_tris[i]; } + + /*! Returns a const copy of the list of triangles */ + const SrArray& tris () const { return _tris; } + + /*! Returns the bounding volume node with index i */ + SrBv* child ( int i ) { return &_bvs[i]; } + const SrBv* const_child ( int i ) const { return &_bvs[i]; } + + /*! Empty the tree and frees all used memory */ + void init (); + + /*! Constructs the tree for the given model */ + void make ( const SrModel& m ); + + private : + void _build_recurse ( int bn, int first_tri, int num_tris ); + void _make_parent_relative ( int bn, const srbvmat parentR, + const srbvvec parentTr, + const srbvvec parentTo ); + }; + +//============================== end of file =============================== + +# endif // SR_BV_TREE_H diff --git a/source/dcdt/se/sr_bv_tree_query.cpp b/source/dcdt/se/sr_bv_tree_query.cpp new file mode 100644 index 0000000000..7306135d76 --- /dev/null +++ b/source/dcdt/se/sr_bv_tree_query.cpp @@ -0,0 +1,839 @@ +#include "precompiled.h"/* Note: this code was adapted from the PQP library; + their copyright notice can be found at the end of this file. */ + +# include "sr_bv_tree_query.h" + +//====================== SrBvTreeQuery ====================== + +SrBvTreeQuery::SrBvTreeQuery () + { + init (); + } + +void SrBvTreeQuery::init () + { + _num_bv_tests = 0; + _num_tri_tests = 0; + _pairs.size(0); + _pairs.compress(); + _dist = 0; + _rel_err = _abs_err = 0; + _closer_than_tolerance = false; + _toler = 0; + } + +inline void setmv ( srbvmat m, srbvvec v, const SrMat& srm ) + { + # define SRM(i) (srbvreal)(srm[i]) + // here we pass from column-major to line-major order: + m[0][0]=SRM(0); m[1][0]=SRM(1); m[2][0]=SRM(2); + m[0][1]=SRM(4); m[1][1]=SRM(5); m[2][1]=SRM(6); + m[0][2]=SRM(8); m[1][2]=SRM(9); m[2][2]=SRM(10); + v[0]=SRM(12); v[1]=SRM(13); v[2]=SRM(14); + # undef SRM + } + + +bool SrBvTreeQuery::collide ( const SrMat& m1, const SrBvTree* t1, + const SrMat& m2, const SrBvTree* t2 ) + { + srbvmat R1, R2; + srbvvec T1, T2; + setmv ( R1, T1, m1 ); + setmv ( R2, T2, m2 ); + collide ( R1, T1, t1, R2, T2, t2, CollideFirstContact ); + return _pairs.size()>0? true:false; + } + +int SrBvTreeQuery::collide_all ( const SrMat& m1, const SrBvTree* t1, + const SrMat& m2, const SrBvTree* t2 ) + { + srbvmat R1, R2; + srbvvec T1, T2; + setmv ( R1, T1, m1 ); + setmv ( R2, T2, m2 ); + collide ( R1, T1, t1, R2, T2, t2, CollideAllContacts ); + return _pairs.size(); + } + +float SrBvTreeQuery::distance ( const SrMat& m1, SrBvTree* t1, + const SrMat& m2, SrBvTree* t2, + float rel_err, float abs_err ) + { + srbvmat R1, R2; + srbvvec T1, T2; + setmv ( R1, T1, m1 ); + setmv ( R2, T2, m2 ); + _distance ( R1, T1, t1, R2, T2, t2, rel_err, abs_err ); + _srp1.set ( _p1 ); + _srp2.set ( _p2 ); + return distance(); + } + +bool SrBvTreeQuery::tolerance ( const SrMat& m1, SrBvTree* t1, + const SrMat& m2, SrBvTree* t2, + float tolerance ) + { + srbvmat R1, R2; + srbvvec T1, T2; + setmv ( R1, T1, m1 ); + setmv ( R2, T2, m2 ); + return SrBvTreeQuery::tolerance ( R1, T1, t1, R2, T2, t2, tolerance ); + } + +float SrBvTreeQuery::greedy_distance ( const SrMat& m1, SrBvTree* t1, + const SrMat& m2, SrBvTree* t2, float delta ) + { + srbvmat R1, R2; + srbvvec T1, T2; + setmv ( R1, T1, m1 ); + setmv ( R2, T2, m2 ); + return SrBvTreeQuery::greedy_distance ( R1, T1, t1, R2, T2, t2, delta ); + } + +//====================== static functions ====================== + +inline srbvreal max ( srbvreal a, srbvreal b, srbvreal c ) +{ + srbvreal t = a; + if (b > t) t = b; + if (c > t) t = c; + return t; +} + +inline srbvreal min ( srbvreal a, srbvreal b, srbvreal c ) +{ + srbvreal t = a; + if (b < t) t = b; + if (c < t) t = c; + return t; +} + +static int project6 ( srbvreal *ax, + srbvreal *p1, srbvreal *p2, srbvreal *p3, + srbvreal *q1, srbvreal *q2, srbvreal *q3 ) +{ + using namespace SrBvMath; + srbvreal P1 = VdotV(ax, p1); + srbvreal P2 = VdotV(ax, p2); + srbvreal P3 = VdotV(ax, p3); + srbvreal Q1 = VdotV(ax, q1); + srbvreal Q2 = VdotV(ax, q2); + srbvreal Q3 = VdotV(ax, q3); + + srbvreal mx1 = max(P1, P2, P3); + srbvreal mn1 = min(P1, P2, P3); + srbvreal mx2 = max(Q1, Q2, Q3); + srbvreal mn2 = min(Q1, Q2, Q3); + + if (mn1 > mx2) return 0; + if (mn2 > mx1) return 0; + return 1; +} + +// very robust triangle intersection test +// uses no divisions +// works on coplanar triangles +static int TriContact( + const srbvreal *P1, const srbvreal *P2, const srbvreal *P3, + const srbvreal *Q1, const srbvreal *Q2, const srbvreal *Q3 ) +{ + // One triangle is (p1,p2,p3). Other is (q1,q2,q3). + // Edges are (e1,e2,e3) and (f1,f2,f3). + // Normals are n1 and m1 + // Outwards are (g1,g2,g3) and (h1,h2,h3). + // + // We assume that the triangle vertices are in the same coordinate system. + // + // First thing we do is establish a new c.s. so that p1 is at (0,0,0). + + srbvreal p1[3], p2[3], p3[3]; + srbvreal q1[3], q2[3], q3[3]; + srbvreal e1[3], e2[3], e3[3]; + srbvreal f1[3], f2[3], f3[3]; + srbvreal g1[3], g2[3], g3[3]; + srbvreal h1[3], h2[3], h3[3]; + srbvreal n1[3], m1[3]; + + srbvreal ef11[3], ef12[3], ef13[3]; + srbvreal ef21[3], ef22[3], ef23[3]; + srbvreal ef31[3], ef32[3], ef33[3]; + + p1[0] = P1[0] - P1[0]; p1[1] = P1[1] - P1[1]; p1[2] = P1[2] - P1[2]; + p2[0] = P2[0] - P1[0]; p2[1] = P2[1] - P1[1]; p2[2] = P2[2] - P1[2]; + p3[0] = P3[0] - P1[0]; p3[1] = P3[1] - P1[1]; p3[2] = P3[2] - P1[2]; + + q1[0] = Q1[0] - P1[0]; q1[1] = Q1[1] - P1[1]; q1[2] = Q1[2] - P1[2]; + q2[0] = Q2[0] - P1[0]; q2[1] = Q2[1] - P1[1]; q2[2] = Q2[2] - P1[2]; + q3[0] = Q3[0] - P1[0]; q3[1] = Q3[1] - P1[1]; q3[2] = Q3[2] - P1[2]; + + e1[0] = p2[0] - p1[0]; e1[1] = p2[1] - p1[1]; e1[2] = p2[2] - p1[2]; + e2[0] = p3[0] - p2[0]; e2[1] = p3[1] - p2[1]; e2[2] = p3[2] - p2[2]; + e3[0] = p1[0] - p3[0]; e3[1] = p1[1] - p3[1]; e3[2] = p1[2] - p3[2]; + + f1[0] = q2[0] - q1[0]; f1[1] = q2[1] - q1[1]; f1[2] = q2[2] - q1[2]; + f2[0] = q3[0] - q2[0]; f2[1] = q3[1] - q2[1]; f2[2] = q3[2] - q2[2]; + f3[0] = q1[0] - q3[0]; f3[1] = q1[1] - q3[1]; f3[2] = q1[2] - q3[2]; + + using namespace SrBvMath; + + VcrossV(n1, e1, e2); + VcrossV(m1, f1, f2); + + VcrossV(g1, e1, n1); + VcrossV(g2, e2, n1); + VcrossV(g3, e3, n1); + VcrossV(h1, f1, m1); + VcrossV(h2, f2, m1); + VcrossV(h3, f3, m1); + + VcrossV(ef11, e1, f1); + VcrossV(ef12, e1, f2); + VcrossV(ef13, e1, f3); + VcrossV(ef21, e2, f1); + VcrossV(ef22, e2, f2); + VcrossV(ef23, e2, f3); + VcrossV(ef31, e3, f1); + VcrossV(ef32, e3, f2); + VcrossV(ef33, e3, f3); + + // now begin the series of tests + + if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0; + + if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0; + + if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0; + if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0; + + return 1; +} + +/**************************/ +/******* COLLIDE **********/ +/**************************/ + +void SrBvTreeQuery::collide ( + srbvmat R1, srbvvec T1, const SrBvTree* o1, + srbvmat R2, srbvvec T2, const SrBvTree* o2, + CollideFlag flag ) +{ + // clear the data: + _num_bv_tests = 0; + _num_tri_tests = 0; + _pairs.size(0); + + // make sure that the models are built: + if ( o1->nodes()==0 || o2->nodes()==0 ) return; + + // Okay, compute what transform [R,T] that takes us from cs1 to cs2. + // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] + // First compute the rotation part, then translation part + using namespace SrBvMath; + MTxM(_R,R1,R2); + srbvvec Ttemp; + VmV(Ttemp, T2, T1); + MTxV(_T, R1, Ttemp); + + // compute the transform from o1->child(0) to o2->child(0) + srbvmat Rtemp, R; + srbvvec T; + MxM(Rtemp,_R,o2->const_child(0)->R); + MTxM(R,o1->const_child(0)->R,Rtemp); + MxVpV(Ttemp,_R,o2->const_child(0)->To,_T); + VmV(Ttemp,Ttemp,o1->const_child(0)->To); + MTxV(T,o1->const_child(0)->R,Ttemp); + + // now start with both top level BVs + _collide_recurse ( R, T, o1, 0, o2, 0, flag ); +} + +float SrBvTreeQuery::greedy_distance ( srbvmat R1, srbvvec T1, SrBvTree* o1, + srbvmat R2, srbvvec T2, SrBvTree* o2, float delta ) + { + using namespace SrBvMath; + MTxM(_R,R1,R2); + double Ttemp[3]; + VmV(Ttemp, T2, T1); + MTxV(_T, R1, Ttemp); + + _num_bv_tests = 0; + _num_tri_tests = 0; + + srbvmat Rtemp, R; + srbvvec T; + + MxM(Rtemp,_R,o2->child(0)->R); + MTxM(R,o1->child(0)->R,Rtemp); + + MxVpV(Ttemp,_R,o2->child(0)->Tr,_T); + VmV(Ttemp,Ttemp,o1->child(0)->Tr); + MTxV(T,o1->child(0)->R,Ttemp); + + // first, see if top-level BVs are separated + _num_bv_tests += 1; + srbvreal d = SrBv::distance(R, T, o1->child(0), o2->child(0)); + + // if we already found separation don't bother recursing to refine it + if ( float(d) > delta ) + return (float)d; + else // if top-level BVs are not separated, recurse + return (float)_greedy_distance_recurse(R,T,o1,0,o2,0,delta); + } + +//====================== private methods ====================== + +inline +srbvreal +TriDistance(srbvreal R[3][3], srbvreal T[3], const SrBvTri *t1, const SrBvTri *t2, + srbvreal p[3], srbvreal q[3]) +{ + // transform tri 2 into same space as tri 1 + + srbvreal tri1[3][3], tri2[3][3]; + using namespace SrBvMath; + VcV(tri1[0], t1->p1); + VcV(tri1[1], t1->p2); + VcV(tri1[2], t1->p3); + MxVpV(tri2[0], R, t2->p1, T); + MxVpV(tri2[1], R, t2->p2, T); + MxVpV(tri2[2], R, t2->p3, T); + + return TriDist(p,q,tri1,tri2); +} + +void SrBvTreeQuery::_collide_recurse ( + srbvmat R, srbvvec T, // b2 relative to b1 + const SrBvTree* o1, int b1, + const SrBvTree* o2, int b2, int flag) +{ + // first thing, see if we're overlapping + + _num_bv_tests++; + + if ( !SrBv::overlap(R,T,o1->const_child(b1), o2->const_child(b2))) return; + + // if we are, see if we test triangles next + + bool l1 = o1->const_child(b1)->leaf(); + bool l2 = o2->const_child(b2)->leaf(); + using namespace SrBvMath; + + if (l1 && l2) + { + _num_tri_tests++; + + // transform the points in b2 into space of b1, then compare + const SrBvTri *t1 = o1->tri(-o1->const_child(b1)->first_child - 1); + const SrBvTri *t2 = o2->tri(-o2->const_child(b2)->first_child - 1); + srbvreal q1[3], q2[3], q3[3]; + const srbvreal *p1 = t1->p1; + const srbvreal *p2 = t1->p2; + const srbvreal *p3 = t1->p3; + MxVpV(q1, _R, t2->p1, _T); + MxVpV(q2, _R, t2->p2, _T); + MxVpV(q3, _R, t2->p3, _T); + if (TriContact(p1, p2, p3, q1, q2, q3)) + { // add this to result + _pairs.push() = t1->id; + _pairs.push() = t2->id; + } + return; + } + + // we dont, so decide whose children to visit next + + srbvreal sz1 = o1->const_child(b1)->size(); + srbvreal sz2 = o2->const_child(b2)->size(); + + srbvreal Rc[3][3],Tc[3],Ttemp[3]; + + if (l2 || (!l1 && (sz1 > sz2))) + { + int c1 = o1->const_child(b1)->first_child; + int c2 = c1 + 1; + + MTxM(Rc,o1->const_child(c1)->R,R); + VmV(Ttemp,T,o1->const_child(c1)->To); + MTxV(Tc,o1->const_child(c1)->R,Ttemp); + _collide_recurse(Rc,Tc,o1,c1,o2,b2,flag); + + if ( flag==CollideFirstContact && _pairs.size()>0 ) return; + + MTxM(Rc,o1->const_child(c2)->R,R); + VmV(Ttemp,T,o1->const_child(c2)->To); + MTxV(Tc,o1->const_child(c2)->R,Ttemp); + _collide_recurse(Rc,Tc,o1,c2,o2,b2,flag); + } + else + { + int c1 = o2->const_child(b2)->first_child; + int c2 = c1 + 1; + + MxM(Rc,R,o2->const_child(c1)->R); + MxVpV(Tc,R,o2->const_child(c1)->To,T); + _collide_recurse(Rc,Tc,o1,b1,o2,c1,flag); + + if ( flag==CollideFirstContact && _pairs.size()>0 ) return; + + MxM(Rc,R,o2->const_child(c2)->R); + MxVpV(Tc,R,o2->const_child(c2)->To,T); + _collide_recurse(Rc,Tc,o1,b1,o2,c2,flag); + } +} + +/***************************/ +/******* DISTANCE **********/ +/***************************/ +void SrBvTreeQuery::_distance ( srbvmat R1, srbvvec T1, SrBvTree* o1, + srbvmat R2, srbvvec T2, SrBvTree* o2, + srbvreal rel_err, srbvreal abs_err ) + { + // init data: + _dist = 0; + _num_bv_tests = 0; + _num_tri_tests = 0; + _abs_err = abs_err; + _rel_err = rel_err; + + // make sure that the models are built: + if ( o1->nodes()==0 || o2->nodes()==0 ) return; + + // Okay, compute what transform [R,T] that takes us from cs2 to cs1. + // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] + // First compute the rotation part, then translation part + using namespace SrBvMath; + + MTxM(_R,R1,R2); + srbvreal Ttemp[3]; + VmV(Ttemp, T2, T1); + MTxV(_T, R1, Ttemp); + + // establish initial upper bound using last triangles which + // provided the minimum distance + srbvvec p,q; + _dist = TriDistance(_R,_T,o1->_last_tri,o2->_last_tri,p,q); + VcV(_p1,p); + VcV(_p2,q); + + // compute the transform from o1->child(0) to o2->child(0) + srbvreal Rtemp[3][3], R[3][3], T[3]; + + MxM(Rtemp,_R,o2->const_child(0)->R); + MTxM(R,o1->const_child(0)->R,Rtemp); + + MxVpV(Ttemp,_R,o2->const_child(0)->Tr,_T); + VmV(Ttemp,Ttemp,o1->const_child(0)->Tr); + MTxV(T,o1->const_child(0)->R,Ttemp); + + _distance_recurse(R,T,o1,0,o2,0); + + // res->p2 is in cs 1 ; transform it to cs 2 + srbvvec u; + VmV(u, _p2, _T); + MTxV(_p2, _R, u); + } + +void SrBvTreeQuery::_distance_recurse ( + srbvmat R, srbvvec T, // b2 relative to b1 + SrBvTree* o1, int b1, + SrBvTree* o2, int b2 ) +{ + using namespace SrBvMath; + + srbvreal sz1 = o1->const_child(b1)->size(); + srbvreal sz2 = o2->const_child(b2)->size(); + bool l1 = o1->const_child(b1)->leaf(); + bool l2 = o2->const_child(b2)->leaf(); + + if ( l1 && l2 ) + { // both leaves. Test the triangles beneath them. + _num_tri_tests++; + + srbvreal p[3], q[3]; + + SrBvTri *t1 = &o1->_tris[-o1->child(b1)->first_child - 1]; + SrBvTri *t2 = &o2->_tris[-o2->child(b2)->first_child - 1]; + + srbvreal d = TriDistance(_R,_T,t1,t2,p,q); + + if ( d<_dist ) + { + _dist = d; + + VcV(_p1, p); // p already in c.s. 1 + VcV(_p2, q); // q must be transformed + // into c.s. 2 later + o1->_last_tri = t1; + o2->_last_tri = t2; + } + return; + } + + // First, perform distance tests on the children. Then traverse + // them recursively, but test the closer pair first, the further + // pair second. + + int a1,a2,c1,c2; // new bv tests 'a' and 'c' + srbvreal R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3]; + + if (l2 || (!l1 && (sz1 > sz2))) + { + // visit the children of b1 + + a1 = o1->child(b1)->first_child; + a2 = b2; + c1 = o1->child(b1)->first_child+1; + c2 = b2; + + MTxM(R1,o1->const_child(a1)->R,R); + VmV(Ttemp,T,o1->const_child(a1)->Tr); + MTxV(T1,o1->const_child(a1)->R,Ttemp); + + MTxM(R2,o1->const_child(c1)->R,R); + VmV(Ttemp,T,o1->const_child(c1)->Tr); + MTxV(T2,o1->const_child(c1)->R,Ttemp); + } + else + { + // visit the children of b2 + a1 = b1; + a2 = o2->child(b2)->first_child; + c1 = b1; + c2 = o2->child(b2)->first_child+1; + + MxM(R1,R,o2->const_child(a2)->R); + MxVpV(T1,R,o2->const_child(a2)->Tr,T); + + MxM(R2,R,o2->const_child(c2)->R); + MxVpV(T2,R,o2->const_child(c2)->Tr,T); + } + + _num_bv_tests += 2; + + srbvreal d1 = SrBv::distance(R1, T1, o1->const_child(a1), o2->const_child(a2)); + srbvreal d2 = SrBv::distance(R2, T2, o1->const_child(c1), o2->const_child(c2)); + + if (d2 < d1) + { + if ((d2 < (_dist - _abs_err)) || + (d2*(1 + _rel_err) < _dist)) + { + _distance_recurse(R2, T2, o1, c1, o2, c2); + } + + if ((d1 < (_dist - _abs_err)) || + (d1*(1 + _rel_err) < _dist)) + { + _distance_recurse(R1, T1, o1, a1, o2, a2); + } + } + else + { + if ((d1 < (_dist - _abs_err)) || + (d1*(1 + _rel_err) < _dist)) + { + _distance_recurse(R1, T1, o1, a1, o2, a2); + } + + if ((d2 < (_dist - _abs_err)) || + (d2*(1 + _rel_err) < _dist)) + { + _distance_recurse(R2, T2, o1, c1, o2, c2); + } + } +} + +/***************************/ +/******* TOLERANCE *********/ +/***************************/ + +void SrBvTreeQuery::_tolerance ( srbvmat R1, srbvvec T1, SrBvTree* o1, + srbvmat R2, srbvvec T2, SrBvTree* o2, + srbvreal tolerance ) +{ + using namespace SrBvMath; + + // init data: +// _dist = 0; + _num_bv_tests = 0; + _num_tri_tests = 0; + if ( tolerance<0 ) tolerance=0.0; + _toler = tolerance; + _closer_than_tolerance = false; + + // make sure that the models are built: + if ( o1->nodes()==0 || o2->nodes()==0 ) return; + + // Compute the transform [R,T] that takes us from cs2 to cs1. + // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] + + MTxM(_R,R1,R2); + srbvreal Ttemp[3]; + VmV(Ttemp, T2, T1); + MTxV(_T, R1, Ttemp); + + // compute the transform from o1->child(0) to o2->child(0) + srbvreal Rtemp[3][3], R[3][3], T[3]; + + MxM(Rtemp,_R,o2->child(0)->R); + MTxM(R,o1->child(0)->R,Rtemp); + MxVpV(Ttemp,_R,o2->child(0)->Tr,_T); + VmV(Ttemp,Ttemp,o1->child(0)->Tr); + MTxV(T,o1->child(0)->R,Ttemp); + + // find a distance lower bound for trivial reject + + srbvreal d = SrBv::distance(R, T, o1->child(0), o2->child(0)); + + if ( d <= _toler ) + { _tolerance_recurse ( R, T, o1, 0, o2, 0 ); + } + + // res->p2 is in cs 1 ; transform it to cs 2 + srbvreal u[3]; + VmV(u, _p2, _T); + MTxV(_p2, _R, u); +} + +void SrBvTreeQuery::_tolerance_recurse + ( srbvmat R, srbvvec T, + SrBvTree* o1, int b1, + SrBvTree* o2, int b2 ) +{ + using namespace SrBvMath; + + srbvreal sz1 = o1->child(b1)->size(); + srbvreal sz2 = o2->child(b2)->size(); + int l1 = o1->child(b1)->leaf(); + int l2 = o2->child(b2)->leaf(); + + if (l1 && l2) + { + // both leaves - find if tri pair within tolerance + + _num_tri_tests++; + + srbvreal p[3], q[3]; + + SrBvTri *t1 = &o1->_tris[-o1->child(b1)->first_child - 1]; + SrBvTri *t2 = &o2->_tris[-o2->child(b2)->first_child - 1]; + + srbvreal d = TriDistance(_R,_T,t1,t2,p,q); + + if ( d<=_toler ) + { + // triangle pair distance less than tolerance + _closer_than_tolerance = true; + _dist = d; + VcV(_p1, p); // p already in c.s. 1 + VcV(_p2, q); // q must be transformed into c.s. 2 later + } + return; + } + + int a1,a2,c1,c2; // new bv tests 'a' and 'c' + srbvreal R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3]; + + if (l2 || (!l1 && (sz1 > sz2))) + { + // visit the children of b1 + + a1 = o1->child(b1)->first_child; + a2 = b2; + c1 = o1->child(b1)->first_child+1; + c2 = b2; + + MTxM(R1,o1->child(a1)->R,R); + VmV(Ttemp,T,o1->child(a1)->Tr); + MTxV(T1,o1->child(a1)->R,Ttemp); + + MTxM(R2,o1->child(c1)->R,R); + VmV(Ttemp,T,o1->child(c1)->Tr); + MTxV(T2,o1->child(c1)->R,Ttemp); + } + else + { + // visit the children of b2 + + a1 = b1; + a2 = o2->child(b2)->first_child; + c1 = b1; + c2 = o2->child(b2)->first_child+1; + + MxM(R1,R,o2->child(a2)->R); + MxVpV(T1,R,o2->child(a2)->Tr,T); + MxM(R2,R,o2->child(c2)->R); + MxVpV(T2,R,o2->child(c2)->Tr,T); + } + + _num_bv_tests += 2; + + srbvreal d1 = SrBv::distance(R1, T1, o1->child(a1), o2->child(a2)); + srbvreal d2 = SrBv::distance(R2, T2, o1->child(c1), o2->child(c2)); + + if (d2 < d1) + { + if (d2 <= _toler) _tolerance_recurse( R2, T2, o1, c1, o2, c2); + if (_closer_than_tolerance) return; + if (d1 <= _toler) _tolerance_recurse( R1, T1, o1, a1, o2, a2); + } + else + { + if (d1 <= _toler) _tolerance_recurse( R1, T1, o1, a1, o2, a2); + if (_closer_than_tolerance) return; + if (d2 <= _toler) _tolerance_recurse( R2, T2, o1, c1, o2, c2); + } +} + + +srbvreal SrBvTreeQuery::_greedy_distance_recurse ( + srbvmat R, srbvvec T, SrBvTree* o1, int b1, + SrBvTree* o2, int b2, srbvreal delta ) +{ + using namespace SrBvMath; + + int l1 = o1->child(b1)->leaf(); + int l2 = o2->child(b2)->leaf(); + + if (l1 && l2) + { + // both leaves. return their distance + _num_tri_tests++; + + double p[3], q[3]; + + const SrBvTri *t1 = o1->tri(-o1->child(b1)->first_child - 1); + const SrBvTri *t2 = o2->tri(-o2->child(b2)->first_child - 1); + + return TriDistance(_R,_T,t1,t2,p,q); + } + + // First, perform distance tests on the children. Then traverse + // them recursively, but test the closer pair first, the further + // pair second. + + int a1,a2,c1,c2; // new bv tests 'a' and 'c' + double R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3]; + + srbvreal sz1 = o1->child(b1)->size(); + srbvreal sz2 = o2->child(b2)->size(); + + if (l2 || (!l1 && (sz1 > sz2))) + { + // visit the children of b1 + + a1 = o1->child(b1)->first_child; + a2 = b2; + c1 = o1->child(b1)->first_child+1; + c2 = b2; + + MTxM(R1,o1->child(a1)->R,R); + VmV(Ttemp,T,o1->child(a1)->Tr); + MTxV(T1,o1->child(a1)->R,Ttemp); + + MTxM(R2,o1->child(c1)->R,R); + VmV(Ttemp,T,o1->child(c1)->Tr); + MTxV(T2,o1->child(c1)->R,Ttemp); + } + else + { + // visit the children of b2 + + a1 = b1; + a2 = o2->child(b2)->first_child; + c1 = b1; + c2 = o2->child(b2)->first_child+1; + + MxM(R1,R,o2->child(a2)->R); + MxVpV(T1,R,o2->child(a2)->Tr,T); + + MxM(R2,R,o2->child(c2)->R); + MxVpV(T2,R,o2->child(c2)->Tr,T); + } + + double d1 = SrBv::distance(R1, T1, o1->child(a1), o2->child(a2)); + double d2 = SrBv::distance(R2, T2, o1->child(c1), o2->child(c2)); + _num_bv_tests += 2; + + // if we already found separation, don't further recurse to refine it + double min_d1d2 = (d1 < d2) ? d1 : d2; + if ( min_d1d2 > delta ) + return min_d1d2; + + // else recurse + if (d2 < d1) // and thus at least d2 <= delta (d1 maybe too) + { + srbvreal alpha = _greedy_distance_recurse(R2, T2, o1, c1, o2, c2, delta); + + if ( alpha > delta ) { + if ( d1 > delta ) + return (alpha < d1)? alpha: d1; + srbvreal beta = _greedy_distance_recurse(R1, T1, o1, a1, o2, a2, delta); + if ( beta > delta ) + return (alpha < beta)? alpha: beta; + } + return 0; + } + else // at least d1 <= delta (d2 maybe too) + { + srbvreal alpha = _greedy_distance_recurse(R1, T1, o1, a1, o2, a2, delta); + + if ( alpha > delta ) { + if ( d2 > delta ) + return (alpha < d2)? alpha: d2; + srbvreal beta = _greedy_distance_recurse(R2, T2, o1, c1, o2, c2, delta); + if ( beta > delta ) + return (alpha < beta)? alpha: beta; + } + return 0; + } +} + +/*************************************************************************\ + Copyright 1999 The University of North Carolina at Chapel Hill. + All Rights Reserved. + + Permission to use, copy, modify and distribute this software and its + documentation for educational, research and non-profit purposes, without + fee, and without a written agreement is hereby granted, provided that the + above copyright notice and the following three paragraphs appear in all + copies. + + IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE + LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE + USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY + OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + DAMAGES. + + THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE + PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF + NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, + UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + + The authors may be contacted via: + + US Mail: S. Gottschalk, E. Larsen + Department of Computer Science + Sitterson Hall, CB #3175 + University of N. Carolina + Chapel Hill, NC 27599-3175 + Phone: (919)962-1749 + EMail: geom@cs.unc.edu +\**************************************************************************/ diff --git a/source/dcdt/se/sr_bv_tree_query.h b/source/dcdt/se/sr_bv_tree_query.h new file mode 100644 index 0000000000..f24d8353c1 --- /dev/null +++ b/source/dcdt/se/sr_bv_tree_query.h @@ -0,0 +1,173 @@ +/** \file sr_bv_tree_query.h + * bounding volume tree */ + +// TO TEST: +// 1. use of double vs. float +// 2. use inline in SrBvMath functions + +# ifndef SR_BV_TREE_QUERY_H +# define SR_BV_TREE_QUERY_H + +# include "sr_mat.h" +# include "sr_bv_tree.h" + +/*! Encapsulates data for collision and proximity queries between + trees of bounding volumes. + Adapted from PQP, see the copyright notice in the source file. */ +class SrBvTreeQuery + { private : + // common data for all tests: + int _num_bv_tests; + int _num_tri_tests; + srbvmat _R; // xform from model 1 to model 2 + srbvvec _T; + // for collision queries only: + SrArray _pairs; + // for both distance and tolerance queries: + srbvreal _dist; + SrPnt _srp1, _srp2; + srbvvec _p1, _p2; + // for distance queries only: + srbvreal _rel_err; + srbvreal _abs_err; + // for proximity queries only: + bool _closer_than_tolerance; + srbvreal _toler; + + public : + /*! Constructor */ + SrBvTreeQuery (); + + /*! Initialize all values and frees all used memory */ + void init (); + + /*! Returns the number of bounding volume tests in last query */ + int num_bv_tests() const { return _num_bv_tests; } + + /*! Returns the number of triangle intersection tests in last query */ + int num_tri_tests() const { return _num_tri_tests; } + + /*! Returns the array with the indices of the colliding faces in + the last collide() query. The indices pairs are sequentially + stored in the array. */ + const SrArray& colliding_pairs () const { return _pairs; } + + /*! Returns the points computed in the last proximity or distance + query. For a proximity query, these are the points that + stablished the distance smaller than the tolerance (otherwise + these points are not meaningful). + For a distance query, these points stablished the minimum + distance, within the relative and absolute error bounds specified.*/ + const SrPnt& p1 () const { return _srp1; } + const SrPnt& p2 () const { return _srp2; } + + /*! Returns the distance computed in the last proximity or distance + query. For a proximity query, this value is only meaningful + if the distance is smaller than the tolerance specified */ + float distance() const { return (float)_dist; } + + /*! The boolean says whether the models in the last proximity query + are closer than tolerance distance specified */ + bool closer_than_tolerance() const { return _closer_than_tolerance; } + + /*! Find collisions between two models given as bv trees. + m1 is the placement of model 1 in the world & + m2 is the placement of model 2 in the world. + Matrices are in column-major order (as OpenGL) and must specify + only a rigid transformation (rotation and/or translation). + In this version, the collision test stops when the first + contact is found; which can be retrieved with colliding_pairs(); + True is returned if a collision was found, and false otherwise. */ + bool collide ( const SrMat& m1, const SrBvTree* t1, + const SrMat& m2, const SrBvTree* t2 ); + + /*! Similar to collide(), but it finds all collisions between the + two given models. Two times the number of collisions is + returned, ie, the size of the colliding_pairs() array. */ + int collide_all ( const SrMat& m1, const SrBvTree* t1, + const SrMat& m2, const SrBvTree* t2 ); + + /*! Computes the distance between two models given as bv trees. + "rel_err" is the relative error margin from actual distance. + "abs_err" is the absolute error margin from actual distance. + The smaller of the two will be satisfied, so set one large + to nullify its effect. + Returns the distance between the two models. + Methods distance(), p1() and p2() will return the distance and + points computed during the query. + Note: pointers t1 and t2 are not const only because the _last_tri + member variable of SrBvTree is updated during the query. */ + float distance ( const SrMat& m1, SrBvTree* t1, + const SrMat& m2, SrBvTree* t2, + float rel_err, float abs_err ); + + /*! Checks if distance between two models (given as bv trees) is <= tolerance + The algorithm returns whether the true distance is <= or > + "tolerance". This routine does not simply compute true distance + and compare to the tolerance - models can often be shown closer or + farther than the tolerance more trivially. In most cases this + query should run faster than a distance query would on the same + models and configurations. + The returned parameter returns the result of the query, which can + be also retrieved with method closer_than_tolerance(). + If the models are closer than ( <= ) tolerance, the points that + established this can be retrieved with methods p1(), and p2(), + and the distance can be retrieved with method distance() */ + bool tolerance ( const SrMat& m1, SrBvTree* t1, + const SrMat& m2, SrBvTree* t2, float tolerance ); + + float greedy_distance ( const SrMat& m1, SrBvTree* t1, + const SrMat& m2, SrBvTree* t2, float delta ); + + public : // lower level methods + + enum CollideFlag { CollideAllContacts=1, CollideFirstContact=2 }; + + /*! Lower level call used by the two main collide methods. + Collision results, if any, are stored in colliding_pairs(). */ + void collide ( srbvmat R1, srbvvec T1, const SrBvTree* o1, + srbvmat R2, srbvvec T2, const SrBvTree* o2, CollideFlag flag ); + + /*! Lower level call called by the other tolerance() method */ + bool tolerance ( srbvmat R1, srbvvec T1, SrBvTree* o1, + srbvmat R2, srbvvec T2, SrBvTree* o2, float toler ) + { _tolerance ( R1, T1, o1, R2, T2, o2, toler ); + _srp1.set ( _p1 ); + _srp2.set ( _p2 ); + return _closer_than_tolerance; + } + + /*! Computes a simple but fast lower bound on the distance between two models */ + float greedy_distance ( srbvmat R1, srbvvec T1, SrBvTree* o1, + srbvmat R2, srbvvec T2, SrBvTree* o2, float delta ); + + private : + + void _collide_recurse ( srbvmat R, srbvvec T, + const SrBvTree* o1, int b1, + const SrBvTree* o2, int b2, int flag ); + + void _distance ( srbvmat R1, srbvvec T1, SrBvTree* o1, + srbvmat R2, srbvvec T2, SrBvTree* o2, + srbvreal rel_err, srbvreal abs_err ); + + void _distance_recurse ( srbvmat R, srbvvec T, + SrBvTree* o1, int b1, + SrBvTree* o2, int b2 ); + + void _tolerance ( srbvmat R1, srbvvec T1, SrBvTree* o1, + srbvmat R2, srbvvec T2, SrBvTree* o2, + srbvreal tolerance ); + + void _tolerance_recurse ( srbvmat R, srbvvec T, + SrBvTree* o1, int b1, + SrBvTree* o2, int b2 ); + + srbvreal _greedy_distance_recurse ( srbvmat R, srbvvec T, + SrBvTree* o1, int b1, + SrBvTree* o2, int b2, srbvreal delta ); + }; + +//============================== end of file =============================== + +# endif // SR_BV_TREE_QUERY_H diff --git a/source/dcdt/se/sr_camera.cpp b/source/dcdt/se/sr_camera.cpp new file mode 100644 index 0000000000..f22cfc3da9 --- /dev/null +++ b/source/dcdt/se/sr_camera.cpp @@ -0,0 +1,202 @@ +#include "precompiled.h" +# include +# include "sr_box.h" +# include "sr_plane.h" +# include "sr_camera.h" + +//# define SR_USE_TRACE1 // ray +# include "sr_trace.h" + +//=================================== SrCamera =================================== + +SrCamera::SrCamera () + { + init (); + } + +SrCamera::SrCamera ( const SrCamera &c ) + :eye(c.eye), center(c.center), up(c.up) + { + fovy = c.fovy; + znear = c.znear; + zfar = c.zfar; + aspect = c.aspect; + scale = c.scale; + } + +SrCamera::SrCamera ( const SrPnt& e, const SrPnt& c, const SrVec& u ) + : eye(e), center(c), up(u) + { + fovy = SR_TORAD(60); + znear = 0.1f; + zfar = 1000.0f; + aspect = 1.0f; + } + +void SrCamera::init () + { + eye.set ( 0, 0, 2.0f ); + center = SrVec::null; + up = SrVec::j; + fovy = SR_TORAD(60); + znear = 0.1f; + zfar = 1000.0f; + aspect = 1.0f; + scale = 1.0f; + } + +SrMat& SrCamera::get_view_mat ( SrMat &m ) const + { + m.look_at ( eye, center, up ); + return m; + } + +SrMat& SrCamera::get_perspective_mat ( SrMat &m ) const + { + m.perspective ( fovy, aspect, znear, zfar ); + return m; + } + +// screenpt coords range in [-1,1] +void SrCamera::get_ray ( float winx, float winy, SrVec &p1, SrVec &p2 ) const + { + p1.set ( winx, winy, znear ); // p1 is in the near clip plane + + SrMat M(SrMat::NotInitialized), V(SrMat::NotInitialized), P(SrMat::NotInitialized); + + V.look_at ( eye, center, up ); + P.perspective ( fovy, aspect, znear, zfar ); + + M.mult ( V, P ); // equiv to M = V * P + + M.invert(); + + p1 = p1 * M; + p2 = p1-eye; // ray is in object coordinates, but before the scaling + + p2.normalize(); + p2 *= (zfar-znear); + p2 += p1; + + float inv_scale = 1.0f/scale; + p1*= inv_scale; + p2*= inv_scale; + + SR_TRACE1 ( "Ray: "<< p1 <<" : "<< p2 ); + } + +/* - -------- \ + | | | \ + h | bbox |--------------.eye + | | | dist / + - -------- / tan(viewang/2)=(h/2)/dist +*/ +void SrCamera::view_all ( const SrBox &box, float fovy_radians ) + { + SrVec size = box.size(); + float h = SR_MAX(size.x,size.y); + + fovy = fovy_radians; + up = SrVec::j; + center = box.center(); + eye = center; + + float dist = (h/2)/tanf(fovy/2); + eye.z = box.b.z + dist; + + float delta = box.max_size() + 0.0001f; + zfar = SR_ABS(eye.z)+delta; + + scale = 1.0f; + } + +void SrCamera::apply_translation_from_mouse_motion ( float lwinx, float lwiny, float winx, float winy ) + { + SrVec p1, p2, x, inc; + + SrPlane plane ( center, eye-center ); + + get_ray ( lwinx, lwiny, p1, x ); + p1 = plane.intersect ( p1, x ); + get_ray ( winx, winy, p2, x ); + p2 = plane.intersect ( p2, x ); + + inc = p1-p2; + + inc *= scale; + + *this += inc; + } + +void SrCamera::operator*= ( const SrQuat& q ) + { + eye -= center; + eye = eye * q; + eye += center; + up -= center; + up = up * q; + up += center; + } + +void SrCamera::operator+= ( const SrVec& v ) + { + eye += v; + center += v; + } + +void SrCamera::operator-= ( const SrVec& v ) + { + eye -= v; + center -= v; + } + +//=============================== friends ========================================== + +SrCamera operator* ( const SrCamera& c, const SrQuat& q ) + { + SrCamera cam(c); + cam *= q; + return cam; + } + +SrCamera operator+ ( const SrCamera& c, const SrVec& v ) + { + SrCamera cam(c); + cam += v; + return cam; + } + +SrOutput& operator<< ( SrOutput& out, const SrCamera& c ) + { +// out << "eye:" << c.eye << " center:" << c.center << " up:" << c.up << srnl; + + out << "eye " << c.eye << srnl << + "center " << c.center << srnl << + "up " << c.up << srnl << + "fovy " << c.fovy << srnl << + "znear " << c.znear << srnl << + "zfar " << c.zfar << srnl << + "aspect " << c.aspect << srnl << + "scale " << c.scale << srnl; + + return out; + } + +SrInput& operator>> ( SrInput& inp, SrCamera& c ) + { + while ( 1 ) + { if ( inp.get_token()==SrInput::EndOfFile ) break; + if ( inp.last_token()=="eye" ) inp>>c.eye; + else if ( inp.last_token()=="center" ) inp>>c.center; + else if ( inp.last_token()=="up" ) inp>>c.up; + else if ( inp.last_token()=="fovy" ) inp>>c.fovy; + else if ( inp.last_token()=="znear" ) inp>>c.znear; + else if ( inp.last_token()=="zfar" ) inp>>c.zfar; + else if ( inp.last_token()=="aspect" ) inp>>c.aspect; + else if ( inp.last_token()=="scale" ) inp>>c.scale; + else { inp.unget_token(); break; } + } + return inp; + } + +//================================ End of File ========================================= diff --git a/source/dcdt/se/sr_camera.h b/source/dcdt/se/sr_camera.h new file mode 100644 index 0000000000..4c5a520ec7 --- /dev/null +++ b/source/dcdt/se/sr_camera.h @@ -0,0 +1,108 @@ + +# ifndef SR_CAMERA_H +# define SR_CAMERA_H + +/** \file sr_camera.h + * Keeps camera parameters + */ + +# include "sr_vec.h" +# include "sr_quat.h" + +class SrMat; +class SrBox; + +/*! \class SrCamera sr_camera.h + \brief Keeps camera parameters + + SrCamera contains the parameters to define a camera. + Attention: if znear is too small inconsistencies in the rendering may appear; + a minimal value of 0.1 should be considered. */ +class SrCamera + { public : + SrPnt eye; //!< position of the eye, default is (0,0,2). + SrPnt center; //!< position where the eye is looking to, default is (0,0,0). + SrVec up; //!< the up vector orients the camera around the eye-center vector, default is (0,1,0) + float fovy; //!< the y field of view in radians. Default is pi/3 (60deg), range is [0.01,pi]. + float znear; //!< must be >0, default is 0.1. + float zfar; //!< must be >0, default is 1000. + float aspect; //!< normally is set to the screen width/heigh, default is 1. + float scale; //!< a scale factor to be applied between the view matrix and the scene + + public : + + /*! Initialize the camera with the default parameters, see init(). */ + SrCamera (); + + /*! Copy constructor. */ + SrCamera ( const SrCamera &c ); + + /*! Initialize the camera with the main parameters eye, center and up. */ + SrCamera ( const SrPnt& e, const SrPnt& c, const SrVec& u ); + + /*! Set the parameters to their default values, which are : + eye=(0,0,2), center=(0,0,0), up=(0,1,0), fovy=60, znear=0.1, zfar=1000, aspect=1. */ + void init (); + + /*! Set m to be the transformation matrix generated by the parameters + eye, center, and up. A reference to m is also returned. + Note: the scale factor is not included in this matrix. */ + SrMat& get_view_mat ( SrMat &m ) const; + + /*! Set m to be the transformation projection matrix generated by the parameters + fovy, znear, zfar, aspect. A reference to m is also returned. */ + SrMat& get_perspective_mat ( SrMat &m ) const; + + /*! Gets the 3d ray (p1,p2) which projects exactly in the given window point + according to the camera current parameters. Points p1 and p2 lye in the + near and far planes respectively. Window points are considered to be + in normalized coordinates, ranging between [-1,1]. */ + void get_ray ( float winx, float winy, SrVec& p1, SrVec& p2 ) const; + + /*! Sets center at the center of the box, and put the eye in the semi-line + rooted at center and with direction z, in a distance from the center + that is sufficient to visualize all the box with the given fov_y parm. + After this call, variable SrCamera::fovy will have the same value as fov_y. + Note: the scale factor is set to one in this method. */ + void view_all ( const SrBox& box, float fov_y ); + + /*! Apply a trackball translation induced from the mouse motion. First, the ray + passing through each window position and the intersection point with the + projection plane is determined. Then, the two intersection points determine the + displacement to be applied to the trackball, after a multiplication with the + current spin rotation. Mouse coordinates must be normalized in [-1,1]x[-1,1]. */ + void apply_translation_from_mouse_motion ( float lwinx, float lwiny, float winx, float winy ); + + /*! Transforms the camera position with the given rotation. The rotation is + applied to the up vector, and to the eye vector in the following way by + going: x-=center; x=x*q; x+=center (x represents eye or up vector). */ + void operator*= ( const SrQuat& q ); + + /*! Adds the vector v to the eye and center points. */ + void operator+= ( const SrVec& v ); + + /*! Subtracts the vector v to the eye and center points. */ + void operator-= ( const SrVec& v ); + + /*! Returns a camera that is the same as the given camera c, but with the + rotation q applied. See the operator *= for a description of how the + rotation is applied to the camera. */ + friend SrCamera operator* ( const SrCamera& c, const SrQuat& q ); + + /*! Returns a camera that is the same as the given camera c, but with the + the translation vector v added to the eye and center points. */ + friend SrCamera operator+ ( const SrCamera& c, const SrVec& v ); + + /*! Output camera data values in format keyword1 value \n keyword2 value ... + (keywords are: eye, center, up, etc*/ + friend SrOutput& operator<< ( SrOutput& out, const SrCamera& c ); + + /*! Input camera data. Not all keywords are required to exist. The routine + returns when a non-keyword entry is found (which is 'ungetted' in inp). */ + friend SrInput& operator>> ( SrInput& inp, SrCamera& c ); + }; + +//================================ End of File ================================================= + +# endif // SR_CAMERA_H + diff --git a/source/dcdt/se/sr_cfg_manager.cpp b/source/dcdt/se/sr_cfg_manager.cpp new file mode 100644 index 0000000000..3fbfa9cc5c --- /dev/null +++ b/source/dcdt/se/sr_cfg_manager.cpp @@ -0,0 +1,34 @@ +#include "precompiled.h" +# include "sr_cfg_manager.h" + +//=========================== SrCfgManagerBase ======================================== + +// ps: could try each "level check" in a new thread +bool SrCfgManagerBase::visible ( const srcfg* ct0, const srcfg* ct1, srcfg* ct, float prec ) + { + float segs, dseg, dt, t; + float len = dist ( ct0, ct1 ); + int k = 1; // k is the current level being tested + + while ( true ) + { // test the 2^(k-1) tests of level k: + segs = (float) sr_pow ( 2, k ); + dseg = 1.0f / segs; + + dt = dseg*2.0f; + + //sr_out<<"level="<0), and if the + counter becomes 0, the class is automatically self deleted. */ + void unref() { SrSharedClass::unref(); } + + public : // virtual callbacks + + /*! Returns a new configuration */ + virtual srcfg* alloc ()=0; + + /*! Returns a new configuration as a copy of c */ + virtual srcfg* alloc ( const srcfg* c )=0; + + /*! Deletes a configuration */ + virtual void free ( srcfg* c )=0; + + /*! Copy the contents of c2 into c1. Note: c1 and c2 will be equal + in several calls, so a test if(c1==c2) should be included here. */ + virtual void copy ( srcfg* c1, const srcfg* c2 )=0; + + /*! Outputs a configuration */ + virtual void output ( SrOutput& o, const srcfg* c )=0; + + /*! Inputs a configuration */ + virtual void input ( SrInput& i, srcfg* c )=0; + + /*! Returns a random configuration, not necessarily valid. */ + virtual void random ( srcfg* c )=0; + + /*! Returns if the configuration is valid. */ + virtual bool valid ( const srcfg* c )=0; + + /*! Returns a distance between the two configurations. */ + virtual float dist ( const srcfg* c1, const srcfg* c2 )=0; + + /*! Returns the interpolated configuration c between c1 and c2, + according to t in [0,1]. */ + virtual void interp ( const srcfg* c1, const srcfg* c2, float t, srcfg* c )=0; + + /*! Returns true if all nodes between the interpolation of ct0 and ct1 are + valid according to the given precision prec. Configuration ct is to be + used as a temporary variable. Note: ct0 and ct1 are already valid. + The default implementation for method visible performs recursive binary + subdivision untill reaching precision prec. */ + virtual bool visible ( const srcfg* ct0, const srcfg* ct1, srcfg* ct, float prec ); + + /*! Returns true if the time encoded in c1 is smaller than in c2. This will be + only relevant to planning in time-varying conditions, if this is not the + case, true must always be returned */ + virtual bool monotone ( const srcfg* c1, const srcfg* c2 )=0; + + /*! This is called just to notify that node child has been added + as a child of node parent */ + virtual void child_added ( srcfg* parent, srcfg* child )=0; + + /*! The time() method returns the time associated with the given configuration c. + This method is only called when solving a time-varying problem */ + virtual float time ( srcfg* c )=0; + }; + +/*! Template SrCfgManager makes automatic type casts to user-defined classes. + If needed in special cases, it can be further derived to rewrite/extend + method SrCfgManagerBase::visible(). + Here is an example of implementations of a configuration class and a class manager that + can be directly used with template SrCfgManager : \code +class MyCfg + { public : + MyCfg (); + MyCfg ( const MyCfg& c ); + ~MyCfg (); + void operator = ( const MyCfg& c ); + void random (); + bool valid () const; + friend SrOutput& operator<< ( SrOutput& out, const MyCfg& c ); + friend SrInput& operator>> ( SrInput& inp, MyCfg& c ); + friend float dist ( const MyCfg& c1, const MyCfg& c2 ); + friend float interp ( const MyCfg& c1, const MyCfg& c2, float t, MyCfg& c ); + }; + +class MyManager + { public : + MyCfg* alloc () { return new MyCfg; } + MyCfg* alloc ( const MyCfg* c ) { return new MyCfg(*c); } + void free ( MyCfg* c ) { delete c; } + void copy ( MyCfg* c1, const MyCfg* c2 ) { *c1=*c2; } + void output ( SrOutput& o, const MyCfg* c ) { o<<(*c); } + void input ( SrInput& i, MyCfg* c ) { i>>(*c); } + void random ( MyCfg* c ) { c->random(); } + bool valid ( const MyCfg* c ) { return c->valid(); } + float dist ( const MyCfg* c1, const MyCfg* c2 ) { return ::dist(*c1,*c2); } + void interp ( const MyCfg* c1, const MyCfg* c2, float t, MyCfg* c ) + { return ::interp(*c1,*c2,t,*c); } + void child_added ( MyCfg* parent, MyCfg* child ) {} + }; \endcode*/ +template +class SrCfgManager : public SrCfgManagerBase, public M + { public : + virtual srcfg* alloc () + { return (srcfg*) M::alloc(); } + virtual srcfg* alloc ( const srcfg* c ) + { return (srcfg*) M::alloc((const C*)c); } + virtual void free ( srcfg* c ) + { M::free ( (C*)c ); } + virtual void copy ( srcfg* c1, const srcfg* c2 ) + { M::copy ( (C*)c1, (const C*)c2 ); } + virtual void output ( SrOutput& o, const srcfg* c ) + { M::output ( o, (const C*)c ); } + virtual void input ( SrInput& i, srcfg* c ) + { M::input ( i, (C*)c ); } + virtual void random ( srcfg* c ) + { M::random ( (C*)c ); } + virtual bool valid ( const srcfg* c ) + { return M::valid ( (const C*)c ); } + virtual float dist ( const srcfg* c1, const srcfg* c2 ) + { return M::dist ( (const C*)c1, (const C*)c2 ); } + virtual void interp ( const srcfg* c1, const srcfg* c2, float t, srcfg* c ) + { M::interp ( (const C*)c1, (const C*)c2, t, (C*)c ); } + virtual bool monotone ( const srcfg* c1, const srcfg* c2 ) + { return M::monotone ( (const C*)c1, (const C*)c2 ); } + virtual void child_added ( srcfg* child, srcfg* parent ) + { M::child_added ( (C*)parent, (C*)child ); } + virtual float time ( srcfg* c ) + { return M::time ( (C*)c ); } + }; + +//============================== end of file =============================== + +# endif // SR_CFG_MANAGER_H + diff --git a/source/dcdt/se/sr_cfg_path.cpp b/source/dcdt/se/sr_cfg_path.cpp new file mode 100644 index 0000000000..f4b67be3e8 --- /dev/null +++ b/source/dcdt/se/sr_cfg_path.cpp @@ -0,0 +1,442 @@ +#include "precompiled.h" +# include "sr_cfg_path.h" +# include "sr_random.h" + +//# define SR_USE_TRACE1 // +# include "sr_trace.h" + +//================================ SrCfgPathBase ======================================== + +SrCfgPathBase::SrCfgPathBase ( SrCfgManagerBase* cman ) + { + _cman = cman; + _cman->ref(); + _size = 0; + _interp_start = 0; + _interp_startdist = 0; + } + +SrCfgPathBase::SrCfgPathBase ( const SrCfgPathBase& p ) + { + _cman = p._cman; + _cman->ref(); + _size = 0; + insert_path ( 0, p ); + _interp_start = 0; + _interp_startdist = 0; + } + +SrCfgPathBase::~SrCfgPathBase () + { + init (); + compress (); + _cman->unref(); + } + +void SrCfgPathBase::init () + { + _size = 0; + _interp_start = 0; + _interp_startdist = 0; + } + +void SrCfgPathBase::compress () + { + while ( _buffer.size()>_size ) _cman->free ( _buffer.pop() ); + } + +void SrCfgPathBase::push ( const srcfg* c ) + { + if ( _buffer.size()==_size ) // add new buffer entry + { _buffer.push() = _cman->alloc(); + } + if ( c ) _cman->copy ( _buffer[_size], c ); + _size++; + } + +void SrCfgPathBase::pop () + { + if ( _size>0 ) _size--; + _interp_start = 0; + _interp_startdist = 0; + } + +void SrCfgPathBase::remove ( int i ) + { + srcfg* cfg = _buffer[i]; + _buffer.move ( i/*dest*/, i+1/*src*/, _size-(i+1)/*n*/ ); + _size--; + _buffer[_size] = cfg; + _interp_start = 0; + _interp_startdist = 0; + } + +void SrCfgPathBase::remove ( int i, int dp ) + { + int bsize = _buffer.size(); + int idp = i+dp; + _buffer.size ( bsize+dp ); + _buffer.move ( bsize/*dest*/, i/*src*/, dp/*n*/ ); // copy part to remove to the buffer end + _buffer.move ( i/*dest*/, idp/*src*/, _size-idp/*n*/ ); // remove + _buffer.move ( _size-dp/*dest*/, bsize/*src*/, dp/*n*/ ); // keep the removed part + _buffer.size ( bsize ); + _size -= dp; + _interp_start = 0; + _interp_startdist = 0; + } + +void SrCfgPathBase::insert ( int i, const srcfg* c ) + { + if ( i>=_size ) { push(c); return; } + push ( c ); + srcfg* newcfg = top(); + _buffer.move ( i+1/*dest*/, i/*src*/, _size-(i+1)/*n*/ ); + _buffer[i] = newcfg; + _interp_start = 0; + _interp_startdist = 0; + } + +void SrCfgPathBase::insert_path ( int i, const SrCfgPathBase& p ) + { + if ( p.size()<1 ) return; + if ( i>_size ) i=_size; + + // open space in the end: + int oldsize = _size; + int newsize = _size+p.size(); + while ( _size!=newsize ) push(0); + + // move new space to the middle: + int n, n2=newsize-(oldsize-i); + srcfg* tmp; + for ( n=i; ncopy ( _buffer[i+n], p._buffer[n] ); + } + + _interp_start = 0; + _interp_startdist = 0; + } + +void SrCfgPathBase::append_path ( SrCfgPathBase& p ) + { + // open space here: + _buffer.insert ( _size, p.size() ); + + // transfer nodes from p: + int i; + for ( i=0; is ) pop(); + _interp_start = 0; + _interp_startdist = 0; + } + +float SrCfgPathBase::len ( int i1, int i2 ) const + { + float l=0; + int i; + if ( _size<2 ) return l; + for ( i=i1; idist(_buffer[i],_buffer[i+1]); + return l; + } + +void SrCfgPathBase::interp ( float t, srcfg* c ) + { + if ( _buffer.size()<2 ) { _interp_start=0; _interp_startdist=0; return; } + + // parameters _interp_start and _interp_startdist are used to optimize + // the time search during sequential play, here we check if they can + // be used or if we should recount the distance form the first node + if ( t<_interp_startdist ) + { _interp_start=0; _interp_startdist=0; } + + float dt; + float d = _interp_startdist; + int i; + + for ( i=_interp_start+1; i<_buffer.size(); i++ ) + { dt = _cman->dist ( _buffer.const_get(i-1), _buffer.const_get(i) ); + if ( d+dt>=t ) break; + d += dt; + } + + if ( i==_buffer.size() ) // may happen because of imprecisions + { i--; t=1; } + else + { t -= d; t /= dt; if ( t>1 ) t=1; } + + _interp_start = i-1; + _interp_startdist = d; + + _cman->interp ( _buffer.const_get(i-1), _buffer.const_get(i), t, c ); + } + +void SrCfgPathBase::temporal_interp ( float t, srcfg* c ) + { + if ( _buffer.size()<2 ) { _interp_start=0; _interp_startdist=0; return; } + + // here _interp_startdist is in fact the "start time" + if ( t<_interp_startdist ) + { _interp_start=0; _interp_startdist=0; } + + float dt; // delta time + float nt; // next time + float ct = _interp_startdist; // current time + int i; + + for ( i=_interp_start+1; i<_buffer.size(); i++ ) + { nt = _cman->time ( _buffer.const_get(i) ); + dt = nt-ct; + if ( nt>=t ) break; + ct = nt; + } + + if ( i==_buffer.size() ) // may happen because of imprecisions + { i--; t=1; } + else + { t -= ct; t /= dt; if ( t>1 ) t=1; } + + _interp_start = i-1; + _interp_startdist = ct; + + _cman->interp ( _buffer.const_get(i-1), _buffer.const_get(i), t, c ); + } + +void SrCfgPathBase::smooth_random ( float prec, float& len ) + { + if ( len<0 ) len = SrCfgPathBase::len(0,_size-1); + float t1 = SrRandom::randf()*len; + float t2 = SrRandom::randf()*len; + linearize ( prec, len, t1, t2 ); + } + +int SrCfgPathBase::linearize ( float prec, float& len, float t1, float t2 ) + { + srcfg* cfg1; // 1st random cfg along the path + srcfg* cfg2; // 2nd random cfg along the path + srcfg* citp; // cfg used during interpolation + + // get buffer space for cfg1 and cfg2: + if ( _buffer.size()<_size+3 ) // add new buffer entries + { push(0); push(0); push(0); pop(); pop(); pop(); } + citp = _buffer[_size]; + cfg1 = _buffer[_size+1]; + cfg2 = _buffer[_size+2]; + + // ensure t1len ) t1=len; + if ( t2>len ) t2=len; + if ( t1>t2 ) SR_SWAP(t1,t2); + + // get configurations cfg1 and cfg2 at t1 and t2: + _interp_start = 0; + _interp_startdist = 0; + interp ( t1, cfg1 ); + int i1 = _interp_start+1; // node after t1 + interp ( t2, cfg2 ); + int i2 = _interp_start; // node prior t2 + + if ( i1>i2 ) return 0; // samples t1 and t2 are in the same edge + + if ( !_cman->visible(cfg1,cfg2,citp,prec) ) return -1; // cannot smooth + + len -= SrCfgPathBase::len(i1-1,i2+1); // remove the older part length from len + + if ( i1==i2 ) // only 1 vertex between cfg1 and cfg2: insert 1 space + { insert ( i1+1, 0 ); + i2 = i1+1; + } + + // positions i1 and i2 become cfg1 and cfg2: + _cman->copy ( _buffer[i1], cfg1 ); + _cman->copy ( _buffer[i2], cfg2 ); + + // delete non-used intermediate vertices: + remove ( i1+1, (i2-i1)-1 ); + len += SrCfgPathBase::len(i1-1,i1+2); // add the new part length to len + + return 1; + } + +void SrCfgPathBase::smooth_ends ( float prec ) + { + if ( _size<5 ) return; // not possible if <5 + + srcfg* citp; // cfg used during interpolation + + // get buffer space for citp: + if ( _buffer.size()<_size+1 ) { push(0); pop(); } + citp = _buffer[_size]; + + // specify mid node and border nodes i1 and i2: + int mid = _size/2; + int i1 = mid; + int i2 = mid; + int posmax = _size-1; + int max = _size-3; + int min = 2; + + // check second half: + while ( i2<=max ) + { if ( _cman->visible(_buffer[i2],_buffer[posmax],citp,prec) ) + { remove ( i2+1, (posmax-i2)-1 ); + break; + } + i2++; + } + + // check first half: + while ( i1>=min ) + { if ( _cman->visible(_buffer[0],_buffer[i1],citp,prec) ) + { remove ( 1, i1-1 ); + break; + } + i1--; + } + } + +float SrCfgPathBase::_diff ( int i, float prec ) + { + if ( i<=0 || i>=size()-1 ) return -1; // protection +/* srcfg* cfg1; + srcfg* cfg2; + + // get buffer space for cfg1 and cfg2: + if ( _buffer.size()<_size+2 ) // add new buffer entries + { push(0); push(0); pop(); pop(); } + cfg1 = _buffer[_size]; + cfg2 = _buffer[_size+1]; +*/ + + float d1 = _cman->dist(_buffer.const_get(i-1),_buffer.const_get(i)); + float d2 = _cman->dist(_buffer.const_get(i),_buffer.const_get(i+1)); + float d = _cman->dist(_buffer.const_get(i-1),_buffer.const_get(i+1)); + + float diff = (d1+d2)-d; + diff /= d; + /* + prec*=5.0f; + + d = _cman->dist ( _buffer.const_get(i-1), _buffer.const_get(i) ); + t = prec/d; + if ( t>1 ) t=1; + _cman->interp ( _buffer.const_get(i-1), _buffer.const_get(i), t, cfg1 ); + + d = _cman->dist ( _buffer.const_get(i), _buffer.const_get(i+1) ); + t = prec/d; + if ( t>1 ) t=1; + _cman->interp ( _buffer.const_get(i), _buffer.const_get(i+1), t, cfg2 ); + + float diff = (_cman->dist(cfg1,_buffer.const_get(i))+_cman->dist(_buffer.const_get(i),cfg2)) + -_cman->dist(cfg1,cfg2);*/ + return diff; + } + +void SrCfgPathBase::smooth_init ( float prec ) + { + _sprec = prec; + //smooth_ends ( prec ); + _slen = len(); + _sbads=0; + _slastangmax=9999999999.0f; + } + +bool SrCfgPathBase::smooth_step () + { + smooth_random ( _sprec, _slen ); +return false; + int i, imax=1; + int lasti = size()-2; + float d; + float angmax=0; + float t=0, tmax=0; + for ( i=1; i<=lasti; i++ ) + { t += _cman->dist ( _buffer.const_get(i-1), _buffer.const_get(i) ); + d = _diff ( i, _sprec ); + if ( d>angmax ) + { angmax=d; imax=i; tmax=t; } + } + + //sr_out<<"angmax => "< "<<_sprec<_sprec ) + { //r = SrRandom::randf()*maxradius; + r/=2.0f; + //sr_out<_slen ) t2=_slen; + result = linearize ( _sprec, _slen, t1, t2 ); + times++; + if ( result==0 ) break; + } + + if ( angmax>_slastangmax ) + { _sbads++; + } + else + { _sbads=0; + _slastangmax=angmax; + } + + //sr_out<<"BADS: "<<_bads<6 ) return true; // no more easy improvements + return false; + } + +SrOutput& operator<< ( SrOutput& o, const SrCfgPathBase& p ) + { + int i; + for ( i=0; ioutput ( o, p._buffer[i] ); + o< _buffer; + SrCfgManagerBase* _cman; + int _size; + int _interp_start; + float _interp_startdist; + float _sprec, _slen; + int _sbads; + float _slastangmax; + + public : + /*! Constructor requires a pointer to the configuration manager */ + SrCfgPathBase ( SrCfgManagerBase* cman ); + + /*! Copy constructor sharing the configuration manager */ + SrCfgPathBase ( const SrCfgPathBase& p ); + + /*! Destructor */ + ~SrCfgPathBase (); + + /*! Makes the path become empty */ + void init (); + + /*! Deletes all non-used internal buffers */ + void compress (); + + /*! Appends a configuration at the end of the path. + Even if c is null, a new valid configuration is pushed. */ + void push ( const srcfg* c ); + + /*! Removes the last configuration in the path */ + void pop (); + + /*! Removes the position i, which must be a valid position */ + void remove ( int i ); + + /*! Removes dp positions at position i (parameters must be valid) */ + void remove ( int i, int dp ); + + /*! Inserts a configuraton at position i; if i>=size() a push() is done. */ + void insert ( int i, const srcfg* c ); + + /*! Inserts a copy of path p at position i; if i>=size() p is appended. */ + void insert_path ( int i, const SrCfgPathBase& p ); + + /*! Appends p to the path; p will become an empty path after this call */ + void append_path ( SrCfgPathBase& p ); + + /*! Swap the positions of the two nodes */ + void swap ( int i, int j ); + + /*! Reverts the order of the nodes in the path */ + void revert (); + + /*! pushes or pops entries until reaching size s */ + void size ( int s ); + + /*! Returns the number of configurations in the path */ + int size () const { return _size; } + + /*! Returns the length of the path from node i1 to node i2. + The length is calculated by adding the distances + of adjacent configurations in the path. */ + float len ( int i1, int i2 ) const; + + /*! Returns the length of the full path. */ + float len () const { return len(0,_size-1); } + + /*! Returns in c the interpolated configuration in the path according to parameter t, + which must be in the closed interval [0,len]. */ + void interp ( float t, srcfg* c ); + + /*! Returns in c the interpolated configuration in the path according to the time + parameter t, which must be a valid time between the times associated with the + first and last nodes of the path. The configuration manager method time() is + used to retrieve the time associated with each configuration. */ + void temporal_interp ( float t, srcfg* c ); + + /*! The smooth routine takes two random configurations interpolated along the path + and replaces the portion between them by a direct interpolation if no collisions + appear (up to precision prec). Parameter len should contain the current lenght of + the path. A <0 value can be given if the length is not known in advance. In any + case, after the smooth, len will contain the updated path length. */ + void smooth_random ( float prec, float& len ); + + /*! tries to replace the subpath(t1,t2) by a "straight interpolation". + Returns: updated len, 0:same edge, -1:collision, 1:done */ + int linearize ( float prec, float& len, float t1, float t2 ); + + /*! Make one pass in all nodes, trying first to smooth nodes closer to the first and + last nodes. Usefull because most often several nodes are created near the root + of the two expanding trees. Usually is called once before the random smooths. */ + void smooth_ends ( float prec ); + + void smooth_init ( float prec ); + bool smooth_step (); + + /*! Copy operator */ + void operator= ( const SrCfgPathBase& p ) { init(); insert_path(0,p); } + + /*! Returns the configuration of the last node in the path */ + srcfg* top () { return _buffer[_size-1]; } + + /*! Returns configuration index i */ + srcfg* get ( int i ) { return _buffer[i]; } + + /*! Const version of get() */ + const srcfg* const_get ( int i ) const { return _buffer.const_get(i); } + + /*! Outputs the path nodes for inspection */ + friend SrOutput& operator<< ( SrOutput& o, const SrCfgPathBase& p ); + + private: + float _diff ( int i, float prec ); + }; + +/*! This template provides automatic type casts for the user-defined + configuration class C. */ +template +class SrCfgPath : public SrCfgPathBase + { public : + SrCfgPath ( SrCfgManagerBase* cman ) : SrCfgPathBase(cman) {} + SrCfgPath ( const SrCfgPath& p ) : SrCfgPathBase(p) {} + + C* operator[] ( int i ) { return get(i); } + C* top () { return (C*)SrCfgPathBase::top(); } + C* get ( int i ) { return (C*)SrCfgPathBase::get(i); } + const C* const_get ( int i ) const { return (const C*)SrCfgPathBase::const_get(i); } + void operator= ( const SrCfgPath& p ) { SrCfgPathBase::init(); SrCfgPathBase::insert_path(0,p); } + }; + + +//================================ End of File ================================================= + +# endif // SR_PATH_H + diff --git a/source/dcdt/se/sr_cfg_planner.cpp b/source/dcdt/se/sr_cfg_planner.cpp new file mode 100644 index 0000000000..01bcaa4553 --- /dev/null +++ b/source/dcdt/se/sr_cfg_planner.cpp @@ -0,0 +1,128 @@ +#include "precompiled.h" +# include "sr_cfg_planner.h" + +//# define SR_USE_TRACE1 // start +//# define SR_USE_TRACE2 // update +//# define SR_USE_TRACE3 // bridge +# include "sr_trace.h" + +//============================= SrCfgPlannerBase ======================================== + +SrCfgPlannerBase::SrCfgPlannerBase ( SrCfgManagerBase* cman ) + :_tree1 ( cman ), _tree2 ( cman ), _path ( cman ) + { + _cman = cman; + + _tmpc1 = _cman->alloc(); + _tmpc2 = _cman->alloc(); + + _solved = false; + _juststarted = false; + _curtree = 1; + } + +SrCfgPlannerBase::~SrCfgPlannerBase () + { + init (); + _cman->free ( _tmpc1 ); + _cman->free ( _tmpc2 ); + } + +void SrCfgPlannerBase::init () + { + _tree1.init (); + _tree2.init (); + _path.init (); + _solved = false; + _juststarted = false; + _curtree = 1; + } + +void SrCfgPlannerBase::start ( const srcfg* c1, const srcfg* c2 ) + { + SR_TRACE1 ( "Start..."); + init (); + + SR_TRACE1 ( "Tree Init..."); + _tree1.init ( c1 ); + _tree2.init ( c2 ); + _curtree = 1; // could be 1 or 2 + _juststarted = true; + + SR_TRACE1 ( "Start OK."); + } + +bool SrCfgPlannerBase::update_rrt ( float step, int tries, float prec ) + { + SrCfgNode* n; // new node added to the current tree + SrCfgNode* nearest1; // nearest node in tree1 + SrCfgNode* nearest2; // nearest node in tree2 + float dist1; // distance from crand to nearest1 + float dist2; // distance from crand to nearest2 + srcfg* crand = _tmpc1; // the random configuration + + SR_TRACE2 ( "UPDT: expanding tree " << _curtree ); + + if ( _juststarted ) + { _juststarted = false; + if ( _try_to_join(_tree1.root(),_tree2.root(),prec) ) return true; // FOUND + } + + _cman->random ( crand ); + nearest1 = _tree1.search_nearest ( crand, &dist1 ); + nearest2 = _tree2.search_nearest ( crand, &dist2 ); + + float dist = _cman->dist(nearest1->cfg(),nearest2->cfg()); + if ( dist<=step ) + { if ( _try_to_join(nearest1,nearest2,prec) ) return true; // FOUND + SR_TRACE2 ( "UPDT: not found." ); + return false; // not found + } + + SR_TRACE2 ( "UPDT: nearest1="<id()<<" nearest2="<id() ); + SR_TRACE2 ( "UPDT: expanding..." ); + if ( _curtree==1 ) + { if ( _cman->monotone ( nearest1->cfg(), crand ) ) + { n = _tree1.expand_node_safe ( nearest1, crand, step, tries, prec, dist1 ); + if ( n ) + { if ( _try_to_join(n,nearest2,prec) ) return true; // FOUND + } + } + } + else + { if ( _cman->monotone ( crand, nearest2->cfg() ) ) + { n = _tree2.expand_node_safe ( nearest2, crand, step, tries, prec, dist2 ); + if ( n ) + { if ( _try_to_join(nearest1,n,prec) ) return true; // FOUND + } + } + } + + _curtree = _curtree==1? 2:1; + + SR_TRACE2 ( "UPDT: not found." ); + + return false; // not found + } + +bool SrCfgPlannerBase::_try_to_join ( SrCfgNode* n1, SrCfgNode* n2, float prec ) + { + bool b; + + b = _cman->monotone ( n1->cfg(), n2->cfg() ); + if ( !b ) return false; + + b = _cman->visible ( n1->cfg(), n2->cfg(), _tmpc2, prec ); + if ( !b ) return false; + + _path.init (); + _tree1.get_branch ( n1, _path ); + _path.revert(); + _tree2.get_branch ( n2, _path ); + + _solved = true; + return _solved; + } + +//============================= End of File =========================================== + diff --git a/source/dcdt/se/sr_cfg_planner.h b/source/dcdt/se/sr_cfg_planner.h new file mode 100644 index 0000000000..260904f35b --- /dev/null +++ b/source/dcdt/se/sr_cfg_planner.h @@ -0,0 +1,88 @@ + +# ifndef SR_CFG_PLANNER_H +# define SR_CFG_PLANNER_H + +//# include +//# include +# include "sr_heap.h" +# include "sr_cfg_path.h" +# include "sr_cfg_tree.h" + +/*! A single-query, bidirectional, lazy and sampling-based planner */ +class SrCfgPlannerBase : public SrSharedClass + { private : + SrCfgTreeBase _tree1, _tree2; + SrCfgPathBase _path; + SrCfgManagerBase* _cman; + srcfg* _tmpc1; + srcfg* _tmpc2; + int _curtree; + bool _solved; + bool _juststarted; + struct HeapEdge { SrCfgTreeBase* tree; SrCfgNode* n1; SrCfgNode* n2; }; + SrHeap _heap; + + public : + + /*! The constructor requires a configuration manager. */ + SrCfgPlannerBase ( SrCfgManagerBase* cman ); + + /*! Destructor frees all used internal data, and unref the associated + configuration managers */ + ~SrCfgPlannerBase (); + + /*! Returns the roadmap tree rooted at the source configuration */ + SrCfgTreeBase& tree1 () { return _tree1; } + + /*! Returns the roadmap tree rooted at the destination configuration */ + SrCfgTreeBase& tree2 () { return _tree2; } + + /*! Returns the number of nodes in both trees */ + int nodes () const { return _tree1.nodes()+_tree2.nodes(); } + + /*! Clears everything */ + void init (); + + /*! Clears everything and define the source and goal configurations. + Configurations c1 and c2 must be valid. + If a time-varying problem will be solved, configuration c1 must correspond + to the start and c2 to the goal, ie c1 happens before c2 */ + void start ( const srcfg* c1, const srcfg* c2 ); + + /*! Returns true if a path to the goal was found, and false otherwise. */ + bool solved () const { return _solved; } + + /*! Returns the last path found by the planner */ + SrCfgPathBase& path () { return _path; } + + /*! Update one of the trees, returning true if a path was found + Parameter step is the incremental step distance, and + tries is the number of bisections to try in case of expantion failure */ + bool update_rrt ( float step, int tries, float prec ); + + /*! Lazy version of the update method. */ + bool update_lazy ( float step, int tries, float prec ); + + private : + bool _test_bridge ( SrCfgNode* n1, SrCfgNode* n2, float prec ); + bool _try_to_join ( SrCfgNode* n1, SrCfgNode* n2, float prec ); + void _heap_add_branch ( SrCfgTreeBase* tree, SrCfgNode* n ); + }; + +/*! Planner template for user-defined configurations */ +template +class SrCfgPlanner : public SrCfgPlannerBase + { public : + SrCfgPlanner ( SrCfgManagerBase* cman ) + : SrCfgPlannerBase(cman) { } + + SrCfgTree& tree1 () { return (SrCfgTree&) SrCfgPlannerBase::tree1(); } + SrCfgTree& tree2 () { return (SrCfgTree&) SrCfgPlannerBase::tree2(); } + + SrCfgPath& path () { return (SrCfgPath&) SrCfgPlannerBase::path(); } + }; + +//================================ End of File ================================================= + +# endif // SR_CFG_PLANNER_H + diff --git a/source/dcdt/se/sr_cfg_planner_lazy.cpp b/source/dcdt/se/sr_cfg_planner_lazy.cpp new file mode 100644 index 0000000000..a6a988678b --- /dev/null +++ b/source/dcdt/se/sr_cfg_planner_lazy.cpp @@ -0,0 +1,137 @@ +#include "precompiled.h" +# include "sr_cfg_planner.h" + +//# define SR_USE_TRACE1 // not used +//# define SR_USE_TRACE2 // update +//# define SR_USE_TRACE3 // bridge +# include "sr_trace.h" + +//============================= SrCfgPlannerBase ======================================== + +bool SrCfgPlannerBase::update_lazy ( float step, int tries, float prec ) + { + SrCfgNode* n; // new node added to the current tree + SrCfgNode* nearest1; // nearest node in tree1 + SrCfgNode* nearest2; // nearest node in tree2 + float dist1; // distance from crand to nearest1 + float dist2; // distance from crand to nearest2 + srcfg* crand = _tmpc1; // the random configuration + + SR_TRACE2 ( "UPDT: expanding tree " << _curtree ); + + if ( _juststarted ) + { _juststarted = false; + if ( _test_bridge(_tree1.root(),_tree2.root(),prec) ) return true; // FOUND + } + + _cman->random ( crand ); + nearest1 = _tree1.search_nearest ( crand, &dist1 ); + nearest2 = _tree2.search_nearest ( crand, &dist2 ); + + float dist = _cman->dist(nearest1->cfg(),nearest2->cfg()); + if ( dist<=step ) + { if ( _test_bridge(nearest1,nearest2,prec) ) return true; // FOUND + } + + SR_TRACE2 ( "UPDT: nearest1="<id()<<" nearest2="<id() ); + SR_TRACE2 ( "UPDT: expanding..." ); + if ( _curtree==1 ) + { n = _tree1.expand_node ( nearest1, crand, step, tries, dist1 ); + if ( n ) + { if ( _test_bridge(n,nearest2,prec) ) return true; // FOUND + } + } + else + { n = _tree2.expand_node ( nearest2, crand, step, tries, dist2 ); + if ( n ) + { if ( _test_bridge(nearest1,n,prec) ) return true; // FOUND + } + } + + _curtree = _curtree==1? 2:1; + + SR_TRACE2 ( "UPDT: not found." ); + + return false; // not found + } + +void SrCfgPlannerBase::_heap_add_branch ( SrCfgTreeBase* tree, SrCfgNode* n ) + { + HeapEdge e; + SrCfgNode *parent; + while ( n->parent() ) + { parent = n->parent(); + if ( !parent->safe(n->parentlink()) ) + { e.tree = tree; + e.n1 = parent; // convention: e.n1 is parent of e.n2 + e.n2 = n; + _heap.insert ( e, parent->level(n->parentlink()) ); + } + n = parent; // move to the parent + } + } + +/*! Test if the path formed by connecting node index n1 of tree 1 with + node index n2 of tree2 is a valid path. The test performs collision + detection in the edges of the path incrementing their levels. A + priority queue is used to first test edges in lower levels. + If all edges in the path become safe, a path is formed and true is returned. + Otherwise, the edge found to be invalid is deleted, the two trees are + updated to keep the remaining edges, and false is returned */ +bool SrCfgPlannerBase::_test_bridge ( SrCfgNode* n1, SrCfgNode* n2, float prec ) + { + // add the cfg of n2 to tree1: + SrCfgNode* n12 = _tree1.add_node ( n1, n2->cfg() ); + + // make priority heap where the cost is the edge level + // and add all non-safe path edges of tree 1 and tree 2 to the heap + SR_TRACE3 ( "BRIDGE: building heap..." ); + _heap.init(); + _heap_add_branch ( &_tree1, n12 ); + _heap_add_branch ( &_tree2, n2 ); + + // test and increment the level of the edges in the heap + int level; + HeapEdge e; + while ( _heap.size()>0 ) + { level = _heap.lowest_cost(); + e = _heap.top(); + + SR_TRACE3 ( "BRIDGE: heap size="<<_heap.size()<<" level="<increment_edge_level ( e.n1, e.n2, prec ) ) break; // collision found + + // remove and reinsert edge if not yet safe: + _heap.remove(); + if ( !e.n1->safe(e.n2->parentlink()) ) + _heap.insert ( e, level+1 ); + } + + if ( _heap.size()==0 ) // all edges were safe: path found + { SR_TRACE3 ( "BRIDGE: path found!" ); + _solved = true; + _path.init (); + _tree1.get_branch ( n1, _path ); + _path.revert(); + _tree2.get_branch ( n2, _path ); + SR_TRACE3 ( "BRIDGE: path done." ); + } + else // failed, reorganize trees + { SR_TRACE3 ( "BRIDGE: failed, transferring subtrees..." ); + + if ( e.tree==&_tree1 ) + { //sr_out<<"\nTRANSFER 1: "<id()<id()<id()<id()<id()<id()<id()<id()<& nodes ) + { + int i; + nodes.push() = this; + for ( i=0; i<_children.size(); i++ ) _children[i].node->get_subtree ( nodes ); + } + +void SrCfgNode::_deledge ( int e ) + { + _children.remove ( e ); + + int i, size = _children.size(); + for ( i=e; i_parentlink = i; + } + +void SrCfgNode::_fixplinks () + { + int i; + for ( i=0; i<_children.size(); i++ ) + _children[i].node->_parentlink = i; + for ( i=0; i<_children.size(); i++ ) + _children[i].node->_fixplinks(); + } + +void SrCfgNode::_reroot () + { + if ( !_parent ) return; + + // new parent will have its old parent as new child: + SrCfgNode* curnode = this; + SrCfgNode* curparent = _parent; + int newparentlink = _children.size(); + int oldparentlink = _parentlink; + _children.push() = _parent->_children[oldparentlink]; + _children.top().node = _parent; + _parent = 0; + _parentlink = -1; + + // walk towards the old root, swaping old/new parents: + int tmp; + SrCfgNode* newparent = this; + curnode = curparent; + curparent = curnode->_parent; // move to parent + while ( curparent ) + { // modify curnode: + curnode->_children[oldparentlink] = curparent->_children[curnode->_parentlink]; + curnode->_children[oldparentlink].node = curparent; // parent becomes child + tmp = oldparentlink; + oldparentlink = curnode->_parentlink; + curnode->_parentlink = newparentlink; + newparentlink = tmp; + curnode->_parent = newparent; + // move to parent: + newparent = curnode; + curnode = curparent; + curparent = curparent->_parent; + } + + // delete extra child of the old root: + curnode->_deledge ( oldparentlink ); + curnode->_parentlink = newparentlink; + curnode->_parent = newparent; + } + +//============================= SrCfgTreeBase ====================================== + +SrCfgTreeBase::SrCfgTreeBase ( SrCfgManagerBase* cman ) + { + _cman = cman; + _cman->ref (); + _root = 0; + } + +SrCfgTreeBase::~SrCfgTreeBase () + { + int i; + for ( i=0; i<_buffer.size(); i++ ) + { _cman->free ( _buffer[i]->_cfg ); + delete _buffer[i]; + } + _cman->unref(); + } + +bool SrCfgTreeBase::check_all ( SrOutput& o ) + { + int i, j; + o << "Starting check:\n"; + + if ( !_root ) { o<<"Check ok, but empty.\n"; return true; } + + o << "Buffer size...\n"; + _nodes.size ( 0 ); + _root->get_subtree ( _nodes ); + if ( _nodes.size()!=_buffer.size()-_freepos.size() ) goto error; + + o << "Buffer indices...\n"; + for ( i=0; i<_buffer.size(); i++ ) + if ( _buffer[i]->_bufferid!=i ) goto error; + + o << "Parent-child pointers...\n"; + for ( i=0; i<_nodes.size(); i++ ) + { if ( !_nodes[i]->_parent ) + { if ( _nodes[i]!=_root ) { o<<"wrong root "; goto error; } } + else + { if ( _nodes[i]->_parent->child(_nodes[i]->_parentlink)!=_nodes[i] ) + { o<< "wrong parent link n:" << + _nodes[i]->id()<<" p:"<<_nodes[i]->_parent->id()<children(); j++ ) + { if ( _nodes[i]->_children[j].node->_parent!=_nodes[i] ) + { o<<"wrong child->_parent pointer "; goto error; } + if ( _nodes[i]->_children[j].node->_parentlink!=j ) + { o<<"wrong child->_parentlink index "<< + _nodes[i]->id()<<"/"<0 ) + { return _buffer[_freepos.pop()]; + } + else + { int id = _buffer.size(); + _buffer.push() = new SrCfgNode; + _buffer[id]->_cfg = _cman->alloc(); + _buffer[id]->_bufferid = id; + return _buffer[id]; + } + } + +void SrCfgTreeBase::_delnode ( SrCfgNode* n ) + { + _freepos.push() = n->_bufferid; + } + +SrCfgNode* SrCfgTreeBase::add_node ( SrCfgNode* parent, const srcfg* cfg, float dist ) + { + SrCfgNode* newn = _newnode (); + + if ( parent ) // set parent data + { newn->_parentlink = parent->children(); + SrCfgNode::Link& l = parent->_children.push(); + if ( dist<0 ) dist = _cman->dist ( parent->_cfg, cfg ); + l.node = newn; + l.dist = dist; + l.level = 0; + } + else // this is the root node + { _root = newn; + newn->_parentlink = -1; + } + + _cman->copy ( newn->_cfg, cfg ); + newn->_parent = parent; + newn->_children.size(0); + + return newn; + } + +void SrCfgTreeBase::_nearest ( SrCfgNode* n, const srcfg* c, SrCfgNode*& nearest, float& mindist ) + { + // check distance: + float dist = _cman->dist ( n->cfg(), c ); + if ( distchildren(); + for ( i=0; ichild(i), c, nearest, mindist ); + } + +SrCfgNode* SrCfgTreeBase::search_nearest ( const srcfg* c, float* d ) + { + if ( !_root ) return _root; + + SrCfgNode* nearest; + float mindist = 1E+30f; // float range in visualc is: 3.4E +/- 38 + + _nearest ( _root, c, nearest, mindist ); + + if (d) *d = mindist; + return nearest; + } + +SrCfgNode* SrCfgTreeBase::expand_node ( SrCfgNode* source, const srcfg* direction, + float step, int maxtries, float dist ) + { + srcfg* csource = source->cfg(); + if ( dist<0 ) dist = _cman->dist ( csource, direction ); + if ( dist<0.00001f ) return 0; // too close + + SrCfgNode* nnew = _newnode(); + srcfg* cnew = nnew->cfg(); + float t = step/dist; + if ( t>1.0f ) t = 1.0f; + + while ( maxtries-->0 ) + { SR_TRACE1 ( "INS: trying to insert..."); + + _cman->interp ( csource, direction, t, cnew ); + t /= 2; + + if ( _cman->valid(cnew) ) + { SR_TRACE1 ( "INS: inserting 1 node."); + _delnode ( nnew ); + return add_node ( source, cnew, -1 ); // instead of -1, could use aprox distance dist*t... + } + } + + SR_TRACE1 ( "INS: no nodes inserted."); + _delnode ( nnew ); + return 0; + } + +SrCfgNode* SrCfgTreeBase::expand_node_safe ( SrCfgNode* source, const srcfg* direction, + float step, int maxtries, float prec, float dist ) + { + srcfg* csource = source->cfg(); + if ( dist<0 ) dist = _cman->dist ( csource, direction ); + if ( dist<0.00001f ) return 0; // too close + + SrCfgNode* nnew = _newnode(); + srcfg* cnew = nnew->cfg(); + SrCfgNode* ntmp = _newnode(); + srcfg* ctmp = ntmp->cfg(); + + float t = step/dist; + if ( t>1.0f ) t = 1.0f; + + if ( 0 ) // test "long" expansion + { float dt = t; + maxtries=5; + while ( t<=1 && maxtries-->0 ) + { SR_TRACE1 ( "INS: trying to insert..."); + _cman->interp ( csource, direction, t, cnew ); + t += dt; + + if ( _cman->valid(cnew) ) + if ( _cman->visible(source->cfg(),cnew,ctmp,prec) ) + { SR_TRACE1 ( "INS: inserting 1 node."); + _delnode ( nnew ); + _delnode ( ntmp ); + nnew = add_node ( source, cnew, -1 ); // instead of -1, could use aprox distance dist*t... + source->_children.top().level = -1; // mark as safe (can be any <0 number) + _cman->child_added ( source->cfg(), cnew ); // notify configuration manager + SR_TRACE1 ( "INS: Ok."); + source = nnew; + if ( t>1 ) return nnew; // end + + nnew = _newnode(); + cnew = nnew->cfg(); + ntmp = _newnode(); + ctmp = ntmp->cfg(); + + } + } + } + else + while ( maxtries-->0 ) + { SR_TRACE1 ( "INS: trying to insert..."); + + _cman->interp ( csource, direction, t, cnew ); + t /= 2; + + if ( _cman->valid(cnew) ) + if ( _cman->visible(source->cfg(),cnew,ctmp,prec) ) + { SR_TRACE1 ( "INS: inserting 1 node."); + _delnode ( nnew ); + _delnode ( ntmp ); + nnew = add_node ( source, cnew, -1 ); // instead of -1, could use aprox distance dist*t... + source->_children.top().level = -1; // mark as safe (can be any <0 number) + _cman->child_added ( source->cfg(), cnew ); // notify configuration manager + SR_TRACE1 ( "INS: Ok."); + return nnew; + //following test not ok: + //return expand_node_safe ( nnew, direction, step, maxtries, prec, -1 ); + } + } + + SR_TRACE1 ( "INS: no nodes inserted."); + _delnode ( nnew ); + _delnode ( ntmp ); + return 0; + } + +void SrCfgTreeBase::get_branch ( SrCfgNode* n, SrArray& nodes ) + { + while ( n ) + { nodes.push() = n; + n = n->parent(); + } + } + +void SrCfgTreeBase::get_branch ( SrCfgNode* n, SrCfgPathBase& path ) + { + while ( n ) + { path.push ( n->cfg() ); + n = n->parent(); + } + } + +void SrCfgTreeBase::get_nodes ( SrArray& nodes ) + { + nodes.size(0); + if ( !_root ) return; + _root->get_subtree ( nodes ); + } + +// level k : (2^k)+1 tot tests, 2^(k-1) new tests, 2^k segments +// safe edge : dist/segments < collision_precision +bool SrCfgTreeBase::increment_edge_level ( SrCfgNode* n1, SrCfgNode* n2, float prec ) + { + SrCfgNode::Link& l = n1->_children[n2->parentlink()]; + SrCfgNode* tmpnode = _newnode(); + srcfg* ct = tmpnode->cfg(); + srcfg* ct0 = n1->cfg(); + srcfg* ct1 = n2->cfg(); + + // we will test the next level k: + int k = 1 + l.level; + //sr_out<<"K: "<interp ( ct0, ct1, t, ct ); + if ( !_cman->valid(ct) ) { _delnode(tmpnode); return false; } + } + + // ok, promote edge to level k and check if it can be marked as safe: + l.level = k; + //sr_out<<"dist:"<_deledge ( n2->parentlink() ); + n2->_parent=0; // n2 is now the root of a disconnected subtree, still using _buffer + n2->_parentlink=-1; + + // save the nodes of subtree n2: + _nodes.size ( 0 ); + n2->get_subtree ( _nodes ); + + // reorder subtree to make joint1 the new root: + joint1->_reroot(); + + // attach joint1 children as children of joint2: + while ( joint1->_children.size() ) + { SrCfgNode::Link& l = joint1->_children.pop(); + l.node->_parent = joint2; + l.node->_parentlink = joint2->_children.size(); + joint2->_children.push() = l; + } + + // finally reorganize buffers; all in _nodes change buffer, except joint1 deleted: + int tmpi; + SrCfgNode *n, *newn, *tmpn; + while ( _nodes.size() ) + { n = _nodes.pop(); + _delnode (n); + if ( n!=joint1 ) // "swap" with a new entry in tree2.buffer + { newn = tree2._newnode(); + SR_SWAPT(_buffer[n->_bufferid],tree2._buffer[newn->_bufferid],tmpn); + SR_SWAPT(n->_bufferid,newn->_bufferid,tmpi); + } + } + } + +void SrCfgTreeBase::output ( SrOutput& o, bool printcfg, SrCfgNode* n ) + { + int i, j; + + _nodes.size(0); + if ( !n ) n = _root; + n->get_subtree ( _nodes ); + + o << "\nSrCfgTree " << _nodes.size() << srnl; + + for ( i=0; i<_nodes.size(); i++ ) + { + n = _nodes[i]; + //t._cman->output ( o, t.cfg(i) ); + o << "Node:" << n->id(); + if ( n->parent() ) o << " parent:"<parent()->id(); + else o<<" root "; + + o << " Children:"; + for ( j=0; jchildren(); j++ ) + { o << srspc << n->child(j)->id() + << ":" << j + << "/" << n->child(j)->parentlink(); + } + + if ( printcfg ) + { o << "\nData: "; + _cman->output(o,n->cfg()); + o <<"\n"; + } + o << srnl; + } + } + +//================================ friends ================================================= + + +/* + void _inpnode ( SrInput& i, SrCfgNode* n ); +void SrCfgTreeBase::_inpnode ( SrInput& in, SrCfgNode* n ) + { + int i, size; + _cman->input ( in, n->_cfg ); + + in.get_token(); // "p" + in >> n->_parent; + in.get_token(); // "c" + in >> size; + in.get_token(); // ":" + + n->_children.size(size); + for ( i=0; i> n->_children[i].dist; + in >> n->_children[i].node; + in >> n->_children[i].level; + } + }*/ + + /*! read the roadmap */ + /*friend SrInput& operator>> ( SrInput& in, SrCfgTreeBase& t ); +SrInput& operator>> ( SrInput& in, SrCfgTreeBase& t ) + { + int i, size; + + t.init(); + + in.get_token(); + if ( in.last_token()!="SrCfgTree" ) return in; + + in >> size; + + for ( i=0; iinput ( in, t.cfg(i) ); + t._inpnode ( in, t.node(i) ); + } + + t._nsize = size; + + return in; + } +*/ +//================================ End of File ================================================= + + + +/* + This method takes O(k*n) time. It looks into all nodes of the graph, + updating the sorted list with the k closest nodes, which is returned. +SrArray& SrCfgTreeBase::search_k_nearests ( const srcfg* c, int k ) + { + _snodes.ensure_capacity(k); + _snodes.size(0); + SrRoadmapNode* n = _graph.first_node(); + if ( !n ) return _snodes; + + NodeDist nd; + int i, worse=-1; + + SrListIterator it(n); + for ( it.first(); it.inrange(); it.next() ) + { if ( it.get()->blocked() ) continue; + nd.n = it.get(); + nd.d = _cman->get_distance ( it.get()->c, c ); + + if ( _snodes.size() _snodes[worse].d ) worse=i; + } + } + + if ( _snodes.size()>1 ) _snodes.sort(nd_compare); + + return _snodes; + } + + + + */ + diff --git a/source/dcdt/se/sr_cfg_tree.h b/source/dcdt/se/sr_cfg_tree.h new file mode 100644 index 0000000000..29eda0a0be --- /dev/null +++ b/source/dcdt/se/sr_cfg_tree.h @@ -0,0 +1,169 @@ + +# ifndef SR_CFG_TREE_H +# define SR_CFG_TREE_H + +# include "sr_set.h" +//# include +# include "sr_cfg_manager.h" +# include "sr_cfg_path.h" + +class SrCfgTreeBase; + +/*! SrCfgNode is a node of SrCfgTree. Each edge has a + level based on the number of collision checks performed: + Level TotTests NewTests Segments + 0 2 0 1 + 1 3 1 2 + 2 5 2 4 + 3 9 4 8 + k (2^k)+1 2^(k-1) 2^k + Safe edge is achieved if dist/(2^k) < collision precision, + and level is marked as safe when this occurs */ +class SrCfgNode + { private : + int _bufferid; + srcfg* _cfg; + SrCfgNode* _parent; + int _parentlink; + struct Link { SrCfgNode* node; float dist; int level; }; + SrArray _children; + friend SrCfgTreeBase; + public : + SrCfgNode* parent () const { return _parent; } + int parentlink () const { return _parentlink; } // -1 if root node + int children () const { return _children.size(); } + srcfg* cfg () const { return _cfg; } + SrCfgNode* child ( int i ) const { return _children[i].node; } + float prec ( int i ) const; // dist/2^abs(level) + float dist ( int i ) const { return _children[i].dist; } + int level ( int i ) const { return _children[i].level; } // <0 if safe + bool safe ( int i ) const { return level(i)<0?true:false; } + void get_subtree ( SrArray& nodes ); // add the node itself and all subtree + int id () const { return _bufferid; } + private : + void _deledge ( int e ); + void _fixplinks (); + void _reroot (); + }; + +/*! The tree-based roadmap */ +class SrCfgTreeBase + { private : + SrCfgManagerBase* _cman; // configuration manager + SrCfgNode* _root; // the root of the tree + SrArray _buffer; // buffer of nodes + SrArray _freepos; // free positions in buffer + SrArray _nodes; // for temporary use + public : + + /*! The constructor requires a valid configuration manager for + dealing with the user-defined configuration */ + SrCfgTreeBase ( SrCfgManagerBase* cman ); + + /*! Destructor frees all used internal data, and unref the + associated configuration manager */ + ~SrCfgTreeBase (); + + /* Returns a pointer to the used configuration manager */ + SrCfgManagerBase* cman () const { return _cman; } + + /*! Debug tool to test all internal pointers, should always return true */ + bool check_all ( SrOutput& o ); + + /*! Set the tree as an empty tree */ + void init (); + + /*! Set the current tree to be a tree containing + only the given node as the root of the tree. */ + void init ( const srcfg* cfg ); + + /*! Add a node to the tree, as a child to the provided parent. + If dist<0 (the default) the parent-cfg distance is retrieved + by using the associated configuration manager. + The level of the parent-cfg edge is set to 0. + Returns the new node created. */ + SrCfgNode* add_node ( SrCfgNode* parent, const srcfg* cfg, float dist=-1 ); + + /*! Returns the number of nodes in the tree */ + int nodes () const { return _buffer.size()-_freepos.size(); } + + /*! Returns the root node or null if tree empty */ + SrCfgNode* root () const { return _root; } + + /*! Performs a O(n) search over all nodes and returns the node + with the closest configuration to c. Null is returned if the + tree is empty. If given pointer d is not null, the nearest + distance is returned in d */ + SrCfgNode* search_nearest ( const srcfg* c, float* d=0 ); + + /*! Add a child node from node source, walking a distance of step in direction + to configuration direction. If the node is not valid, step is divided + by 2 until the node becomes valid, or until max_tries tentatives are performed. + Note that here edges are not tested for validity, only nodes (lazy evaluation). + The new node is returned, or null in case no node could be added. + If parameter dist>0, it will be used as being dist(source->cfg(),direction). */ + SrCfgNode* expand_node ( SrCfgNode* source, const srcfg* direction, float step, int maxtries, float dist=-1 ); + + /*! Same as expand node, but here the expansion is done only if the new edge is + valid, therefore the edge visibility tests is called */ + SrCfgNode* expand_node_safe ( SrCfgNode* source, const srcfg* direction, + float step, int maxtries, float prec, float dist=-1 ); + + /*! Returns a list of nodes forming the tree branch joining node n to the root. + The first element of the array is always n, and the last is always the root node. + The array size is not set to zero, ie, the indices are apended to the array. */ + void get_branch ( SrCfgNode* n, SrArray& nodes ); + + /*! Same as the other get_branch() method, but the result goes to a path object */ + void get_branch ( SrCfgNode* n, SrCfgPathBase& path ); + + /*! Returns an unordered list with all nodes in the tree */ + void get_nodes ( SrArray& nodes ); + + /*! Performs 2^(k-1) collision tests to check if the edge [n1,n2] can be promoted + to level k = n1->level(n2->parentlink())+1. + False is returned in case the level could not be promoted due to a collision. + True is returned if the level could be promoted, and in this case, the edge + will be marked as safe if the new level achieves the required precision + with the following test: n1->dist(n2->parentlink())/2^k < prec */ + bool increment_edge_level ( SrCfgNode* n1, SrCfgNode* n2, float prec ); + + /*! Removes the edge [n1,n2] and transfers the disconnected subtree to tree2, + by "identifying" joint1 with joint2 of tree2 and reorganizing the subtree + so as to make joint2 the subtree root. + Important: n1 must be parent of n2, and joint1 must be in n2 subtree */ + void transfer_subtree ( SrCfgNode* n1, SrCfgNode* n2, SrCfgNode* joint1, + SrCfgTreeBase& tree2, SrCfgNode* joint2 ); + + /*! Output of the roadmap tree for inspection: + - if printcfg is true, node data is also sent to output + - n is the root of the subgraph to print (if 0, the real root is taken)*/ + void output ( SrOutput& o, bool printcfg=true, SrCfgNode* n=0 ); + + private : + SrCfgNode* _newnode (); + void _delnode ( SrCfgNode* n ); + srcfg* _tmpnode (); + void _nearest ( SrCfgNode* n, const srcfg* c, SrCfgNode*& nearest, float& mindist ); + }; + +/*! This template version performs all required type casts to bind the + tree to the user-defined configuration class. Class C must be managed + by the used configuration manager */ +template +class SrCfgTree : public SrCfgTreeBase + { public : + /*! Constructor receiving a user-defined manager */ + SrCfgTree ( SrCfgManagerBase* cman ) : SrCfgTreeBase(cman) { } + + /*! Automatically allocates a manager using SrCfgManager template */ + SrCfgTree () : SrCfgTreeBase ( new SrCfgManager ) { } + + /*! Returns the configuration at node index n */ + C* cfg ( SrCfgNode* n ) const { return (C*)n->cfg(); } + }; + +//================================ End of File ================================================= + +# endif // SR_CFG_TREE_H + diff --git a/source/dcdt/se/sr_class_manager.h b/source/dcdt/se/sr_class_manager.h new file mode 100644 index 0000000000..aeaf8eea55 --- /dev/null +++ b/source/dcdt/se/sr_class_manager.h @@ -0,0 +1,59 @@ + +# ifndef SR_CLASS_MANAGER_H +# define SR_CLASS_MANAGER_H + +/** \file sr_class_manager.h + * Generic way to allocate, io and compare classes */ + +# include "sr_input.h" +# include "sr_output.h" +# include "sr_shared_class.h" + +class SrClassManagerBase : public SrSharedClass + { protected : + virtual ~SrClassManagerBase() {}; + + public : // callbacks + virtual void* alloc ()=0; + virtual void* alloc ( const void* obj )=0; + virtual void free ( void* obj )=0; + virtual void output ( SrOutput& o, const void* obj ) { } + virtual void input ( SrInput& i, void* obj ) { } + virtual int compare ( const void* obj1, const void* obj2 ) { return 0; } + }; + +/*! Example of an implementation of a class to be automatically managed + with SrClassManager : +class MyData + { public : + MyData (); + MyData ( const MyData& d ); + ~MyData (); + friend SrOutput& operator<< ( SrOutput& out, const MyData& d ); + friend SrInput& operator>> ( SrInput& inp, MyData& d ); + friend int sr_compare ( const MyData* d1, const MyData* d2 ); + };*/ + +template +class SrClassManager : public SrClassManagerBase + { protected : + virtual ~SrClassManager () {} + + public : + virtual void* alloc () { return (void*) new X; } + + virtual void* alloc ( const void* obj ) { return (void*) new X(*((X*)obj)); } + + virtual void free ( void* obj ) { delete (X*) obj; } + + virtual void output ( SrOutput& o, const void* obj ) { o<<*((const X*)obj); } + + virtual void input ( SrInput& i, void* obj ) { i>>*((X*)obj); } + + virtual int compare ( const void* obj1, const void* obj2 ) + { return sr_compare ( (const X*)obj1, (const X*)obj2 ); } + }; + +//============================== end of file =============================== + +# endif // SR_CLASS_MANAGER_H diff --git a/source/dcdt/se/sr_color.cpp b/source/dcdt/se/sr_color.cpp new file mode 100644 index 0000000000..54cb53f6ae --- /dev/null +++ b/source/dcdt/se/sr_color.cpp @@ -0,0 +1,108 @@ +#include "precompiled.h" +# include "sr_color.h" + +//========================================= static ======================================= + +const SrColor SrColor::black (0,0,0); +const SrColor SrColor::red (255,0,0); +const SrColor SrColor::green (0,255,0); +const SrColor SrColor::yellow (255,255,0); +const SrColor SrColor::blue (0,0,255); +const SrColor SrColor::magenta(255,0,255); +const SrColor SrColor::cyan (0,255,255); +const SrColor SrColor::white (255,255,255); +const SrColor SrColor::gray (127,127,127); + +//========================================= SrColor ======================================= + +void SrColor::set ( srbyte x, srbyte y, srbyte z, srbyte w ) + { + r=x; g=y; b=z; a=w; + } + +void SrColor::set ( int x, int y, int z, int w ) + { + r=(srbyte)x; + g=(srbyte)y; + b=(srbyte)z; + a=(srbyte)w; + } + +void SrColor::set ( float x, float y, float z, float w ) + { + r = (srbyte) ( x*255.0f ); + g = (srbyte) ( y*255.0f ); + b = (srbyte) ( z*255.0f ); + a = (srbyte) ( w*255.0f ); + } + +void SrColor::set ( const char* s ) + { + switch ( s[0] ) + { case 'b' : *this = s[2]=='a'? black:blue; break; + case 'r' : *this = red; break; + case 'g' : *this = s[2]=='e'? green:gray; break; + case 'y' : *this = yellow; break; + case 'm' : *this = magenta; break; + case 'c' : *this = cyan; break; + case 'w' : *this = white; break; + } + } + +// we dont do double versions to avoid automatic typecasts complications... +void SrColor::get ( float f[4] ) const + { + f[0] = ((float)r) / 255.0f; + f[1] = ((float)g) / 255.0f; + f[2] = ((float)b) / 255.0f; + f[3] = ((float)a) / 255.0f; + } + +void SrColor::get ( int i[4] ) const + { + i[0] = (int)r; + i[1] = (int)g; + i[2] = (int)b; + i[3] = (int)a; + } + +void SrColor::get ( srbyte x[4] ) const + { + x[0] = r; + x[1] = g; + x[2] = b; + x[3] = a; + } + +bool operator == ( const SrColor &c1, const SrColor &c2 ) + { + return c1.r==c2.r && c1.g==c2.g &&c1.b==c2.b && c1.a==c2.a? true:false; + } + +bool operator != ( const SrColor &c1, const SrColor &c2 ) + { + return c1.r==c2.r && c1.g==c2.g &&c1.b==c2.b && c1.a==c2.a? false:true; + } + +SrColor lerp ( const SrColor &c1, const SrColor &c2, float t ) + { + SrColor c; + + c.r = (srbyte) (SR_LERP ( float(c1.r), float(c2.r), t ) + 0.5f); + c.g = (srbyte) (SR_LERP ( float(c1.g), float(c2.g), t ) + 0.5f); + c.b = (srbyte) (SR_LERP ( float(c1.b), float(c2.b), t ) + 0.5f); + c.a = (srbyte) (SR_LERP ( float(c1.a), float(c2.a), t ) + 0.5f); + return c; + } + +SrOutput& operator<< ( SrOutput& o, const SrColor& c ) + { + return o << c.r <<' '<< c.g <<' '<< c.b <<' '<< c.a; + } + +SrInput& operator>> ( SrInput& in, SrColor& c ) + { + return in >> c.r >> c.g >> c.b >> c.a; + } + +//=================================== End of File ========================================== diff --git a/source/dcdt/se/sr_color.h b/source/dcdt/se/sr_color.h new file mode 100644 index 0000000000..34435fe7cf --- /dev/null +++ b/source/dcdt/se/sr_color.h @@ -0,0 +1,99 @@ + +# ifndef SR_COLOR_H +# define SR_COLOR_H + +/** \file sr_color.h + * A color definition + */ + +# include "sr_input.h" +# include "sr_output.h" + +/*! \class SrColor sr_color.h + \brief specifies a color + + SrColor specifies a color using 8 bits for each basic color (red,green,blue) + and more 8 bits for the alpha (the opacity). In this way, each component can + have a value from 0 to 255 and the total class has a sizeof of 4 bytes. + The default constructor initializes with values (r,g,b,a)=(127,127,127,255). */ +class SrColor + { public : + static const SrColor black, //!< black color (0,0,0) + red, //!< red color (255,0,0) + green, //!< green color (0,255,0) + yellow, //!< yellow color (255,255,0) + blue, //!< blue color (0,0,255) + magenta, //!< magenta color (255,0,255) + cyan, //!< cyan color (0,255,255) + white, //!< white color (255,255,255) + gray; //!< gray color (127,127,127) + + srbyte r; //!< r component, in {0,...,255}, default is 127 + srbyte g; //!< g component, in {0,...,255}, default is 127 + srbyte b; //!< b component, in {0,...,255}, default is 127 + srbyte a; //!< a component, in {0,...,255}, default is 255, that is full opacity + + public : + + /*! Default constructor. Initializes with color gray. */ + SrColor () { *this=gray; } + + /*! Constructor setting all components. */ + SrColor ( srbyte x, srbyte y, srbyte z, srbyte w=255 ) { set(x,y,z,w); } + + /*! Constructor setting all components. */ + SrColor ( int x, int y, int z, int w=255 ) { set(x,y,z,w); } + + /*! Constructor setting all components with float types. */ + SrColor ( float x, float y, float z, float w=1.0f ) { set(x,y,z,w); } + + /*! Constructor from a 4 dimension float pointer. */ + SrColor ( const float v[4] ) { set(v); } + + /*! Sets the components of the color, the alpha value has a default parameter of 255. */ + void set ( srbyte x, srbyte y, srbyte z, srbyte w=255 ); + + /*! Sets the components of the color with integer values also betwenn 1 and 255, + the alpha value has a default parameter of 255. */ + void set ( int x, int y, int z, int w=255 ); + + /*! Sets the components of the color with float values, each one inside [0.0,1.0], + the alpha value has a default parameter of 1.0. */ + void set ( float x, float y, float z, float w=1.0f ); + + /*! Sets the components from and array of four floats. */ + void set ( const float v[4] ) { set(v[0],v[1],v[2],v[3]); } + + /*! Sets the color with a string containing one of the following: + black, red, green, yellow, blue, magenta, cyan, white, gray */ + void set ( const char* s ); + + /*! Put the four components in f[], translating each one to range [0.0,1.0] */ + void get ( float f[4] ) const; + + /*! Put the four components in i[], each component varying from 0 to 255. */ + void get ( int i[4] ) const; + + /*! Put the four components in b[], each component varying from 0 to 255. */ + void get ( srbyte b[4] ) const; + + /*! Comparison equal operator. */ + friend bool operator == ( const SrColor &c1, const SrColor &c2 ); + + /*! Comparison difference operator. */ + friend bool operator != ( const SrColor &c1, const SrColor &c2 ); + + /*! Interpolates two colors. */ + friend SrColor lerp ( const SrColor &c1, const SrColor &c2, float t ); + + /*! Outputs in format 'r g b a'. */ + friend SrOutput& operator<< ( SrOutput& o, const SrColor& v ); + + /*! Reads from format 'r g b a'. */ + friend SrInput& operator>> ( SrInput& in, SrColor& v ); + }; + +//================================ End of File ================================================= + +# endif // SR_COLOR_H + diff --git a/source/dcdt/se/sr_cylinder.cpp b/source/dcdt/se/sr_cylinder.cpp new file mode 100644 index 0000000000..5d721ec2e7 --- /dev/null +++ b/source/dcdt/se/sr_cylinder.cpp @@ -0,0 +1,55 @@ +#include "precompiled.h" +# include "sr_cylinder.h" +# include "sr_box.h" + +//================================== SrCylinder ==================================== + +const char* SrCylinder::class_name = "Cylinder"; + +SrCylinder::SrCylinder () : a(SrVec::null), b(SrVec::i) + { + radius = 0.1f; + } + +SrCylinder::SrCylinder ( const SrCylinder& c ) : a(c.a), b(c.b) + { + radius = c.radius; + } + +void SrCylinder::get_bounding_box ( SrBox& box ) const + { + SrVec va = b-a; + va.normalize(); + SrVec vr1; + if ( angle(SrVec::i,va)<0.1f ) + vr1 = cross ( SrVec::j, va ); + else + vr1 = cross ( SrVec::i, va ); + + SrVec vr2 = cross ( vr1, va ); + + vr1.len ( radius ); + vr2.len ( radius ); + + box.set_empty(); + box.extend ( a+vr1 ); + box.extend ( a-vr1 ); + box.extend ( a+vr2 ); + box.extend ( a-vr2 ); + box.extend ( b+vr1 ); + box.extend ( b-vr1 ); + box.extend ( b+vr2 ); + box.extend ( b-vr2 ); + } + +SrOutput& operator<< ( SrOutput& o, const SrCylinder& c ) + { + return o << c.a << srspc << c.b << srspc << c.radius; + } + +SrInput& operator>> ( SrInput& in, SrCylinder& c ) + { + return in >> c.a >> c.b >> c.radius; + } + +//================================ EOF ================================================= diff --git a/source/dcdt/se/sr_cylinder.h b/source/dcdt/se/sr_cylinder.h new file mode 100644 index 0000000000..a3fe269207 --- /dev/null +++ b/source/dcdt/se/sr_cylinder.h @@ -0,0 +1,46 @@ + +# ifndef SR_CYLINDER_H +# define SR_CYLINDER_H + +/** \file sr_cylinder.h + * a cylinder + */ + +# include "sr_vec.h" + +class SrBox; + +/*! \class SrCylinder sr_cylinder.h + \brief a cylinder + + SrCylinder represents a cylinder based on its endpoints and radius. + By default, the cylinder has endpoints (0,0,0) and (1,0,0) and radius 0.1*/ +class SrCylinder + { public : + SrPnt a, b; + float radius; + static const char* class_name; //!< constain the static string "Cylinder" + + public : + + /*! Constructs a cylinder with endpoints (0,0,0) and (1,0,0) and radius 1 */ + SrCylinder (); + + /*! Copy constructor */ + SrCylinder ( const SrCylinder& c ); + + /*! Returns the bounding box of all vertices used. The returned box can be empty. */ + void get_bounding_box ( SrBox &b ) const; + + /*! Outputs in format "p1 p2 radius " */ + friend SrOutput& operator<< ( SrOutput& o, const SrCylinder& c ); + + /*! Input from format "p1 p2 radius " */ + friend SrInput& operator>> ( SrInput& in, SrCylinder& c ); + }; + + +//================================ End of File ================================================= + +# endif // SR_SCENE_CYLINDER_H + diff --git a/source/dcdt/se/sr_deque.h b/source/dcdt/se/sr_deque.h new file mode 100644 index 0000000000..797fc1f996 --- /dev/null +++ b/source/dcdt/se/sr_deque.h @@ -0,0 +1,55 @@ + +# ifndef SR_DEQUE_H +# define SR_DEQUE_H + +/** \file sr_deque.h + * double-ended queue */ + +# include "sr_array.h" + +/*! \class SrDeque sr_deque.h + \brief double-ended queue + A double-ended queue templete based on SrArray. */ +template +class SrDeque // used by the funnel algorithm + { private : + SrArray _array; + int _base; + bool _topmode; + + public : + SrDeque () { _base=0; _topmode=true; } + + int size () { return _array.size()-_base; } + void init ( int cap ) { _array.ensure_capacity(cap); _array.size(cap/2); _base=_array.size(); } + void init () { _array.size(_array.capacity()/2); _base=_array.size(); } + void compress () { _array.remove(0,_base); _base=0; _array.compress(); } + + X& top ( int i ) { return _array[_array.size()-i-1]; } + X& top () { return _array.top(); } + X& popt () { return _array.pop(); } + X& pusht () { return _array.push(); } + + X& bottom ( int i ) { return _array[_base+i]; } + X& bottom () { return _array[_base]; } + X& popb () { return _array[_base++]; } + X& pushb () + { if (_base==0) { _base=_array.size(); _array.insert(0,_base); } + return _array[--_base]; + } + + X& operator[] ( int i ) { return _array[_base+i]; } + + public : + bool top_mode () const { return _topmode; } + void top_mode ( bool b ) { _topmode=b; } + X& get ( int i ) { return _topmode? top(i):bottom(i); } + X& get () { return _topmode? top():bottom(); } + X& pop () { return _topmode? popt():popb(); } + X& push () { return _topmode? pusht():pushb(); } + }; + +//============================== end of file =============================== + +#endif // SR_DEQUE_H + diff --git a/source/dcdt/se/sr_euler.cpp b/source/dcdt/se/sr_euler.cpp new file mode 100644 index 0000000000..af79ee5237 --- /dev/null +++ b/source/dcdt/se/sr_euler.cpp @@ -0,0 +1,188 @@ +#include "precompiled.h" +# include "sr_euler.h" +# include "sr_mat.h" +# include + +//# define SR_USE_TRACE1 +# include "sr_trace.h" + +# define ISZERO(a) ( (a)>-(srtiny) && (a)<(srtiny) ) + +# define EQUAL(a,b) ( ( (a)>(b)? ((a)-(b)):((b)-(a)) )<=(srtiny) ) + +# define GETSINCOS double cx=cos(rx); double cy=cos(ry); double cz=cos(rz); \ + double sx=sin(rx); double sy=sin(ry); double sz=sin(rz) + +# define ATAN2(x,y) (float) atan2 ( (double)x, (double)y ) +# define NORM(x,y) sqrt(double(x)*double(x)+double(y)*double(y)) + +//============================ Get Angles ================================ + +void sr_euler_angles ( int order, const SrMat& m, float& rx, float& ry, float& rz ) + { + switch ( order ) + { case 123: sr_euler_angles_xyz(m,rx,ry,rz); break; + case 132: sr_euler_angles_xzy(m,rx,ry,rz); break; + case 213: sr_euler_angles_yxz(m,rx,ry,rz); break; + case 231: sr_euler_angles_yzx(m,rx,ry,rz); break; + case 312: sr_euler_angles_zxy(m,rx,ry,rz); break; + case 321: sr_euler_angles_zyx(m,rx,ry,rz); break; + default: rx=ry=rz=0; + } + } + +void sr_euler_angles_xyz ( const SrMat& m, float& rx, float& ry, float& rz ) + { + ry = ATAN2 ( -m[2], NORM(m[0],m[1]) ); + rx = ATAN2 ( m[6], m[10] ); + rz = ATAN2 ( m[1], m[0] ); + } + +void sr_euler_angles_xzy ( const SrMat& m, float& rx, float& ry, float& rz ) + { + rx = ATAN2 ( -m[9], m[5] ); + ry = ATAN2 ( -m[2], m[0] ); + rz = ATAN2 ( m[1], NORM(m[5],m[9]) ); + } + +void sr_euler_angles_yxz ( const SrMat& m, float& rx, float& ry, float& rz ) + { + rx = ATAN2 ( m[6], NORM(m[2],m[10]) ); + ry = ATAN2 ( -m[2], m[10] ); + rz = ATAN2 ( -m[4], m[5] ); + } + +void sr_euler_angles_yzx ( const SrMat& m, float& rx, float& ry, float& rz ) + { + rz = ATAN2 ( -m[4], NORM(m[0],m[8]) ); + rx = ATAN2 ( m[6], m[5] ); + ry = ATAN2 ( m[8], m[0] ); + } + +void sr_euler_angles_zxy ( const SrMat& m, float& rx, float& ry, float& rz ) + { + rx = ATAN2 ( -m[9], NORM(m[1],m[5]) ); + rz = ATAN2 ( m[1], m[5] ); + ry = ATAN2 ( m[8], m[10] ); + } + +void sr_euler_angles_zyx ( const SrMat& m, float& rx, float& ry, float& rz ) + { + ry = ATAN2 ( m[8], NORM(m[0],m[4]) ); + rx = ATAN2 ( -m[9], m[10] ); + rz = ATAN2 ( -m[4], m[0] ); + } + +//============================ Get Mat ================================ + +void sr_euler_mat ( int order, SrMat& m, float rx, float ry, float rz ) + { + switch ( order ) + { case 123: sr_euler_mat_xyz(m,rx,ry,rz); break; + case 132: sr_euler_mat_xzy(m,rx,ry,rz); break; + case 213: sr_euler_mat_yxz(m,rx,ry,rz); break; + case 231: sr_euler_mat_yzx(m,rx,ry,rz); break; + case 312: sr_euler_mat_zxy(m,rx,ry,rz); break; + case 321: sr_euler_mat_zyx(m,rx,ry,rz); break; + default: m.identity(); + } + } + +void sr_euler_mat_xyz ( SrMat& m, float rx, float ry, float rz ) + { + GETSINCOS; + // the following is the same as: R=Rx*Ry*Rz (in column-major format) + m[0]=float(cy*cz); m[1]=float(cy*sz); m[2]=float(-sy); + m[4]=float(-cx*sz+sx*sy*cz); m[5]=float(cx*cz+sx*sy*sz); m[6]=float(sx*cy); + m[8]=float(sx*sz+cx*sy*cz); m[9]=float(-sx*cz+cx*sy*sz); m[10]=float(cx*cy); + } + +void sr_euler_mat_xzy ( SrMat& m, float rx, float ry, float rz ) + { + GETSINCOS; + // the following is the same as: R=Rx*Rz*Ry (in column-major format) + m[0]=float(cy*cz); m[1]=float(sz); m[2]=float(-cz*sy); + m[4]=float(-cx*sz*cy+sx*sy); m[5]=float(cx*cz); m[6]=float(cx*sy*sz+sx*cy); + m[8]=float(sx*sz*cy+cx*sy); m[9]=float(-sx*cz); m[10]=float(-sx*sy*sz+cx*cy); + } + +void sr_euler_mat_yxz ( SrMat& m, float rx, float ry, float rz ) + { + GETSINCOS; + // the following is the same as: R=Ry*Rx*Rz (in column-major format) + m[0]=float(cy*cz-sx*sy*sz); m[1]=float(cy*sz+sx*sy*cz); m[2]=float(-cx*sy); + m[4]=float(-cx*sz); m[5]=float(cx*cz); m[6]=float(sx); + m[8]=float(cz*sy+sx*cy*sz); m[9]=float(sy*sz-sx*cy*cz); m[10]=float(cx*cy); + } + +void sr_euler_mat_yzx ( SrMat& m, float rx, float ry, float rz ) + { + GETSINCOS; + // the following is the same as: R=Ry*Rz*Rx (in column-major format) + m[0]=float(cy*cz); m[1]=float(cx*cy*sz+sx*sy); m[2]=float(cy*sz*sx-cx*sy); + m[4]=float(-sz); m[5]=float(cx*cz); m[6]=float(cz*sx); + m[8]=float(sy*cz); m[9]=float(cx*sy*sz-cy*sx); m[10]=float(sx*sy*sz+cx*cy); + } + +void sr_euler_mat_zxy ( SrMat& m, float rx, float ry, float rz ) + { + GETSINCOS; + // the following is the same as: R=Rz*Rx*Ry (in column-major format) + m[0]=float(cy*cz+sx*sy*sz); m[1]=float(cx*sz); m[2]=float(-sy*cz+sx*cy*sz); + m[4]=float(-sz*cy+sx*sy*cz); m[5]=float(cx*cz); m[6]=float(sy*sz+sx*cy*cz); + m[8]=float(cx*sy); m[9]=float(-sx); m[10]=float(cx*cy); + } + +void sr_euler_mat_zyx ( SrMat& m, float rx, float ry, float rz ) + { + GETSINCOS; + // the following is the same as: R=Rz*Ry*Rx (in column-major format) + m[0]=float(cy*cz); m[1]=float(cx*sz+cz*sy*sx); m[2]=float(sx*sz-cx*cz*sy); + m[4]=float(-sz*cy); m[5]=float(cx*cz-sx*sy*sz); m[6]=float(sx*cz+cx*sy*sz); + m[8]=float(sy); m[9]=float(-sx*cy); m[10]=float(cx*cy); + } + +//================================ EOF =================================== + +/* Math: + |1 0 0| |cy 0 -sy| | cz sz 0| + Rx=|0 cx sx| Ry=| 0 1 0| Rz=|-sz cz 0| + |0 -sx cx| |sy 0 cy| | 0 0 1| + + | cy 0 -sy| |cycz cysz -sy| | cycz sz -czsy| + RxRy = |sxsy cx sxcy| RyRz = | -sz cz 0| RzRy = |-szcy cz sysz| + |cxsy -sx cxcy| |sycz sysz cy| | sy 0 cy| + + | cycz cysz -sy | | m0 m1 m2 | +RxRyRz = |-cxsz+sxsycz cxcz+sxsysz sx*cy | = | m4 m5 m6 | + | sxsz+cxsycz -sxcz+cxsysz cx*cy | | m8 m9 m10| +X: sx/cx = m6/m10 +Z: sz/cz = m1/m0 +Y: cy^2*cz^2 + cy^2*sz^2 = m0^2+m1^2 => cy = norm(m0,m1) => sy/cy = -m2/norm(m0,m1) + + | cycz sz -czsy | | m0 m1 m2 | +RxRzRy = | -cxszcy+sxsy cxcz cxsysz+sxcy | = | m4 m5 m6 | + | sxszcy+cxsy -sxcz -sxsysz+cxcy | | m8 m9 m10| +X: -sx/cx = m9/m5 +Y: -sy/cy = m2/m0 +Z: cx^2*cz^2 + cz^2*sx^2 = m5^2+m9^2 => cz = norm(m5,m9) => sz/cz = m1/norm(m5,m9) + + | cycz+sxsysz cxsz -sycz+sxcysz | | m0 m1 m2 | +RzRxRy = | -szcy+sxsycz cxcz sysz+sxcycz | = | m4 m5 m6 | + | cxsy -sx cxcy | | m8 m9 m10| +Z: sz/cz = m1/m5 +Y: sy/cy = m8/m10 +X: cx^2*sz^2 + cx^2*cz^2 = m1^2+m5^2 => cx = norm(m1,m5) => sx/cx = -m9/norm(m1,m5) + + | cycz cxsz+czsysx sxsz-cxczsy | | m0 m1 m2 | +RzRyRx = | -szcy cxcz-sxsysz sxcz+cxsysz | = | m4 m5 m6 | + | sy -sxcy cxcy | | m8 m9 m10| +X: -sx/cx = m9/m10 +Z: -sz/cz = m4/m0 +Y: cy^2*cz^2 + sz^2*cy^2 = m0^2+m4^2 => cy = norm(m0,m4) => sy/cy = m8/norm(m0,m4) + +(Euler is fixed axis, the "inver order" becomes moving axis) +*/ + +//================================ EOF =================================== + diff --git a/source/dcdt/se/sr_euler.h b/source/dcdt/se/sr_euler.h new file mode 100644 index 0000000000..efe1487ae9 --- /dev/null +++ b/source/dcdt/se/sr_euler.h @@ -0,0 +1,41 @@ +/** \file sr_euler.h + * euler angles tools */ + +# ifndef SR_EULER_H +# define SR_EULER_H + +//================================ Angles from Mat ================================ + +class SrMat; + +/*! Extract from the rotation matrix m the x,y,z Euler angles, + assuming they were encoded in m in the given order. + Parameter order should be: 123 for XYZ, 132 for XZY, etc. + The other sr_euler_angles* functions are similar, but specific to the desired order. */ +void sr_euler_angles ( int order, const SrMat& m, float& rx, float& ry, float& rz ); + +void sr_euler_angles_xyz ( const SrMat& m, float& rx, float& ry, float& rz ); +void sr_euler_angles_xzy ( const SrMat& m, float& rx, float& ry, float& rz ); +void sr_euler_angles_yxz ( const SrMat& m, float& rx, float& ry, float& rz ); +void sr_euler_angles_yzx ( const SrMat& m, float& rx, float& ry, float& rz ); +void sr_euler_angles_zxy ( const SrMat& m, float& rx, float& ry, float& rz ); +void sr_euler_angles_zyx ( const SrMat& m, float& rx, float& ry, float& rz ); + +//================================ Mat from Angles ================================ + +/*! Set the rotation matrix m according to the given x,y,z Euler angles. + Parameter order should be: 123 for XYZ, 132 for XZY, etc. + Note: the 4th line and column of m are not changed. + The other sr_euler_mat* functions are similar, but specific to the desired order. */ +void sr_euler_mat ( int order, SrMat& m, float rx, float ry, float rz ); + +void sr_euler_mat_xyz ( SrMat& m, float rx, float ry, float rz ); +void sr_euler_mat_xzy ( SrMat& m, float rx, float ry, float rz ); +void sr_euler_mat_yxz ( SrMat& m, float rx, float ry, float rz ); +void sr_euler_mat_yzx ( SrMat& m, float rx, float ry, float rz ); +void sr_euler_mat_zxy ( SrMat& m, float rx, float ry, float rz ); +void sr_euler_mat_zyx ( SrMat& m, float rx, float ry, float rz ); + +//============================== end of file =============================== + +# endif // SR_EULER_H diff --git a/source/dcdt/se/sr_event.cpp b/source/dcdt/se/sr_event.cpp new file mode 100644 index 0000000000..43965fd186 --- /dev/null +++ b/source/dcdt/se/sr_event.cpp @@ -0,0 +1,62 @@ +#include "precompiled.h" +# include "sr_event.h" + +//# define SR_USE_TRACE1 +# include "sr_trace.h" + +//===================================== SrEvent ================================= + +SrEvent::SrEvent () + { + init (); + } + +void SrEvent::init () + { + type = None; + key = 0; + button = 0; + button1 = button2 = button3 = 0; + ctrl = shift = alt = key = 0; + width = heigth = 0; + pixel_size = 0.05f; + } + +void SrEvent::init_lmouse () + { + type = None; + key = 0; + button = 0; + button1 = button2 = button3 = 0; + ctrl = shift = alt = key = 0; + lmouse = mouse; + lmouse = mouse; + mouse.x = mouse.y = 0; + } + +const char *SrEvent::type_name () const + { + switch ( type ) + { case None : return "none"; + case Push : return "push"; + case Drag : return "drag"; + case Release : return "release"; + case Keyboard : return "keyboard"; + } + return "undefined?!"; + } + +SrOutput& operator<< ( SrOutput& out, const SrEvent& e ) + { + out << e.type_name(); + + if ( e.type==SrEvent::Keyboard ) + out << " [" << (e.key? e.key:' ') << ':' << (int)e.key << ']'; + + out << " POS:" << e.mouse << + " BUTS:" << (int)e.button1<<(int)e.button2<<(int)e.button3<< + " ACS:" << (int)e.alt<<(int)e.ctrl<<(int)e.shift; + + return out; + } + diff --git a/source/dcdt/se/sr_event.h b/source/dcdt/se/sr_event.h new file mode 100644 index 0000000000..a71f429cbe --- /dev/null +++ b/source/dcdt/se/sr_event.h @@ -0,0 +1,95 @@ + +# ifndef SR_EVENT_H +# define SR_EVENT_H + +/** \file sr_event.h + * A user-generated window-event + */ + +# include "sr_vec2.h" +# include "sr_line.h" +# include "sr_output.h" + +/*! \class SrEvent sr_event.h + \brief Keeps a window event + + SrEvent is used to describe a mouse or keyboard event, in a + system-independent way. */ +class SrEvent + { public : + /*! Enumerators for the type of event. */ + enum Type { None, //!< No event occured. + Push, //!< A mouse button was pushed. + Drag, //!< The mouse moved with a button down. + Release, //!< A mouse button was released. + Keyboard //!< A key was pressed. + }; + + /*! Enumerators with codes for special keys. */ + enum KeyCodes { KeyEsc=65307, KeyDel=65535, KeyIns=65379, KeyBack=65288, + KeyUp=65362, KeyDown=65364, KeyLeft=65361, KeyRigth=65363, + KeyEnd=65367, KeyPgDn=65366, KeyPgUp=65365, KeyHome=65360, + KeyShift=653505, KeyCtrl=65307, KeyAlt=65313, KeySpace=32, + KeyEnter=65293 }; + + public : //--- event occured : + Type type; //!< The type of the occured event + char button; //!< The button number 1, 2 or 3 if event type was Push or Release, 0 otherwise + int key; //!< The ascii code / code of the key pressed (uppercase) if it was a keyboard event, 0 otherwise + int width; //!< The width of the screen when the event occured + int heigth; //!< The heigth of the screen when the event occured +// float scenew; //!< Width in the units of the scene +// float sceneh; //!< Heigth in the units of the scene + + public : //--- states at event time : + char button1; //!< Contains 1 if the left mouse button state is pressed, 0 otherwise + char button2; //!< Contains 1 if the middle mouse button state is pressed, 0 otherwise + char button3; //!< Contains 1 if the right mouse button state is pressed, 0 otherwise + char alt; //!< Contains 1 if the Alt modifier key state is pressed, 0 otherwise + char ctrl; //!< Contains 1 if the Ctrl modifier key state is pressed, 0 otherwise + char shift; //!< Contains 1 if the Shift modifier key state is pressed, 0 otherwise + + public : //--- mouse coordinates : + SrVec2 mouse; //!< Current mouse push position in normalized coords [-1,1] + SrVec2 lmouse; //!< Last mouse x push position in normalized coords [-1,1] + + public : //--- scene information : + SrLine ray; + SrLine lray; + SrPnt mousep; //!< mouse point in scene coordinates at plane z=0 + SrPnt lmousep; //!< last mouse point in scene coordinates at plane z=0 + float pixel_size; //!< 0.05 by default but should be updated according to the camera + + public : //--- methods : + + /*! Initialize as a None event type, by calling init(). */ + SrEvent (); + + /*! Makes the event as the None type, and puts all data with their default values of zero. */ + void init (); + + /*! Puts mouse keyboard information to their default value, but saves the mouse + values in the lmouse variables. */ + void init_lmouse (); + + /*! Returns a string with the name of the event type. */ + const char *type_name () const; + + /*! Returns the difference: mousex-lmousex. */ + float mousedx () const { return mouse.x-lmouse.x; } + + /*! Returns the difference: mousey-lmousey. */ + float mousedy () const { return mouse.y-lmouse.y; } + + /*! Returns true if the event type is push, drag, or release; and false otherwise. */ + bool mouse_event () const { return type==Push||type==Drag||type==Release? true:false; } + + /*! Outputs data of this event for data inspection. */ + friend SrOutput& operator<< ( SrOutput& out, const SrEvent& e ); + }; + + +//================================ End of File ================================================= + +# endif // SR_EVENT_H + diff --git a/source/dcdt/se/sr_exp_table.cpp b/source/dcdt/se/sr_exp_table.cpp new file mode 100644 index 0000000000..5e43081f47 --- /dev/null +++ b/source/dcdt/se/sr_exp_table.cpp @@ -0,0 +1,511 @@ +#include "precompiled.h"//*************************************************************************** +// +// SrExpTABLE.H +// By Marcelo Kallmann 08/98 - Brazil +// +//*************************************************************************** + +# include +# include +# include + +# include "sr_array.h" +# include "sr_string.h" +# include "sr_exp_table.h" + +//# define SR_USE_TRACE1 // Parse +//# define SR_USE_TRACE2 // Eval +//# define SR_USE_TRACE3 // Table +# include "sr_trace.h" + +// ============================= EToken ======================================== + +typedef SrExpTable::Token EToken; + +inline void setNumb ( EToken& t, double d ) { t.type=(char)EToken::Numb; t.data.realval=d; } +inline void setVar ( EToken& t, int id ) { t.type=(char)EToken::Var; t.data.index=id; } +inline void setFunc ( EToken& t, int id ) { t.type=(char)EToken::Func; t.data.index=id; } +inline void setBinOp ( EToken& t, char op ) { t.type=(char)EToken::BinOp; t.data.code=op; } +inline void setUnaOp ( EToken& t, char op ) { t.type=(char)EToken::UnaOp; t.data.code=op; } +inline void setLeftPar ( EToken& t ) { t.type=(char)EToken::LeftPar; } +inline void setRightPar ( EToken& t ) { t.type=(char)EToken::RightPar; } + +//======================= Modes while parsing ================================= + +enum srParMode { srParModeUnary, srParModeBinary, srParModeFunction }; + +//======================== functions methods =================================== + +enum srFunc { srFuncAbs, srFuncAcos, srFuncAsin, srFuncAtan, srFuncCeil, + srFuncCos, srFuncExp, srFuncFact, srFuncFloor, srFuncLn, + srFuncLog, srFuncRound, srFuncSign, srFuncSin, srFuncSqrt, + srFuncTan, srFuncUndefined }; // Alphabetical order ! + +static char *Functions[] = { "abs", "acos", "asin", "atan", "ceil", + "cos", "exp", "fact", "floor", "ln", + "log", "round", "sign", "sin", "sqrt", + "tan" }; // Alphabetical order ! + +static int compfunc ( const void *a, const void *b ) + { + return strcmp ( ((char*)a), *((char**)b) ); + } + +static srFunc func_code ( const char *st ) + { + void *result = bsearch ( (const void*)st, // to search + (const void*)Functions, // table + (size_t)srFuncUndefined, // num elems in table + (size_t)sizeof(char*), // size of each elem in table + compfunc // compare function + ); + if (!result) return srFuncUndefined; + return (srFunc) ( ((int)result-(int)Functions)/sizeof(int) ); + } + +const char *SrExpTable::function_name ( int index ) + { + if ( index<0 || index>=(int)srFuncUndefined ) return 0; + return Functions[index]; + } + +static int oprank ( char op ) + { + switch ( op ) + { case '[' : return 8; + case '^' : case '%' : return 7; + case '/' : case '*' : case '|' : return 6; + case '+' : case '-' : return 5; + case '>' : case '<' : + case ')' : case '(' : // >= <= + case '~' : case '=' : return 4; + case 'a' : case 'o' : return 3; // and or + } + return 0; + } + +//===================================== messages ============================== +// Global functions : + +SrExpTable::Msg SrExpTable::translate_error ( SrInput::ErrorType e ) + { + switch ( e ) + { case SrInput::UndefChar : return MsgUndefChar; + case SrInput::TooBig : return MsgNameTooBig; + case SrInput::OpenString : return MsgStringNotClosed; + case SrInput::InvalidPoint : return MsgMisplacedDecPoint; + default : return MsgOk; + } + } + +const char* SrExpTable::msg_desc ( SrExpTable::Msg m ) + { + switch ( m ) + { // messages translated from sr_input.h : + case MsgUndefChar : return SrInput::error_desc ( SrInput::UndefChar ); + case MsgNameTooBig : return SrInput::error_desc ( SrInput::TooBig ); + case MsgStringNotClosed : return SrInput::error_desc ( SrInput::OpenString ); + case MsgMisplacedDecPoint : return SrInput::error_desc ( SrInput::InvalidPoint ); + // new messages : + case MsgOk : return "Ok"; + case MsgUnexpectedEndOfFile : return "Unexpected end of file encountered"; + case MsgUndefinedName : return "Undefined variable"; + case MsgUndefBinOp : return "Undefined binary operator: internal error"; + case MsgUndefUnaOp : return "Undefined unary operator: internal error"; + case MsgUndefPreDefFunc : return "Undefined pre-defined function: internal error"; + case MsgOperandExpected : return "Operand expected in expression"; + case MsgOperatorExpected : return "Operator expected in expression"; + case MsgFuncWithoutPar : return "Function name must be followed by a left parenthesis"; + case MsgDivideByZero : return "Divide by zero"; + case MsgRaisedZeroToNeg : return "Cannot raise zero to a negative power"; + case MsgRaisedNegToReal : return "Cannot raise a negative number to a non-integer power"; + case MsgUnmatchedParentesis : return "Unmatched parentesis"; + case MsgAcosArgsOutOfRange : return "Arc cos arguments out of range"; + case MsgAsinArgsOutOfRange : return "Arc sin arguments out of range"; + case MsgStackEmpty : return "Stack is empty"; + case MsgSqrtArgIsNeg : return "Sqrt argument is negative"; + default : return NULL; + } + } + +//============================ internal ====================================== + +# define ISBINARY(c) strchr("+-*/^%|><=[",c) +# define ISUNARY(c) strchr("+-!",c) + +static bool is_binary ( const SrString& token ) + { + if ( strchr("+-*/^%|><~=[",token[0]) ) return true; + if ( token=="and" || token=="or" ) return true; + return false; + } + +static SrExpTable::Msg get_translated_token ( SrInput& in, + int& curline, + EToken& etok, + srParMode mode, + const char* delimeters, + SrExpTableVarManager* varman ) + { + SrInput::TokenType toktype; + SrString token; + + toktype = in.get_token(token); + curline = in.curline(); + if ( toktype==SrInput::Error ) return SrExpTable::translate_error ( in.last_error() ); + if ( toktype==SrInput::EndOfFile ) return SrExpTable::MsgUnexpectedEndOfFile; + + if ( delimeters && toktype==SrInput::Delimiter && strchr(delimeters,token[0]) ) + { in.unget_token ( token, toktype ); + return SrExpTable::MsgUnexpectedEndOfFile; // permits to stop the parser before end of file + } + + if ( mode==srParModeUnary && ISUNARY(token[0]) ) + { setUnaOp ( etok, token[0] ); + SR_TRACE1 ( "Got a unary op: ["<' || op=='<' ) + { toktype = in.get_token(token); + if ( toktype==SrInput::Error ) return SrExpTable::translate_error ( in.last_error() ); + curline = in.curline(); + if ( op=='>' && token[0]=='=' ) op = ')'; // that will mean >= + else + if ( op=='<' && token[0]=='=' ) op = '('; // that will mean <= + else + if ( op=='<' && token[0]=='>' ) op = '~'; // <> mean difference op ~ + else + in.unget_token ( token, toktype ); + } + else if ( op=='=' ) + { toktype = in.get_token(token); + if ( toktype==SrInput::Error ) return SrExpTable::translate_error ( in.last_error() ); + if ( token[0]!='=' ) return SrExpTable::MsgWrongEqualOperator; + } + else if ( op=='[' ) + { + in.unget_token ( "(", SrInput::Delimiter ); // so that arrays indices may form an expression + } + setBinOp ( etok, op ); + SR_TRACE1 ( "Got a binary op: ["<get_variable_index(token) ); // get the user index for the variable + SR_TRACE1("Got a var ["<unref(); + delete _delimiters; + } + +void SrExpTable::set_var_manager ( SrExpTableVarManager* var_man ) + { + if ( _varman ) _varman->unref(); + _varman = var_man; + _varman->ref(); + } + +void SrExpTable::set_parser_delimiters ( const char *delimiters ) + { + sr_string_set ( _delimiters, delimiters ); + } + +SrExpTable::Msg SrExpTable::parse ( SrInput& in, int &curline ) + { + Msg msg; + Token etok, t; + srParMode mode = srParModeUnary; + + _table.size ( 0 ); + _stack.size ( 0 ); + + while ( true ) + { + msg = get_translated_token ( in, curline, etok, mode, _delimiters, _varman ); + if ( msg==MsgUnexpectedEndOfFile ) break; + if ( msg!=MsgOk ) return msg; + + if ( mode==srParModeUnary && (etok.type==Token::BinOp||etok.type==Token::RightPar) ) + { return MsgOperandExpected; } + + if ( mode==srParModeBinary && etok.type!=Token::BinOp && etok.type!=Token::RightPar ) + { return MsgOperatorExpected; } + + if ( mode==srParModeFunction && etok.type!=Token::LeftPar ) + { return MsgFuncWithoutPar; } + + switch ( etok.type ) + { case Token::Numb : + case Token::Var : _stack.push(etok); mode=srParModeBinary; break; + case Token::LeftPar : + case Token::UnaOp : _stack.push(etok); mode=srParModeUnary; break; + case Token::Func : _stack.push(etok); mode=srParModeFunction; break; + + case Token::BinOp : + while ( !_stack.empty() ) + { t=_stack.top (); + if ( t.type==Token::BinOp && oprank(etok.data.code)>oprank(t.data.code) ) break; + if ( t.type==Token::LeftPar ) break; + _stack.pop(); _table.push(t); + } + _stack.push(etok); mode=srParModeUnary; break; + + case Token::RightPar : + if ( _stack.empty() ) return MsgStackEmpty; + while ( !_stack.empty() ) + { t=_stack.pop(); + if (t.type==Token::LeftPar) break; + _table.push(t); + } + if ( t.type!=Token::LeftPar ) return MsgOk; // here the stack will be empty + if ( !_stack.empty() && _stack.top().type==Token::Func ) + _table.push( _stack.pop() ); + mode=srParModeBinary; + break; + } + } + + if ( mode==srParModeUnary) return MsgOperandExpected; + + while ( !_stack.empty() ) + { if ( _stack.top().type==Token::LeftPar ) { return MsgUnmatchedParentesis; } + _table.push( _stack.pop() ); + } + + _table.push().type=Token::EndMark; // mark the end + + # ifdef SR_USE_TRACE3 + sr_out << "ExpTable: " << *this << srnl; + # endif + + return MsgOk; + } + +static SrExpTable::Msg operateBinOp ( double x, double y, char op, + SrArray& stack, + SrExpTableVarManager* varman ) + { + double r; + switch ( op ) + { case '[' : r = varman->get_variable_value(int(x),int(y)); break; + case '+' : r = x+y; break; + case '-' : r = x-y; break; + case '/' : if (!y) return SrExpTable::MsgDivideByZero; r=x/y; break; + case '|' : if (!y) return SrExpTable::MsgDivideByZero; r=(double)(((int)x)|((int)y)); break; + case '*' : r=x*y; break; + case '%' : r = (x/100.0)*y; break; + case '^' : if ( x==0.0 && y<0.0 ) return SrExpTable::MsgRaisedZeroToNeg; + if ( x<0.0 && y!=(int)y ) return SrExpTable::MsgRaisedNegToReal; + r=pow(x,y); break; + case 'a' : r = x!=0 && y!=0? 1.0:0.0; break; + case 'o' : r = x!=0 || y!=0? 1.0:0.0; break; + case '>' : r = x>y? 1.0:0.0; break; + case '<' : r = x=y? 1.0:0.0; break; + case '(' : r = x<=y? 1.0:0.0; break; + case '~' : r = x!=y? 1.0:0.0; break; + case '=' : r = x==y? 1.0:0.0; break; + default : return SrExpTable::MsgUndefBinOp; + } + SR_TRACE2 ( "Operating: "<< x << " " << op << " " << y << " = " << r ); + setNumb ( stack.push(), r ); + return SrExpTable::MsgOk; + } + +static SrExpTable::Msg operateUnaOp ( double x, char op, SrArray& stack ) + { + double r; + switch ( op ) + { case '+' : r=x; break; + case '-' : r=-x; break; + case '!' : r = x==0? 1.0:0.0; break; + default : return SrExpTable::MsgUndefUnaOp; + } + SR_TRACE2 ( "Operating: " << op << " " << x << " = " << r ); + + setNumb ( stack.push(), r ); + return SrExpTable::MsgOk; + } + +static double dround ( double x ) { return (x>0.0)? (double)(long int)(x+0.5) : (double)(long int)(x-0.5); } +static double dsign ( double x ) { return (x<0.0)? -1: (x>0.0)? 1:0; } +static double dfact ( int x ) { if(x<2)return 1l; long m=(long)x; while(--x>1)m*=(long)x; return(double)m; } + +static SrExpTable::Msg operateFunc ( double x, srFunc f, SrArray& stack ) + { + double r; + switch ( f ) + { case srFuncAbs : r = x<0.0? -x:x; break; + case srFuncAcos : if ( x<-1.0 || x>1.0 ) return SrExpTable::MsgAcosArgsOutOfRange; + r=acos(x); break; + case srFuncAsin : if ( x<-1.0 || x>1.0 ) return SrExpTable::MsgAsinArgsOutOfRange; + r=asin(x); break; + case srFuncAtan : r=atan(x); break; + case srFuncCeil : r=ceil(x); break; + case srFuncCos : r=cos(x); break; + case srFuncExp : r=pow(SR_E,x); break; + case srFuncFact : r=dfact((int)x); break; + case srFuncFloor : r=floor(x); break; + case srFuncLn : r=log(x); break; + case srFuncLog : r=log10(x); break; + case srFuncSign : r=dsign(x); break; + case srFuncSin : r=sin(x); break; + case srFuncSqrt : if ( x<-0.0 ) return SrExpTable::MsgSqrtArgIsNeg; + r=sqrt(x); break; + case srFuncTan : r=tan(x); break; + case srFuncRound : r=dround(x); break; + default : return SrExpTable::MsgUndefPreDefFunc; + } + + SR_TRACE2 ( "Operating: " << Functions[f] << "(" << x << ") = " << r ); + + setNumb ( stack.push(), r ); + return SrExpTable::MsgOk; + } + +static SrExpTable::Msg pop_value ( double &v, SrArray& stack, + SrExpTableVarManager* varman, bool isarrayop ) + { + if ( stack.empty() ) return SrExpTable::MsgStackEmpty; + EToken t = stack.pop(); + + if ( t.type==EToken::Var ) + { v = isarrayop? (double)t.data.index : varman->get_variable_value(t.data.index,-1); + return SrExpTable::MsgOk; + } + else if ( t.type==EToken::Numb ) + { v = t.data.realval; + return SrExpTable::MsgOk; + } + else + return SrExpTable::MsgOperandExpected; + } + +SrExpTable::Msg SrExpTable::evaluate ( double &result ) + { + int i; + double u, v; + Token::Type t; + Msg msg; + + _stack.size ( 0 ); + + for ( i=0; i<_table.size(); i++ ) + { t = (Token::Type) _table[i].type; + if ( t==Token::EndMark ) break; // Found a mark for the end (only when not compressed) + + if ( t==Token::Numb || t==Token::Var ) // a value + { _stack.push ( _table[i] ); + } + else + { msg = pop_value ( v, _stack, _varman, false ); + if ( msg!=MsgOk ) return msg; + + switch ( t ) + { case Token::Func : + msg = operateFunc ( v, (srFunc)_table[i].data.index, _stack ); + break; + + case Token::UnaOp : + msg = operateUnaOp ( v, _table[i].data.code, _stack ); + break; + + case Token::BinOp : + msg = pop_value ( u, _stack, _varman, _table[i].data.code=='['?true:false ); + if ( msg!=MsgOk ) break; + msg = operateBinOp ( u, v, _table[i].data.code, _stack, _varman ); + break; + } + + if ( msg!=MsgOk ) return msg; + } + } + + if ( _stack.size()!=1 ) return MsgOperatorExpected; + + return pop_value ( result, _stack, _varman, false ); + } + +void SrExpTable::compress () + { + _table.compress (); + _stack.compress(); + } + + /*! Takes the internal data of t, and leave t as an empty table. */ +// void take_data ( SrExpTable& t ); + +// void take_data ( SrArray& tokens ); +// void leave_data ( SrArray& tokens ); + +/* +void SrExpTable::take_data ( SrExpTable& t ) + { + if ( _tokens ) free ( _tokens ); + _tokens = t._tokens; t._tokens = 0; + _size = t._size; t._size = 0; + _capacity = t._capacity; t._capacity = 0; + } +*/ + +//=============================== friends ============================ + +SrOutput& operator << ( SrOutput& o, const SrExpTable& t ) + { + EToken tk; + + for ( int i=0; i=0, array_id will contain the array index + requested for the variable. */ + virtual double get_variable_value ( int var_id, int array_id )=0; + }; + + +/*! \class SrExpTable sr_exp_table.h + \brief Expression Table Evaluation + + SrExpTable is used to parse and keep an expression table for later evaluation. + Recognized operators and their precedence are: + (1) !x (logical unary operator that negates a value 0<->1) + (2) x^y, x%y (x raised to y, x percent of y) + (3) x*y, x/y, x|y (x multiplied by y, x divided by y, x modulus y) + (4) x+y, x-y (x plus y, x minus y) + (5) x>y, x=y, x<=y, x==y, x<>y (comparison logical operators) + (6) x and y, x or y (logical operators) + Names true and false are converted to 1 and 0. + All calculations are done using double types. + Suported functions are: abs, acos, asin, atan, ceil, cos, exp, fact, floor, + ln, log, round, sign, sin, sqrt, tan */ +class SrExpTable + { public : + + enum Msg { MsgOk, MsgUndefChar, MsgNameTooBig, + MsgStringNotClosed, MsgMisplacedDecPoint, MsgUnexpectedEndOfFile, + MsgUndefinedName, MsgUndefBinOp, MsgUndefUnaOp, + MsgUndefPreDefFunc, MsgOperandExpected, MsgOperatorExpected, + MsgFuncWithoutPar, MsgDivideByZero, MsgRaisedZeroToNeg, + MsgRaisedNegToReal, MsgUnmatchedParentesis, MsgAcosArgsOutOfRange, + MsgAsinArgsOutOfRange, MsgStackEmpty, MsgSqrtArgIsNeg, + MsgWrongEqualOperator + }; + + + struct Token + { enum Type { Numb, Var, Func, BinOp, UnaOp, LeftPar, RightPar, EndMark }; + char type; + union { double realval; int index; char code; } data; + }; + + private : + + SrArray _stack; + SrArray _table; + SrExpTableVarManager* _varman; + char* _delimiters; + + public : // static methods : + + /*! This method can be used to get the name of all supported pre-defined + functions. It returns the name of a given function index, that starts + from zero. When index is out of range, null is returned. */ + static const char* function_name ( int index ); + + /*! This method returns the corresponding SrExpTable::Msg from the given + SrInput::Error, the returned Msg can be one of : MsgOk, MsgUndefChar, + MsgNameTooBig, MsgStringNotClosed, MsgMisplacedDecPoint. */ + static Msg translate_error ( SrInput::ErrorType e ); + + /*! Returns a string description of the given msg. Descriptions start with + an upper case letter, and have no ending period. */ + static const char* msg_desc ( Msg m ); + + public : + + /*! Constructor. */ + SrExpTable (); + + /*! Destructor. */ + ~SrExpTable (); + + /*! A derived class of SrExpTableVarManager is needed in order to enable + the use of variables. The passed manager must be allocated with operator + new and its deletion is controlled by SrExpTable. A null pointer can + be passed to indicate that variables are not used. */ + void set_var_manager ( SrExpTableVarManager* var_man ); + + /*! Make the parsing stop when any special delimeter is found. + The parsing stops when one of the + specified delimiters is read, and then, + after ungetting the read delimiter, MsgOk is returned when parse() is called. + If the passed string is null or "", delimiters will not be taken into account. */ + void set_parser_delimiters ( const char *delimiters ); + + /*! Parses an expression from the given input. The current line of the + parsed input is updated in curline. */ + Msg parse ( SrInput &in, int &curline ); + + /*! Evaluates the expression and put the value in result if no error occured. */ + Msg evaluate ( double &result ); + + /*! Compress the internal arrays. */ + void compress (); + + /*! Print the internal table. */ + friend SrOutput& operator << ( SrOutput& o, const SrExpTable& t ); + }; + + +# endif // SR_EXP_TABLE_H diff --git a/source/dcdt/se/sr_fl.h b/source/dcdt/se/sr_fl.h new file mode 100644 index 0000000000..91346b4c0c --- /dev/null +++ b/source/dcdt/se/sr_fl.h @@ -0,0 +1,54 @@ + +/** \file sr_fl.h + * fltk utilities integrated with sr + */ + +# ifndef SR_FL_H +# define SR_FL_H + +# include +# include +# include +# include + +/*! Fltk color chooser utility receiving a sr parameter SrColor */ +bool fl_color_chooser ( const char *title, SrColor& c ); + +/*! Returns true if ok was pressed and false otherwise. Parameter value + is displayed and returns the chosen value*/ +bool fl_value_input ( const char* title, const char* label, float& value ); + +/*! Returns true if ok was pressed and false otherwise. Parameter s + is displayed for edition and returns the entered string */ +bool fl_string_input ( const char* title, const char* label, SrString& s ); + +/*! Uses an internal instance of SrOutputWindow to display a text. + The internal instance is only allocated when used for the first time. */ +void fl_text_output ( const char* title, const char* text ); + +/*! Opens a matrix edition window, returning true if ok was pressed, + and false otherwise. If title is null, the window title is not changed. */ +bool fl_matrix_input ( const char* title, SrMat& m ); + +/*! Scan the directory dir, and put all found directories and files in + dirs and files. String array ext may contain file extensions to + filter the read files (e.g "cpp" "txt", etc). + String dir must finish with a slash '/' character. + File names are returned in their full path names */ +void fl_scan_dir ( const SrString& dir, + SrStringArray& dirs, + SrStringArray& files, + const SrStringArray& ext ); + +/*! Recursively scans and put in file all files starting at basedir. + String array ext may contain file extensions to filter the read + files; extensions are set without the "dot". + File names are returned in their full path names */ +void fl_scan_files ( const SrString& basedir, + SrStringArray& files, + const SrStringArray& ext ); + +//================================ End of File ================================================= + +# endif // SR_VIEWER_H + diff --git a/source/dcdt/se/sr_geo2.cpp b/source/dcdt/se/sr_geo2.cpp new file mode 100644 index 0000000000..8399650a85 --- /dev/null +++ b/source/dcdt/se/sr_geo2.cpp @@ -0,0 +1,262 @@ +#include "precompiled.h" +# include +# include + +# include "sr_geo2.h" + +// This file is designed to be as most as possible independent of other sr types + +//================================= Macros ========================================= + +# define gABS(x) (x>0? (x):-(x)) +# define gMAX(a,b) (a>b? (a):(b)) +# define gSWAP(a,b) { tmp=a; a=b; b=tmp; } +# define gOUTXY(a,b) printf("(%+6.4f,%+6.4f) ", a, b ) +# define gOUTL printf("\n"); +# define gEPS 1.0E-14 // doubles have 15 decimals +# define gNEXTZERO(a) ( (a)>-(gEPS) && (a)<(gEPS) ) + +// The following macro can be defined to activate some extra operations that +// try to get more precise results in the functions of this file. Normally +// these extra operations are not worth to activate +// # define gMAXPREC + +//================================= funcs ===================================== + +bool sr_segments_intersect ( double p1x, double p1y, double p2x, double p2y, + double p3x, double p3y, double p4x, double p4y ) + { + double d = (p4y-p3y)*(p1x-p2x)-(p1y-p2y)*(p4x-p3x); + if ( gNEXTZERO(d) ) return false; // they are parallel + double t = ((p4y-p3y)*(p4x-p2x)-(p4x-p3x)*(p4y-p2y)) / d; + if ( t<0.0 || t>1.0 ) return false; // outside [p1,p2] + double s = ((p4y-p2y)*(p1x-p2x)-(p1y-p2y)*(p4x-p2x)) / d; + if ( s<0.0 || s>1.0 ) return false; // outside [p3,p4] + return true; +} + +bool sr_segments_intersect ( double p1x, double p1y, double p2x, double p2y, + double p3x, double p3y, double p4x, double p4y, + double& x, double &y ) + { + double d = (p4y-p3y)*(p1x-p2x)-(p1y-p2y)*(p4x-p3x); + if ( gNEXTZERO(d) ) return false; // they are parallel + double t = ((p4y-p3y)*(p4x-p2x)-(p4x-p3x)*(p4y-p2y)) / d; + if ( t<0.0 || t>1.0 ) return false; // outside [p1,p2] + double s = ((p4y-p2y)*(p1x-p2x)-(p1y-p2y)*(p4x-p2x)) / d; + if ( s<0.0 || s>1.0 ) return false; // outside [p3,p4] + x = t*p1x+(1-t)*p2x; + y = t*p1y+(1-t)*p2y; + # ifdef gMAXPREC + x += s*p3x+(1-s)*p4x; + y += s*p3y+(1-s)*p4y; + x /= 2; + y /= 2; + # endif + return true; +} + +bool sr_lines_intersect ( double p1x, double p1y, double p2x, double p2y, + double p3x, double p3y, double p4x, double p4y ) + { + double d = (p4y-p3y)*(p1x-p2x)-(p1y-p2y)*(p4x-p3x); + if ( gNEXTZERO(d) ) return false; // they are parallel + return true; +} + +bool sr_lines_intersect ( double p1x, double p1y, double p2x, double p2y, + double p3x, double p3y, double p4x, double p4y, + double& x, double &y ) + { + double d = (p4y-p3y)*(p1x-p2x)-(p1y-p2y)*(p4x-p3x); + if ( gNEXTZERO(d) ) return false; // they are parallel + double t = ((p4y-p3y)*(p4x-p2x)-(p4x-p3x)*(p4y-p2y)) / d; + x = t*p1x+(1-t)*p2x; + y = t*p1y+(1-t)*p2y; + # ifdef gMAXPREC // 1000 random tests reveal E-10 order error between t and s results + double s = ((p4y-p2y)*(p1x-p2x)-(p1y-p2y)*(p4x-p2x)) / d; + x += s*p3x+(1-s)*p4x; + y += s*p3y+(1-s)*p4y; + x /= 2; + y /= 2; + # endif + return true; +} + +void sr_line_projection ( double p1x, double p1y, double p2x, double p2y, double px, double py, double& qx, double& qy ) + { + qx = px; + qy = py; + double vx = -(p2y-p1y); + double vy = p2x-p1x; // v = (p2-p1).ortho(), ortho==(-y,x) + sr_lines_intersect ( p1x, p1y, p2x, p2y, px, py, px+vx, py+vy, qx, qy ); + } + +bool sr_segment_projection ( double p1x, double p1y, double p2x, double p2y, double px, double py, double& qx, double& qy, double epsilon ) + { + double tmp; + sr_line_projection ( p1x, p1y, p2x, p2y, px, py, qx, qy ); + if ( epsilon==0 ) epsilon=gEPS; // if 0 the inequalities dont work with collinear inputs + +/* if ( p1x>p2x ) gSWAP(p1x,p2x); + if ( qx+epsilon<=p1x || qx-epsilon>=p2x ) return false; + if ( p1y>p2y ) gSWAP(p1y,p2y); + if ( qy+epsilon<=p1y || qy-epsilon>=p2y ) return false;*/ + + if ( p1x>p2x ) gSWAP(p1x,p2x); + if ( qx+epsilonp2x ) return false; + if ( p1y>p2y ) gSWAP(p1y,p2y); + if ( qy+epsilonp2y ) return false; + return true; + } + +double sr_dist2 ( double p1x, double p1y, double p2x, double p2y ) + { + p1x = p2x-p1x; + p1y = p2y-p1y; + return p1x*p1x + p1y*p1y; + } + +double sr_point_segment_dist ( double px, double py, double p1x, double p1y, double p2x, double p2y ) + { + double dist, qx, qy; + + if ( sr_segment_projection(p1x,p1y,p2x,p2y,px,py,qx,qy,0) ) + { dist = sr_dist2(px,py,qx,qy); + } + else + { dist = sr_dist2(px,py,p1x,p1y); + double d = sr_dist2(px,py,p2x,p2y); + if (d=0 && + SR_CCW(px,py,p2x,p2y,p3x,p3y)>=0 && + SR_CCW(px,py,p3x,p3y,p1x,p1y)>=0 ? true:false; + } + +// circle_test : +// p1, p2, p3 must be in ccw order +// Calculates the following determinant : +// | p1x p1y p1x*p1x + p1y*p1y 1.0 | +// | p2x p2y p2x*p2x + p2y*p2y 1.0 | +// | p3x p3y p3x*p3x + p3y*p3y 1.0 | +// | px py px*px + py*py 1.0 | +// This is not the most accurate calculation, but is the fastest. +bool sr_in_circle ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y, double px, double py ) + { + double p1z = p1x*p1x + p1y*p1y; + double p2z = p2x*p2x + p2y*p2y; + double p3z = p3x*p3x + p3y*p3y; + double pz = px*px + py*py; + + double m12 = p2x*p3y - p2y*p3x; + double m13 = p2x*p3z - p2z*p3x; + double m14 = p2x - p3x; + double m23 = p2y*p3z - p2z*p3y; + double m24 = p2y - p3y; + double m34 = p2z - p3z; + double d1 = p1y*m34 - p1z*m24 + m23; + double d2 = p1x*m34 - p1z*m14 + m13; + double d3 = p1x*m24 - p1y*m14 + m12; + double d4 = p1x*m23 - p1y*m13 + p1z*m12; + + double det = d4 - px*d1 + py*d2 - pz*d3; + + const double prec = gEPS; // I have encountered cases working only with this tiny epsilon + return det>prec? true:false; + } + +void sr_barycentric ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y, + double px, double py, double& u, double& v, double& w ) + { + # define DET3(a,b,c,d,e,f,g,h,i) a*e*i +b*f*g +d*h*c -c*e*g -b*d*i -a*f*h + double A = DET3 ( p1x, p2x, p3x, p1y, p2y, p3y, 1, 1, 1 ); + double A1 = DET3 ( px, p2x, p3x, py, p2y, p3y, 1, 1, 1 ); + double A2 = DET3 ( p1x, px, p3x, p1y, py, p3y, 1, 1, 1 ); + //double A3 = DET3 ( p1x, p2x, px, p1y, p2y, py, 1, 1, 1 ); + # undef DET3 + u = A1/A; + v = A2/A; + w = 1.0-u-v; // == A3/A; + } + +//=============================== Documentation ====================================== + +/* +Intersection of segments math: + +t p1 + (1-t)p2 = p (1) +s p3 + (1-s)p4 = p (2) + +=> Making (1)=(2) : + t p1 + (1-t)p2 = s p3 + (1-s)p4 + t(p1-p2) + s(p4-p3) = p4-p2 + t(p1x-p2x) + s(p4x-p3x) = p4x-p2x (3) + t(p1y-p2y) + s(p4y-p3y) = p4y-p2y (4) + +=> Putting t from (3) to (4) : + t = [(p4x-p2x) - s(p4x-p3x)] / (p1x-p2x) + [(p4x-p2x) - s(p4x-p3x)] / (p1x-p2x) = [(p4y-p2y) - s(p4y-p3y)] / (p1y-p2y) + (p1y-p2y)(p4x-p2x) - s(p1y-p2y)(p4x-p3x) = (p4y-p2y)(p1x-p2x) - s(p4y-p3y)(p1x-p2x) + s(p4y-p3y)(p1x-p2x) - s(p1y-p2y)(p4x-p3x) = (p4y-p2y)(p1x-p2x) - (p1y-p2y)(p4x-p2x) + s = [(p4y-p2y)(p1x-p2x)-(p1y-p2y)(p4x-p2x)] / [(p4y-p3y)(p1x-p2x)-(p1y-p2y)(p4x-p3x)] + Let d = (p4y-p3y)(p1x-p2x)-(p1y-p2y)(p4x-p3x) (5) + s = [(p4y-p2y)(p1x-p2x)-(p1y-p2y)(p4x-p2x)] / d (6) + +=> Putting s from (3) to (4) : + s = [(p4x-p2x) - t(p1x-p2x)] / (p4x-p3x) + [(p4x-p2x) - t(p1x-p2x)] / (p4x-p3x) = [(p4y-p2y) - t(p1y-p2y)] / (p4y-p3y) + (p4y-p3y)(p4x-p2x) - t(p4y-p3y)(p1x-p2x) = (p4x-p3x)(p4y-p2y) - t(p4x-p3x)(p1y-p2y) + t(p4x-p3x)(p1y-p2y) - t(p4y-p3y)(p1x-p2x) = (p4x-p3x)(p4y-p2y) - (p4y-p3y)(p4x-p2x) + t = [(p4x-p3x)(p4y-p2y)-(p4y-p3y)(p4x-p2x)] / [(p4x-p3x)(p1y-p2y)-(p4y-p3y)(p1x-p2x)] + t = -1*[(p4x-p3x)(p4y-p2y)-(p4y-p3y)(p4x-p2x)] / -1*[(p4x-p3x)(p1y-p2y)-(p4y-p3y)(p1x-p2x)] + t = [(p4y-p3y)(p4x-p2x)-(p4x-p3x)(p4y-p2y)] / [(p4y-p3y)(p1x-p2x)-(p4x-p3x)(p1y-p2y)] + Using (5) : + t = [(p4y-p3y)(p4x-p2x)-(p4x-p3x)(p4y-p2y)] / d (7) + +=> From (6) and (7), t and s determines p: + p = t p1 + (1-t)p2 = s p3 + (1-s)p4 */ + +//=============================== End of File ====================================== diff --git a/source/dcdt/se/sr_geo2.h b/source/dcdt/se/sr_geo2.h new file mode 100644 index 0000000000..fa850b805e --- /dev/null +++ b/source/dcdt/se/sr_geo2.h @@ -0,0 +1,90 @@ + +/** \file sr_geo2.h + * 2d geometric primitives */ + +# ifndef SR_GEO2_H +# define SR_GEO2_H + +# define SR_CCW(ax,ay,bx,by,cx,cy) ((ax*by)-(bx*ay)+(bx*cy)-(cx*by)+(cx*ay)-(ax*cy)) + +/*! Returns true if segments (p1,p2) and (p3,p4) intersect, and false otherwise. */ +bool sr_segments_intersect ( double p1x, double p1y, double p2x, double p2y, + double p3x, double p3y, double p4x, double p4y ); + +/*! Returns true if segments (p1,p2) and (p3,p4) intersect, and false otherwise. + In case of intersection, p will be the intersection point. */ +bool sr_segments_intersect ( double p1x, double p1y, double p2x, double p2y, + double p3x, double p3y, double p4x, double p4y, + double& x, double &y ); + +/*! Returns true if lines (p1,p2) and (p3,p4) intersect, and false otherwise. */ +bool sr_lines_intersect ( double p1x, double p1y, double p2x, double p2y, + double p3x, double p3y, double p4x, double p4y ); + +/*! Returns true if lines (p1,p2) and (p3,p4) intersect, and false otherwise. + In case of intersection, p will be the intersection point. */ +bool sr_lines_intersect ( double p1x, double p1y, double p2x, double p2y, + double p3x, double p3y, double p4x, double p4y, + double& x, double &y ); + +/*! Orthogonal projection of p in the line (p1,p2). The projected point becomes q. */ +void sr_line_projection ( double p1x, double p1y, double p2x, double p2y, + double px, double py, + double& qx, double& qy ); + +/*! Returns true if the orthogonal projection of p is inside (within epsilon distance) + the segment (p1,p2). In such a case, the projected point becomes q, otherwise + false is returned. */ +bool sr_segment_projection ( double p1x, double p1y, double p2x, double p2y, + double px, double py, + double& qx, double& qy, double epsilon ); + +/*! Returns the square of the distance between p1 and p2 */ +double sr_dist2 ( double p1x, double p1y, double p2x, double p2y ); + +/*! Returns the minimum distance between p and segment (p1,p2) */ +double sr_point_segment_dist ( double px, double py, + double p1x, double p1y, double p2x, double p2y ); + +/*! Returns the minimum distance between p and line (p1,p2) */ +double sr_point_line_dist ( double px, double py, double p1x, double p1y, double p2x, double p2y ); + +/*! Returns true if the distance between p1 and p2 is smaller (or equal) than epsilon */ +bool sr_next ( double p1x, double p1y, double p2x, double p2y, double epsilon ); + +/*! Returns >0 if the three points are in counter-clockwise order, <0 if + the order is clockwise and 0 if points are collinear. */ +double sr_ccw ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y ); + +/*! Returns true if p is in the segment (p1,p2), within precision epsilon, and false + otherwise. More precisely, true is returned if the projection of p in the segment (p1,p2) + has a distance to p less or equal than epsilon. */ +bool sr_in_segment ( double p1x, double p1y, double p2x, double p2y, + double px, double py, double epsilon ); + +/*! Returns true if p is in the segment (p1,p2), within precision epsilon, and false + otherwise. More precisely, true is returned if the projection of p in the segment + p1,p2) has a distance to p less or equal than epsilon. Parameter dist2 contains + the square of that distance. */ +bool sr_in_segment ( double p1x, double p1y, double p2x, double p2y, double px, double py, + double epsilon, double& dist2 ); + +/*! Returns true if p is inside (or in the border) of triangle (p1,p2,p3), otherwise false + is returned. The test is based on 3 CCW>=0 tests and no epsilons are used. */ +bool sr_in_triangle ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y, + double px, double py ); + +/*! Returns true if p is inside the circle passing at p1, p2, and p3, otherwise false + is returned. Points p1, p2 and p3 must be in ccw orientation. + This is a fast 4x4 determinant evaluation that will return true only + if p is strictly inside the circle, testing if determinant>1.0E-14 */ +bool sr_in_circle ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y, + double px, double py ); + +/*! Returns (u,v,w), w==1-u-v, u+v+w==1, such that p1*u + p2*v + p3w == p */ +void sr_barycentric ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y, + double px, double py, double& u, double& v, double& w ); + +//============================== end of file =============================== + +# endif // SR_GEO2_H diff --git a/source/dcdt/se/sr_gl.h b/source/dcdt/se/sr_gl.h new file mode 100644 index 0000000000..0331512571 --- /dev/null +++ b/source/dcdt/se/sr_gl.h @@ -0,0 +1,73 @@ + +# ifndef SR_GL_H +# define SR_GL_H + +/** \file sr_gl.h + * Sr wrapper and extensions for OpenGL + * + * Overload of most used OpenGL functions to directly work with SR types, + * and some extra utilities. + */ + +# include + +# ifdef SR_TARGET_WINDOWS // defined in sr.h +# include +# endif + +# include + +class SrVec; +class SrMat; +class SrQuat; +class SrColor; +class SrLight; +class SrImage; +class SrOutput; +class SrMaterial; + +//======================================= geometry ================================ + +void glNormal ( const SrVec &v ); + +void glVertex ( const SrVec &v ); +void glVertex ( const SrVec &v1, const SrVec &v2 ); +void glVertex ( const SrVec &v1, const SrVec &v2, const SrVec &v3 ); +void glVertex ( const SrVec &v1, const SrVec &v2, const SrVec &v3, const SrVec &v4 ); +void glVertex ( float x, float y, float z ); +void glVertex ( float x, float y, float z, float a, float b, float c ); +void glVertex ( float x, float y ); +void glDrawBox ( const SrVec& a, const SrVec& b ); //!< Send quads with normals forming the box + +//====================================== appearance ================================ + +void glClearColor ( const SrColor& c ); +void glColor ( const SrColor& c ); +void glLight ( int id, const SrLight& l ); //!< id = x E {0,...,7}, from GL_LIGHTx +void glMaterial ( const SrMaterial &m ); //!< Sets material for GL_FRONT_AND_BACK +void glMaterialFront ( const SrMaterial &m ); +void glMaterialBack ( const SrMaterial &m ); + +//==================================== matrices ============================== + +void glMultMatrix ( const SrMat &m ); +void glLoadMatrix ( const SrMat &m ); +void glTranslate ( const SrVec &v ); +void glScale ( float s ); +void glRotate ( const SrQuat &q ); +void glLookAt ( const SrVec &eye, const SrVec ¢er, const SrVec &up ); +void glPerspective ( float fovy, float aspect, float znear, float zfar ); +void glGetViewMatrix ( SrMat &m ); +void glGetProjectionMatrix ( SrMat &m ); + +//==================================== extras ============================== + +void glSnapshot ( SrImage& img ); + +//==================================== info ============================== + +void glPrintInfo ( SrOutput &o ); + +//================================ End of File ================================== + +# endif // SR_GL_H diff --git a/source/dcdt/se/sr_gl_render_funcs.h b/source/dcdt/se/sr_gl_render_funcs.h new file mode 100644 index 0000000000..99e3f56f3a --- /dev/null +++ b/source/dcdt/se/sr_gl_render_funcs.h @@ -0,0 +1,48 @@ + +# ifndef SR_GL_RENDER_FUNCS_H +# define SR_GL_RENDER_FUNCS_H + +/** \file sr_gl_render_funcs.h + * OpenGL render functions of SR shapes + */ + +class SrSnShapeBase; + +/*! All render functions used to render SR shapes are static methods + of class SrGlRenderFuncs. They are automatically registered to + the ogl render action. See SrGlRenderAction class description. */ +class SrGlRenderFuncs + { public: + static void render_model ( SrSnShapeBase* shape ); + + static void render_lines ( SrSnShapeBase* shape ); + + static void render_points ( SrSnShapeBase* shape ); + + /*! The default render mode is smooth, and flat has the same + effect of smooth shading. */ + static void render_box ( SrSnShapeBase* shape ); + + /*! The resolution value stored in SrSnShapeBase + indicates how many triangles is used to + draw the sphere. A value lower or equal to 0.2 defines + the first level (8 triangles), then levels are increased + each time 0.2 is added to the resolution. Resolution 1.0 + represents a well discretized sphere. */ + static void render_sphere ( SrSnShapeBase* shape ); + + /*! The resolution value stored in SrSnShapeBase + represents 1/10th of the number of edges discretizing the base + circle of the cylinder. For ex., resolution 1.0 results in + 10 edges, and gives 40 triangles to render the cylinder. */ + static void render_cylinder ( SrSnShapeBase* shape ); + + static void render_polygon ( SrSnShapeBase* shape ); + + static void render_polygons ( SrSnShapeBase* shape ); + }; + +//================================ End of File ================================================= + +# endif // SR_GL_RENDER_FUNCS_H + diff --git a/source/dcdt/se/sr_graph.cpp b/source/dcdt/se/sr_graph.cpp new file mode 100644 index 0000000000..1a4cee7856 --- /dev/null +++ b/source/dcdt/se/sr_graph.cpp @@ -0,0 +1,663 @@ +#include "precompiled.h" +# include +# include "sr_graph.h" +# include "sr_heap.h" + +//============================== SrGraphLink =============================================== + + + + +//============================== SrGraphNode =============================================== + +SrGraphNode::~SrGraphNode () + { + SrClassManagerBase* lman = _graph->link_class_manager(); + if ( _links.size()>0 ) + lman->free ( _links.pop() ); + } + +SrGraphLink* SrGraphNode::linkto ( SrGraphNode* n, float cost ) + { + SrGraphLink* l = (SrGraphLink*) _graph->link_class_manager()->alloc(); + l->_node = n; + l->_index = 0; + l->_cost = cost; + + _links.push () = l; + + return l; + } + +void SrGraphNode::unlink ( int ni ) + { + SrClassManagerBase* lman = _graph->link_class_manager(); + lman->free ( _links[ni] ); + _links[ni] = _links.pop(); + } + +int SrGraphNode::search_link ( SrGraphNode* n ) const + { + int i; + for ( i=0; i<_links.size(); i++ ) + if ( _links[i]->node()==n ) return i; + return -1; + } + +void SrGraphNode::output ( SrOutput& o ) const + { + float cost; + int i, max; + max = _links.size()-1; + if ( max<0 ) return; + + SrClassManagerBase* lman = _graph->link_class_manager(); + + o<<'('; + for ( i=0; i<=max; i++ ) + { // output blocked status + o << _links[i]->_blocked << srspc; + + // output index to target node + o << _links[i]->_node->_index << srspc; + + // output link cost + cost = _links[i]->_cost; + if ( cost==SR_TRUNC(cost) ) o << int(cost); + else o << cost; + + // output user data + o << srspc; + lman->output(o,_links[i]); + + if ( i nodes; + SrHeap leafs; + SrGraphBase* graph; + SrGraphNode* closest; + int iclosest; + float cdist; + float (*distfunc) ( const SrGraphNode*, const SrGraphNode*, void* udata ); + void *udata; + bool bidirectional_block; + + public : + SrGraphPathTree () + { bidirectional_block = false; + } + + void init ( SrGraphBase* g, SrGraphNode* n ) + { nodes.size(1); + nodes[0].parent = -1; + nodes[0].cost = 0; + nodes[0].node = n; + Leaf l; + l.l = l.d = 0; + leafs.insert ( l, 0 ); + graph = g; + distfunc=0; + udata = 0; + closest = 0; + iclosest = 0; + cdist = 0; + } + + bool has_leaf () + { return leafs.size()==0? false:true; + } + + bool expand_lowest_cost_leaf ( SrGraphNode* goalnode ) + { int i; + int n = leafs.top().l; + int d = leafs.top().d; + leafs.remove (); + SrGraphNode* node = nodes[n].node; + Leaf leaf; + const SrArray& a = node->links(); + for ( i=0; imarked(a[i]) || + a[i]->blocked() || + a[i]->node()->blocked() ) continue; + if ( bidirectional_block ) + { if ( a[i]->node()->link(node)->blocked() ) continue; } + nodes.push(); + nodes.top().parent = n; + nodes.top().cost = nodes[n].cost + a[i]->cost(); + nodes.top().node = a[i]->node(); + leaf.l = nodes.size()-1; + leaf.d = d+1; + leafs.insert ( leaf, nodes.top().cost ); + graph->mark ( a[i] ); + if ( distfunc ) + { float d = distfunc ( nodes.top().node, goalnode, udata ); + if ( !closest || dnode()==goalnode ) return true; + } + return false; + } + }; + +//============================== SrGraphBase =============================================== + +# define MARKFREE 0 +# define MARKING 1 +# define INDEXING 2 + +SrGraphBase::SrGraphBase ( SrClassManagerBase* nm, SrClassManagerBase* lm ) + :_nodes(nm) + { + _curmark = 1; + _mark_status = MARKFREE; + _pt = 0; // allocated only if shortest path is called + _lman = lm; + _lman->ref(); // nm is managed by the list _nodes + _leave_indices_after_save = 0; + } + +SrGraphBase::~SrGraphBase () + { + _nodes.init (); // Important: this ensures that _lman is used before _lman->unref() + delete _pt; + _lman->unref(); + } + +void SrGraphBase::init () + { + _nodes.init(); + _curmark = 1; + _mark_status = MARKFREE; + } + +void SrGraphBase::compress () + { + if ( _nodes.empty() ) return; + _nodes.gofirst(); + do { _nodes.cur()->compress(); + } while ( _nodes.notlast() ); + } + +int SrGraphBase::num_links () const + { + if ( _nodes.empty() ) return 0; + + int n=0; + + SrGraphNode* first = _nodes.first(); + SrGraphNode* cur = first; + do { n += cur->num_links(); + cur = cur->next(); + } while ( cur!=first ); + + return n; + } + +//----------------------------------- marking -------------------------------- + +void SrGraphBase::begin_marking () + { + if ( _mark_status!=MARKFREE ) sr_out.fatal_error("SrGraphBase::begin_mark() is locked!"); + _mark_status = MARKING; + if ( _curmark==sruintmax ) _normalize_mark (); + else _curmark++; + } + +void SrGraphBase::end_marking () + { + _mark_status=MARKFREE; + } + +bool SrGraphBase::marked ( SrGraphNode* n ) + { + if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::marked(n): marking is not active!\n" ); + return n->_index==_curmark? true:false; + } + +void SrGraphBase::mark ( SrGraphNode* n ) + { + if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::mark(n): marking is not active!\n" ); + n->_index = _curmark; + } + +void SrGraphBase::unmark ( SrGraphNode* n ) + { + if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::unmark(n): marking is not active!\n"); + n->_index = _curmark-1; + } + +bool SrGraphBase::marked ( SrGraphLink* l ) + { + if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::marked(l): marking is not active!\n" ); + return l->_index==_curmark? true:false; + } + +void SrGraphBase::mark ( SrGraphLink* l ) + { + if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::mark(l): marking is not active!\n" ); + l->_index = _curmark; + } + +void SrGraphBase::unmark ( SrGraphLink* l ) + { + if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::unmark(l): marking is not active!\n"); + l->_index = _curmark-1; + } + +//----------------------------------- indexing -------------------------------- + +void SrGraphBase::begin_indexing () + { + if ( _mark_status!=MARKFREE ) sr_out.fatal_error("SrGraphBase::begin_indexing() is locked!"); + _mark_status = INDEXING; + } + +void SrGraphBase::end_indexing () + { + _normalize_mark (); + _mark_status=MARKFREE; + } + +sruint SrGraphBase::index ( SrGraphNode* n ) + { + if ( _mark_status!=INDEXING ) sr_out.fatal_error ("SrGraphBase::index(n): indexing is not active!"); + return n->_index; + } + +void SrGraphBase::index ( SrGraphNode* n, sruint i ) + { + if ( _mark_status!=INDEXING ) sr_out.fatal_error ("SrGraphBase::index(n,i): indexing is not active!"); + n->_index = i; + } + +sruint SrGraphBase::index ( SrGraphLink* l ) + { + if ( _mark_status!=INDEXING ) sr_out.fatal_error ("SrGraphBase::index(l): indexing is not active!"); + return l->_index; + } + +void SrGraphBase::index ( SrGraphLink* l, sruint i ) + { + if ( _mark_status!=INDEXING ) sr_out.fatal_error ("SrGraphBase::index(l,i): indexing is not active!"); + l->_index = i; + } + +//----------------------------------- construction -------------------------------- + +SrGraphNode* SrGraphBase::insert ( SrGraphNode* n ) + { + _nodes.insert_next ( n ); + n->_graph = this; + return n; + } + +SrGraphNode* SrGraphBase::extract ( SrGraphNode* n ) + { + _nodes.cur(n); + return _nodes.extract(); + } + +void SrGraphBase::remove_node ( SrGraphNode* n ) + { + _nodes.cur(n); + _nodes.remove(); + } + +int SrGraphBase::remove_link ( SrGraphNode* n1, SrGraphNode* n2 ) + { + int i; + int n=0; + + while ( true ) + { i = n1->search_link(n2); + if ( i<0 ) break; + n++; + n1->unlink(i); + } + + while ( true ) + { i = n2->search_link(n1); + if ( i<0 ) break; + n++; + n2->unlink(i); + } + + return n; + } + +void SrGraphBase::link ( SrGraphNode* n1, SrGraphNode* n2, float c ) + { + n1->linkto(n2,c); + n2->linkto(n1,c); + } + +//----------------------------------- get edges ---------------------------------- + +void SrGraphBase::get_directed_edges ( SrArray& edges ) + { + edges.size ( num_nodes() ); + edges.size ( 0 ); + + int i; + SrGraphNode* n; + SrListIterator it(_nodes); + + for ( it.first(); it.inrange(); it.next() ) + { n = it.get(); + for ( i=0; inum_links(); i++ ) + { edges.push() = n; + edges.push() = n->link(i)->node(); + } + } + } + +void SrGraphBase::get_undirected_edges ( SrArray& edges ) + { + edges.size ( num_nodes() ); + edges.size ( 0 ); + + int i, li; + SrGraphNode* n; + SrListIterator it(_nodes); + + begin_marking(); + + for ( it.first(); it.inrange(); it.next() ) + { n = it.get(); + for ( i=0; ilinks().size(); i++ ) + { if ( !marked(n->link(i)) ) + { edges.push() = n; + edges.push() = n->link(i)->node(); + mark ( n->link(i) ); + li = edges.top()->search_link(n); + if ( li>=0 ) mark ( edges.top()->link(li) ); + } + } + } + + end_marking(); + } + +//----------------------------------- components ---------------------------------- + +static void _traverse ( SrGraphBase* graph, + SrArray& stack, + SrArray& nodes ) + { + int i; + SrGraphNode *n, *ln; + while ( stack.size()>0 ) + { n = stack.pop(); + graph->mark ( n ); + nodes.push() = n; + for ( i=0; inum_links(); i++ ) + { ln = n->link(i)->node(); + if ( !graph->marked(ln) ) stack.push()=ln; + } + } + } + +void SrGraphBase::get_connected_nodes ( SrGraphNode* source, SrArray& nodes ) + { + nodes.size ( num_nodes() ); + nodes.size ( 0 ); + + SrArray& stack = _buffer; + stack.size ( 0 ); + + begin_marking(); + stack.push() = source; + _traverse ( this, stack, nodes ); + end_marking(); + } + +void SrGraphBase::get_disconnected_components ( SrArray& components, SrArray& nodes ) + { + nodes.size ( num_nodes() ); + nodes.size ( 0 ); + + components.size ( 0 ); + + SrArray& stack = _buffer; + stack.size ( 0 ); + + SrGraphNode* n; + SrListIterator it(_nodes); + + begin_marking(); + + for ( it.first(); it.inrange(); it.next() ) + { n = it.get(); + if ( !marked(n) ) + { components.push() = nodes.size(); + stack.push() = n; + _traverse ( this, stack, nodes ); + components.push() = nodes.size()-1; + } + } + + end_marking(); + } + +//----------------------------------- shortest path ---------------------------------- + +float SrGraphBase::get_shortest_path ( SrGraphNode* n1, + SrGraphNode* n2, + SrArray& path, + float (*distfunc) ( const SrGraphNode*, const SrGraphNode*, void* udata ), + void* udata ) + { + path.size(0); + if ( n1==n2 ) { path.push()=n1; return 0; } + + begin_marking (); + + if ( !_pt ) _pt = new SrGraphPathTree; + _pt->init ( this, n2 ); + + if ( distfunc ) + { _pt->distfunc = distfunc; + _pt->udata = udata; + } + + int i; + bool end = false; + + while ( !end ) + { if ( !_pt->has_leaf() ) + { end_marking(); + if ( !distfunc ) return 0; + i = _pt->iclosest; + float cost = _pt->nodes[i].cost; + while ( i>=0 ) + { path.push () = _pt->nodes[i].node; + i = _pt->nodes[i].parent; + } + return cost; + } + + end = _pt->expand_lowest_cost_leaf ( n1 ); + } + + end_marking (); + + i = _pt->nodes.size()-1; // last element is n1 + float cost = _pt->nodes[i].cost; + while ( i>=0 ) + { path.push () = _pt->nodes[i].node; + i = _pt->nodes[i].parent; + } + + return cost; + } + +bool SrGraphBase::local_search + ( SrGraphNode* startn, + SrGraphNode* endn, + int maxdepth, + float maxdist, + int& depth, + float& dist ) + { + depth=0; + dist=0; + + if ( startn==endn ) return true; + + begin_marking (); + + if ( !_pt ) _pt = new SrGraphPathTree; + _pt->init ( this, startn ); + + int i; + bool not_found = false; + bool end = false; + + while ( !end ) + { if ( !_pt->has_leaf() ) { not_found=true; break; } // not found! + + dist = _pt->nodes[_pt->leafs.top().l].cost; + depth = _pt->leafs.top().d; + + if ( maxdepth>0 && depth>maxdepth ) { i=-1; break; } // max depth reached + if ( maxdist>0 && dist>maxdist ) { i=-1; break; } // max dist reached + + end = _pt->expand_lowest_cost_leaf ( endn ); + } + + end_marking (); + + if ( not_found ) return false; // not found! + return true; + } + +void SrGraphBase::bidirectional_block_test ( bool b ) + { + if ( !_pt ) _pt = new SrGraphPathTree; + _pt->bidirectional_block = b; + } + +//------------------------------------- I/O -------------------------------- + +void SrGraphBase::output ( SrOutput& o ) + { + SrListIterator it(_nodes); + SrClassManagerBase* nman = node_class_manager(); + + // set indices + if ( _mark_status!=MARKFREE ) sr_out.fatal_error("SrGraphBase::operator<<(): begin_indexing() is locked!"); + sruint i=0; + begin_indexing(); + for ( it.first(); it.inrange(); it.next() ) index(it.get(),i++); + + // print + o<<'['; + for ( it.first(); it.inrange(); it.next() ) + { o << it->index() << srspc; // output node index + o << it->_blocked << srspc; // output node blocked status + nman->output ( o, it.get() ); // output user data + if ( it.get()->num_links()>0 ) // output node links (blocked, ids, cost, and udata) + { o<SrGraphNode::output ( o ); + } + if ( !it.inlast() ) o << srnl; + } + o<<']'; + + if ( !_leave_indices_after_save ) end_indexing(); + _leave_indices_after_save = 0; + } + +static void set_blocked ( int& blocked, const char* last_token ) + { + if ( last_token[0]=='b' ) // back-compatibility + blocked = 1; + else if ( last_token[0]=='f' ) // back-compatibility + blocked = 0; + else // should be an integer + blocked = atoi(last_token); + } + +void SrGraphBase::input ( SrInput& inp ) + { + SrArray& nodes = _buffer; + nodes.size(128); + nodes.size(0); + + SrClassManagerBase* nman = node_class_manager(); + SrClassManagerBase* lman = link_class_manager(); + + init (); + + inp.getd(); // [ + inp.get_token(); // get node counter (which is not needed) + + while ( inp.last_token()[0]!=']' ) + { + nodes.push() = _nodes.insert_next(); // allocate one node + nodes.top()->_graph = this; + + inp.get_token(); // get node blocked status + set_blocked ( nodes.top()->_blocked, inp.last_token() ); + + nman->input ( inp, nodes.top() ); // read node user data + + inp.get_token(); // new node counter, or '(', or ']' + + if ( inp.last_token()[0]=='(') + while ( true ) + { inp.get_token(); + if ( inp.last_token()[0]==')' ) { inp.get_token(); break; } + SrArray& la = nodes.top()->_links; + la.push() = (SrGraphLink*) lman->alloc(); + set_blocked ( la.top()->_blocked, inp.last_token() ); // get link blocked status + + inp.get_token(); // get id + la.top()->_index = atoi(inp.last_token()); // store id + inp.getn(); // get cost + la.top()->_cost = (float) atof(inp.last_token()); // store cost + lman->input ( inp, la.top() ); + } + } + + // now convert indices to pointers: + int i, j; + for ( i=0; i& la = nodes[i]->_links; + for ( j=0; j_node = nodes[ int(la[j]->_index) ]; + } + } + } + +//---------------------------- private methods -------------------------------- + +// set all indices (nodes and links) to 0 and curmark to 1 +void SrGraphBase::_normalize_mark() + { + int i; + SrGraphNode* n; + + if ( _nodes.empty() ) return; + + _nodes.gofirst(); + do { n = _nodes.cur(); + n->_index = 0; + for ( i=0; inum_links(); i++ ) n->link(i)->_index=0; + } while ( _nodes.notlast() ); + _curmark = 1; + } + +//============================== end of file =============================== + diff --git a/source/dcdt/se/sr_graph.h b/source/dcdt/se/sr_graph.h new file mode 100644 index 0000000000..ab83047763 --- /dev/null +++ b/source/dcdt/se/sr_graph.h @@ -0,0 +1,320 @@ + +# ifndef SR_GRAPH_H +# define SR_GRAPH_H + +/** \file sr_graph.h + * Graph maintenance classes + */ + +# include "sr_array.h" +# include "sr_list.h" + +class SrGraphNode; +class SrGraphBase; + +/*! SrGraphLink contains the minimal needed information for + a link. To attach user-related information, SrGraphLink + must be derived and as well a corresponding + SrClassManagerBased, which will manage only the user data. */ +class SrGraphLink + { private : + SrGraphNode* _node; // node pointing to + sruint _index; // index for traversing + float _cost; // cost for minimum paths + int _blocked; // used as boolean or as a ref counter + friend class SrGraphNode; + friend class SrGraphBase; + protected : + SrGraphLink () { _index=0; _cost=0; _node=0; _blocked=0; } + ~SrGraphLink () {} + public : + void cost ( float c ) { _cost=c; } + float cost () const { return _cost; } + SrGraphNode* node () const { return _node; } + sruint index () const { return _index; } + int blocked () const { return _blocked; } + void blocked ( bool b ) { _blocked = b? 1:0; } + void blocked ( int b ) { _blocked = b; } + }; + +/*! SrGraphNode contains the minimal needed information for + a node. To attach user-related information, SrGraphLink + must be derived and as well a corresponding + SrClassManagerBased, which will manage only the user data. */ +class SrGraphNode : public SrListNode + { private : + SrArray _links; + sruint _index; + int _blocked; // used as boolean or as a ref counter + SrGraphBase* _graph; + friend class SrGraphBase; + protected : + SrGraphNode () { _index=0; _graph=0; _blocked=0; } + ~SrGraphNode (); + + public : + + /*! Returns the next node in the global list of nodes */ + SrGraphNode* next() const { return (SrGraphNode*) SrListNode::next(); } + + /*! Returns the prior node in the global list of nodes */ + SrGraphNode* prior() const { return (SrGraphNode*) SrListNode::prior(); } + + /*! Returns the index of this node */ + sruint index () { return _index; } + + int blocked () const { return _blocked; } + void blocked ( bool b ) { _blocked = b? 1:0; } + void blocked ( int b ) { _blocked = b; } + + /*! Compress internal links array */ + void compress () { _links.compress(); } + + /*! Returns the new link created */ + SrGraphLink* linkto ( SrGraphNode* n, float cost=0 ); + + /*! Remove link of index ni, with a fast remove: the order in the + links array is not mantained */ + void unlink ( int ni ); + + /*! Remove the link pointing to n. Should be called ONLY when it is + guaranteed that the link to n really exists! Uses method unlink(ni) */ + void unlink ( SrGraphNode* n ) { unlink ( search_link(n) ); } + + /*! Returns the index in the links array, or -1 if not linked to n */ + int search_link ( SrGraphNode* n ) const; + + /*! Returns the link pointing to n. Should be called ONLY when it is + guaranteed that the link to n really exists! */ + SrGraphLink* link ( SrGraphNode*n ) { return _links[search_link(n)]; } + + /*! Returns the link index i, which must be a valid index */ + SrGraphLink* link ( int i ) { return _links[i]; } + + /*! Returns the last link */ + SrGraphLink* last_link () { return _links.top(); } + + /*! Returns the number of links in this node */ + int num_links () const { return _links.size(); } + + /*! Returns the links array */ + const SrArray& links() const { return _links; } + + private : + void output ( SrOutput& o ) const; + }; + +/*! The following define is to be used inside a user derived class + of SrGraphLink, in order to easily redefine public SrGraphLink + methods with correct type casts. */ +# define SR_GRAPH_LINK_CASTED_METHODS(N,L) \ +N* node() const { return (N*)SrGraphLink::node(); } + +/*! The following define is to be used inside a user derived class + of SrGraphNode, in order to easily redefine public SrGraphNode + methods with correct type casts. */ +# define SR_GRAPH_NODE_CASTED_METHODS(N,L) \ +N* next() const { return (N*)SrListNode::next(); } \ +N* prior() const { return (N*)SrListNode::prior(); } \ +L* linkto ( N* n, float c ) { return (L*)SrGraphNode::linkto(n,c); } \ +L* link ( N* n ) { return (L*)SrGraphNode::link(n); } \ +L* link ( int i ) { return (L*)SrGraphNode::link(i); } \ +const SrArray& links() const { return (const SrArray&) SrGraphNode::links(); } + +class SrGraphPathTree; + +/*! SrGraphBase maintains a directed graph with nodes and links. Links around + a node do not have any order meaning. + Note also that the user should avoid redundant links, as no tests are done + (for speed purposes) in several methods. + SrGraphBase is the base class, the user should use SrGraph template instead. */ +class SrGraphBase + { private : + SrList _nodes; + SrArray _buffer; + sruint _curmark; + char _mark_status; + SrGraphPathTree* _pt; + SrClassManagerBase* _lman; // link manager for a class deriving SrGraphLink + char _leave_indices_after_save; + + public : + /*! Constructor requires managers for nodes and links */ + SrGraphBase ( SrClassManagerBase* nm, SrClassManagerBase* lm ); + + /*! Destructor deletes all data in the graph */ + ~SrGraphBase (); + + /*! Returns a pointer to the node manager */ + SrClassManagerBase* node_class_manager() const { return _nodes.class_manager(); } + + /*! Returns a pointer to the link manager */ + SrClassManagerBase* link_class_manager() const { return _lman; } + + /*! Make the graph empty */ + void init (); + + /*! Compress all link arrays in the nodes */ + void compress (); + + /*! Returns the number of nodes in the graph */ + int num_nodes () const { return _nodes.elements(); } + + /*! Counts and returns the number of (directional) links in the graph */ + int num_links () const; + + /*! Methods for marking nodes and links */ + void begin_marking (); + void end_marking (); + bool marked ( SrGraphNode* n ); + void mark ( SrGraphNode* n ); + void unmark ( SrGraphNode* n ); + bool marked ( SrGraphLink* l ); + void mark ( SrGraphLink* l ); + void unmark ( SrGraphLink* l ); + + /*! Methods for indexing nodes and links */ + void begin_indexing (); + void end_indexing (); + sruint index ( SrGraphNode* n ); + void index ( SrGraphNode* n, sruint i ); + sruint index ( SrGraphLink* l ); + void index ( SrGraphLink* l, sruint i ); + + /*! Returns the list of nodes, that should be used with + care to not invalidate some SrGraph operations. */ + SrList& nodes () { return _nodes; } + + /*! Inserts node n in the list of nodes, as a new + unconnected component. n is returned. */ + SrGraphNode* insert ( SrGraphNode* n ); + + /*! Extract (without deleting) the node from the graph. Nothing + is done concerning possible links between the graph and + the node being extracted */ + SrGraphNode* extract ( SrGraphNode* n ); + + /*! Removes and deletes the node from the graph. Its the user + responsibility to ensure that the graph has no links with + the node being deleted */ + void remove_node ( SrGraphNode* n ); + + /*! Searches and removes the edge(s) linking n1 with n2 if any. + Links are removed with a "fast remove process" so that indices + in the links array of the involved nodes may be modified. + Return the number of (directed) links removed. */ + int remove_link ( SrGraphNode* n1, SrGraphNode* n2 ); + + /*! Links n1 to n2 and n2 to n1, with cost c in both directions */ + void link ( SrGraphNode* n1, SrGraphNode* n2, float c=0 ); + + /*! Returns the first node of the list of nodes */ + SrGraphNode* first_node () const { return _nodes.first(); } + + /*! Get all directed edges of the graph. Note that if n1 is linked + to n2 and n2 is linked to n1, both edges (n1,n2) and (n2,n1) will + appear in the edges array */ + void get_directed_edges ( SrArray& edges ); + + /*! Get all edges of the graph without duplications from the directional + point of view. */ + void get_undirected_edges ( SrArray& edges ); + + /*! Get all nodes which are in the same connected component of source */ + void get_connected_nodes ( SrGraphNode* source, SrArray& nodes ); + + /*! Organize nodes by connected components. The indices in array components say each + start and end position in array nodes for each component */ + void get_disconnected_components ( SrArray& components, SrArray& nodes ); + + /*! The returned path contains pointers to existing nodes in the graph. + In the case the two nodes are in two different disconnected components + an empty path is returned. If n1==n2 a path with the single node n1 + is returned. In all cases, returns the distance (cost) of path. + In case no path is found, the optional parameters distfunc and udata + can be used to return the path to the closest processed node to the goal. */ + float get_shortest_path ( SrGraphNode* n1, SrGraphNode* n2, SrArray& path, + float (*distfunc) ( const SrGraphNode*, const SrGraphNode*, void* udata )=0, + void* udata=0 ); + + /*! Performs a A* search from startn, until finding endn. The search is + stopped if either maxnodes or maxdist is reached. If these parameters + are <0 they are not taken into account. True is returned if endn could + be reached, and parameters depth and dist will contain the final + higher depth and distance (along the shortest path) reached. */ + bool local_search ( SrGraphNode* startn, SrGraphNode* endn, + int maxnodes, float maxdepth, int& depth, float& dist ); + + /*! When set to true, graph search will consider a link blocked if any of its + directions is blocked, i.e., blocking only one direction of a link will + block both the directions. Afftected methods are get_shortest_path() and + local_search(). Default is false. */ + void bidirectional_block_test ( bool b ); + + bool are_near_in_graph ( SrGraphNode* startn, SrGraphNode* endn, int maxnodes ); + + /*! If this is set to true, it will be the user responsibility to call + end_indexing() after the next graph save. It is used to retrieve the indices + used during saving to reference additional data to be saved in derived classes. */ + void leave_indices_after_save ( bool b ) { _leave_indices_after_save = b? 1:0; } + + /*! Returns the internal buffer, which might be usefull for some specific needs; + see for instance the description of the input() method. */ + SrArray& buffer () { return _buffer; } + + /*! Outputs the graph in the format: [ (l1..lk)e1 (..)e2 (..)en ]. + Nodes are indexed (starting from 0), and after the output + all indices of the nodes are set to 0. */ + void output ( SrOutput& o ); + + /*! Initializes the current graph and load another one from the given input. + Method buffer() can be used to retrieve an array with all nodes loaded, + indexed by the indices used in the loaded file */ + void input ( SrInput& i ); + + friend SrOutput& operator<< ( SrOutput& o, SrGraphBase& g ) { g.output(o); return o; } + + friend SrInput& operator>> ( SrInput& i, SrGraphBase& g ) { g.input(i); return i; } + + private : + void _normalize_mark(); + + }; + +/*! SrGraph is a template that includes type casts for the user + derived types (of SrGraphNode and SrGraphLink) to correctly + call SrGraphBase methods. */ +template +class SrGraph : public SrGraphBase + { public : + SrGraph () : SrGraphBase ( new SrClassManager, new SrClassManager ) {} + + SrGraph ( SrClassManagerBase* nm, SrClassManagerBase* lm ) : SrGraphBase ( nm, lm ) {} + + const SrList& nodes () { return (SrList&) SrGraphBase::nodes(); } + + N* insert ( N* n ) { return (N*) SrGraphBase::insert((SrGraphNode*)n); } + N* extract ( N* n ) { return (N*) SrGraphBase::extract((SrGraphNode*)n); } + N* first_node () const { return (N*) SrGraphBase::first_node(); } + + void get_undirected_edges ( SrArray& edges ) + { SrGraphBase::get_undirected_edges( (SrArray&)edges ); } + + void get_disconnected_components ( SrArray& components, SrArray& nodes ) + { SrGraphBase::get_disconnected_components( components, (SrArray&)nodes ); } + + float get_shortest_path ( N* n1, N* n2, SrArray& path, + float (*distfunc) ( const SrGraphNode*, const SrGraphNode*, void* udata )=0, + void* udata=0 ) + { return SrGraphBase::get_shortest_path((SrGraphNode*)n1,(SrGraphNode*)n2,(SrArray&)path,distfunc,udata); } + + SrArray& buffer () { return (SrArray&) SrGraphBase::buffer(); } + + friend SrOutput& operator<< ( SrOutput& o, SrGraph& g ) { return o<<(SrGraphBase&)g; } + friend SrInput& operator>> ( SrInput& i, SrGraph& g ) { return i>>(SrGraphBase&)g; } + }; + +//================================ End of File ================================================= + +# endif // SR_GRAPH_H + diff --git a/source/dcdt/se/sr_grid.cpp b/source/dcdt/se/sr_grid.cpp new file mode 100644 index 0000000000..d0675be62a --- /dev/null +++ b/source/dcdt/se/sr_grid.cpp @@ -0,0 +1,249 @@ +#include "precompiled.h" +# include "sr_grid.h" + +//============================ SrGridBase ================================= + +SrGridBase::SrGridBase ( int dim, int ns ) + { + _cells = 0; + if ( dim*ns>0 ) init ( dim, ns ); + } + +void SrGridBase::init ( int dim, int ns ) + { + int i; + _axis.size ( dim ); + + for ( i=0; i& axis_desc ) + { + if ( &axis_desc != &_axis ) _axis=axis_desc; + + if ( _axis.size()==0 ) { _cells=0; _size.size(0); _seglen.size(0); return; } + + int i, j; + + _cells = _axis[0].segs; + int dim = _axis.size(); + + for ( i=1; i& coords ) const + { + int dim = _axis.size(); + + if ( dim!=coords.size() || dim<=0 ) return 0; + + int i, cell = coords[0]; + for ( i=1; i& coords ) const + { + int d = dimensions(); + + if ( coords.size()!=d ) coords.size(d); + + d--; + while ( d>0 ) + { coords[d] = index/_size[d]; + index = index%_size[d]; + d--; + } + coords[0] = index; + } + +void SrGridBase::cell_boundary ( int i, int j, SrPnt2& a, SrPnt2& b ) const + { + if ( dimensions()!=2 ) return; + + a.x = _axis[0].min + float(i)*_seglen[0]; + a.y = _axis[1].min + float(j)*_seglen[1]; + + b.x = a.x + _seglen[0]; + b.y = a.y + _seglen[1]; + } + +void SrGridBase::cell_boundary ( int i, int j, int k, SrPnt& a, SrPnt& b ) const + { + if ( dimensions()!=3 ) return; + + a.x = _axis[0].min + float(i)*_seglen[0]; + a.y = _axis[1].min + float(j)*_seglen[1]; + a.z = _axis[2].min + float(k)*_seglen[2]; + + b.x = a.x + _seglen[0]; + b.y = a.y + _seglen[1]; + b.z = a.z + _seglen[2]; + } + +void SrGridBase::get_intersection ( SrPnt2 a, SrPnt2 b, SrArray& cells ) const + { + if ( dimensions()!=2 ) return; + + if ( a.x>b.x || a.y>b.y ) return; + + if ( a.x>=_axis[0].max || a.y>=_axis[1].max ) return; + if ( b.x<=_axis[0].min || b.y<=_axis[1].min ) return; + + float f = _seglen[0]/2; + a.x = SR_BOUND ( a.x, _axis[0].min, _axis[0].max-f ); + b.x = SR_BOUND ( b.x, _axis[0].min, _axis[0].max-f ); + f = _seglen[1]/2; + a.y = SR_BOUND ( a.y, _axis[1].min, _axis[1].max-f ); + b.y = SR_BOUND ( b.y, _axis[1].min, _axis[1].max-f ); + + int i1 = (int) ((a.x-_axis[0].min) / _seglen[0]); + int j1 = (int) ((a.y-_axis[1].min) / _seglen[1]); + + f = (b.x-_axis[0].min) / _seglen[0]; + int i2 = int(f); if ( float(i2)==f ) i2--; + + f = (b.y-_axis[1].min) / _seglen[1]; + int j2 = int(f); if ( float(j2)==f ) j2--; + + int i, j, index; + + for ( j=j1; j<=j2; j++ ) + { index = _size[1]*j + i1; // == cell_index(i1,j); + for ( i=i1; i<=i2; i++ ) + { cells.push() = index; + index++; + } + } + } + +void SrGridBase::get_intersection ( SrPnt a, SrPnt b, SrArray& cells ) const + { + if ( dimensions()!=3 ) return; + + if ( a.x>b.x || a.y>b.y || a.z>b.z ) return; + + if ( a.x>=_axis[0].max || a.y>=_axis[1].max || a.z>=_axis[2].max ) return; + if ( b.x<=_axis[0].min || b.y<=_axis[1].min || b.z<=_axis[2].min ) return; + + float f = _seglen[0]/2; + a.x = SR_BOUND ( a.x, _axis[0].min, _axis[0].max-f ); + b.x = SR_BOUND ( b.x, _axis[0].min, _axis[0].max-f ); + f = _seglen[1]/2; + a.y = SR_BOUND ( a.y, _axis[1].min, _axis[1].max-f ); + b.y = SR_BOUND ( b.y, _axis[1].min, _axis[1].max-f ); + f = _seglen[2]/2; + a.z = SR_BOUND ( a.z, _axis[2].min, _axis[2].max-f ); + b.z = SR_BOUND ( b.z, _axis[2].min, _axis[2].max-f ); + + int i1 = (int) ((a.x-_axis[0].min) / _seglen[0]); + int j1 = (int) ((a.y-_axis[1].min) / _seglen[1]); + int k1 = (int) ((a.z-_axis[2].min) / _seglen[2]); + + f = (b.x-_axis[0].min) / _seglen[0]; + int i2 = int(f); if ( float(i2)==f ) i2--; + + f = (b.y-_axis[1].min) / _seglen[1]; + int j2 = int(f); if ( float(j2)==f ) j2--; + + f = (b.z-_axis[2].min) / _seglen[2]; + int k2 = int(f); if ( float(k2)==f ) k2--; + + int i, j, k, index; + + for ( k=k1; k<=k2; k++ ) + for ( j=j1; j<=j2; j++ ) + { index = _size[2]*k + _size[1]*j + i1; // == cell_index(i1,j,k); + for ( i=i1; i<=i2; i++ ) + { cells.push() = index; + index++; + } + } + } + +int SrGridBase::get_point_location ( SrPnt2 a ) const + { + if ( dimensions()!=2 ) return -1; + + if ( a.x>=_axis[0].max || a.y>=_axis[1].max ) return -1; + if ( a.x<=_axis[0].min || a.y<=_axis[1].min ) return -1; + + a.x = SR_BOUND ( a.x, _axis[0].min, _axis[0].max-(_seglen[0]/2) ); + a.y = SR_BOUND ( a.y, _axis[1].min, _axis[1].max-(_seglen[1]/2) ); + + int i = (int) ((a.x-_axis[0].min) / _seglen[0]); + int j = (int) ((a.y-_axis[1].min) / _seglen[1]); + + return _size[1]*j + i; + } + +int SrGridBase::get_point_location ( SrPnt a ) const + { + if ( dimensions()!=3 ) return -1; + + if ( a.x>=_axis[0].max || a.y>=_axis[1].max || a.z>=_axis[2].max ) return -1; + if ( a.x<=_axis[0].min || a.y<=_axis[1].min || a.z<=_axis[2].min ) return -1; + + a.x = SR_BOUND ( a.x, _axis[0].min, _axis[0].max-(_seglen[0]/2) ); + a.y = SR_BOUND ( a.y, _axis[1].min, _axis[1].max-(_seglen[1]/2) ); + a.z = SR_BOUND ( a.z, _axis[2].min, _axis[2].max-(_seglen[2]/2) ); + + int i = (int) ((a.x-_axis[0].min) / _seglen[0]); + int j = (int) ((a.y-_axis[1].min) / _seglen[1]); + int k = (int) ((a.z-_axis[2].min) / _seglen[2]); + + return _size[2]*k + _size[1]*j + i; + } + +//============================== end of file =============================== + diff --git a/source/dcdt/se/sr_grid.h b/source/dcdt/se/sr_grid.h new file mode 100644 index 0000000000..5a6c9d6e1f --- /dev/null +++ b/source/dcdt/se/sr_grid.h @@ -0,0 +1,167 @@ + +# ifndef SR_GRID_H +# define SR_GRID_H + +/** \file sr_grid.h + * Manages data in a n-D regular grid */ + +# include "sr_vec.h" +# include "sr_vec2.h" +# include "sr_array.h" +# include "sr_output.h" +# include "sr_class_manager.h" + +/*! Describes how to subdivide one dimensional axis. + This class is required to set up the decomposition in SrGrid */ +class SrGridAxis + { public : + int segs; // number of segments to divide this axis + float min; // minimum coordinate + float max; // maximum coordinate + + public : + SrGridAxis ( int se, float mi, float ma ) + { segs=se; min=mi; max=ma; } + + void set ( int se, float mi, float ma ) + { segs=se; min=mi; max=ma; } + + friend SrOutput& operator<< ( SrOutput& o, const SrGridAxis& a ) + { return o << a.segs << srspc << a.min << srspc << a.max; } + + friend SrInput& operator>> ( SrInput& i, SrGridAxis& a ) + { return i >> a.segs >> a.min >> a.max; } + }; + +/*! \class SrGridBase sr_grid.h + \brief n-D regular grid base class + + SrGridBase has methods to translate n dimensional grid coordinates + into a one dimension bucket array coordinate. Use the derived SrGrid + template class to associate user data to the grid. */ +class SrGridBase + { private : + SrArray _axis; + SrArray _size; // subspaces sizes for fast bucket determination + SrArray _seglen; // axis segments lengths for fast cell determination + int _cells; + + public : + + /*! Constructs a grid of given dimension (dim) and number of + segments in each axis (ns). The number of cells will then be + ns^dim. Axis are normalized in [0,1] */ + SrGridBase ( int dim=0, int ns=0 ); + + /*! Init the grid with given dimension and number of + segments in each axis. The number of cells will then be + ns^dim. Axis are normalized in [0,1] */ + void init ( int dim, int ns ); + + /*! Destroy the existing grid and initializes one according to + the descriptions sent in the desc array, which size will + define the dimension of the grid and thus the dimension + of the index tuple to specify a grid */ + void init ( const SrArray& axis_desc ); + + const SrArray& axis_desc() const { return _axis; } + + float max_coord ( int axis ) const { return _axis[axis].max; } + float min_coord ( int axis ) const { return _axis[axis].min; } + int segments ( int axis ) const { return _axis[axis].segs; } + float seglen ( int axis ) const { return _seglen[axis]; } + + /*! Returns the number of dimensions of the grid */ + int dimensions () const { return _axis.size(); } + + /*! Returns the total number of cells in the grid */ + int cells () const { return _cells; } + + /*! Get the index of a cell from its coordinates. 2D version. */ + int cell_index ( int i, int j ) const; + + /*! Get the index of a cell from its coordinates. 3D version */ + int cell_index ( int i, int j, int k ) const; + + /*! Get the index of a cell from its coordinates. Multidimensional version */ + int cell_index ( const SrArray& coords ) const; + + /*! Get the cell coordinates from its index. 2D version. */ + void cell_coords ( int index, int& i, int& j ) const; + + /*! Get the cell coordinates from its index. 3D version. */ + void cell_coords ( int index, int& i, int& j, int& k ) const; + + /*! Get the cell coordinates from its index. Multidimensional version. */ + void cell_coords ( int index, SrArray& coords ) const; + + /*! Get the lower and upper coordinates of the Euclidian 2D box of cell (i,j). */ + void cell_boundary ( int i, int j, SrPnt2& a, SrPnt2& b ) const; + + /*! Get the lower and upper coordinates of the Euclidian 3D box of cell (i,j,k). */ + void cell_boundary ( int i, int j, int k, SrPnt& a, SrPnt& b ) const; + + /*! Get the indices of all cells intersecting with the given box. + If a box boundary is exactly at a grid separator, only the cell + with greater index is considered. Indices are just pushed to + array cells, which is not emptied before being used */ + void get_intersection ( SrPnt2 a, SrPnt2 b, SrArray& cells ) const; + + /*! 3D version of get_intersection() */ + void get_intersection ( SrPnt a, SrPnt b, SrArray& cells ) const; + + /*! Returns the index of the cell containing a, or -1 if a is outside the grid */ + int get_point_location ( SrPnt2 a ) const; + + /*! Returns the index of the cell containing a, or -1 if a is outside the grid */ + int get_point_location ( SrPnt a ) const; + }; + +/*! \class SrGrid sr_grid.h + \brief n-D regular grid template class + + SrGrid defines automatic type casts to the user type. + WARNING: SrGrid is designed to efficiently store large + amount of cells and it uses SrArray, which implies + that no constructors or destructors of type X are called. + The user must initialize and delete data properly. */ +template +class SrGrid : public SrGridBase + { private : + SrArray _data; + + public : + SrGrid ( int dim=0, int ns=0 ) : SrGridBase ( dim, ns ) + { _data.size(SrGridBase::cells()); } + + void init ( int dim, int ns ) + { SrGridBase::init ( dim, ns ); + _data.size(SrGridBase::cells()); + } + + void init ( const SrArray& axis_desc ) + { SrGridBase::init ( axis_desc ); + _data.size(SrGridBase::cells()); + } + + /*! Returns the cell of given index, that should be in 0<=i& coords ) + { return _data[SrGridBase::cell_index(i,j)]; } + }; + +//============================== end of file =============================== + +# endif // SR_LIST_H + + diff --git a/source/dcdt/se/sr_hash_table.cpp b/source/dcdt/se/sr_hash_table.cpp new file mode 100644 index 0000000000..b6e350af60 --- /dev/null +++ b/source/dcdt/se/sr_hash_table.cpp @@ -0,0 +1,242 @@ +#include "precompiled.h" +# include "sr_hash_table.h" + +//================================ hash function ============================= + +static int hash ( const char* s, int size ) + { + int h = 0; + while ( *s ) + { h = 31*h + SR_LOWER(*s); + s++; + } + h = SR_ABS(h); + h = h%size; + return h; + } + +/*static int hash ( const char *string, int limit ) // worse + { + unsigned int i = 0; + unsigned int accum = 0; + + while( string[ i ] != 0 ) { + accum = string[ i ] + ^ ( accum << ( sizeof( char ) * 8 + 1 ) ) + ^ ( accum >> ( ( sizeof( accum ) - sizeof( char ) ) * 8 - 1 ) ); + i++; + } + + return (accum % limit); + }*/ + +//================================ SrHashTableBase =============================== + +SrHashTableBase::SrHashTableBase ( int hsize ) + { + init ( hsize ); + } + +SrHashTableBase::~SrHashTableBase () + { + init ( 0 ); + } + +void SrHashTableBase::init ( int hsize ) + { + if ( hsize==0 && _hash_size==0 ) return; // already initialized to 0 + + // destroys actual table: + while ( _table.size() ) + { if ( _table.top().dynamic ) sr_string_set ( _table.top().st, 0 ); // delete + _table.pop(); + } + + // builds new table: + _free.size ( 0 ); + _free.capacity ( 0 ); + _last_id = -1; + _elements = 0; + _hash_size = hsize; + _table.capacity ( hsize ); + _table.size ( hsize ); + int i; + for ( i=0; i a; + a.capacity ( _elements ); + a.size ( 0 ); + + // 1. store current valid elements: + int i; + for ( i=0; i<_table.size(); i++ ) + { if ( _table[i].st ) + { a.push() = _table[i]; + _table[i].st=0; // ensures st pointer will not be deleted + } + } + + // 2. init table with new hash size: + init ( new_hsize ); + + // 3. put back original data in new table: + for ( i=0; i=0 ) + { len++; + j = _table[j].next; + } + + if ( len>longest ) longest=len; + + } + + return longest; + } + +int SrHashTableBase::lookup_index ( const char *st ) const + { + if ( !st ) return -1; + + int id = ::hash ( st, _hash_size ); + + if ( _table[id].st==0 ) return -1; // empty entry, not found + + while ( true ) + { if ( sr_compare(_table[id].st,st)==0 ) return id; // already there + + // else check next colliding entry: + if ( _table[id].next<0 ) return -1; // no more entries, not found + id = _table[id].next; + } + } + +void* SrHashTableBase::lookup ( const char* st ) const + { + int id = lookup_index ( st ); + return id<0? 0: _table[id].data; + } + +bool SrHashTableBase::_insert ( const char *st, void* data, char dynamic ) + { + if ( !st ) { _last_id=-1; return false; } + if ( _hash_size<=0 ) init ( 256 ); // automatic initialization to avoid problems + + int id = ::hash ( st, _hash_size ); + + if ( _table[id].st==0 ) // empty entry, just take it + { _set_entry ( id, st, data, dynamic ); + _elements++; + _last_id = id; + return true; + } + + while ( true ) + { if ( sr_compare(_table[id].st,st)==0 ) // already there + { _last_id = id; + return false; + } + + // else check next colliding entry: + if ( _table[id].next<0 ) // no more entries, add one: + { int newid; + if ( _free.size()>0 ) + { newid = _free.pop(); + } + else + { newid = _table.size(); + _table.push(); + } + _table[id].next = newid; + _set_entry ( newid, st, data, dynamic ); + _elements++; + _last_id = newid; + return true; + } + + id = _table[id].next; + } + } + +void* SrHashTableBase::remove ( const char *st ) + { + if ( !st ) { _last_id=-1; return false; } + + int id = ::hash ( st, _hash_size ); + + if ( _table[id].st==0 ) return 0; // already empty entry + + int priorid=id; + while ( true ) + { if ( sr_compare(_table[id].st,st)==0 ) // found: remove it + { + void* data = _table[id].data; + int next = _table[id].next; + _clear_entry ( id ); + _elements--; + + // fix links: + if ( priorid==id ) // removing first entry + { if ( next>=0 ) + { _table[id] = _table[next]; + _set_entry ( next, 0 /*st*/, 0 /*data*/, 0 /*dynamic*/ ); + _free.push() = next; + } + else + { } // nothing to do + } + else // removing entry in the "linked list" + { _table[priorid].next = next; + _free.push() = id; + } + + return data; + } + + priorid = id; + id = _table[id].next; + } + } + +void SrHashTableBase::_set_entry ( int id, const char *st, void* data, char dynamic ) + { + if ( dynamic ) + _table[id].st = sr_string_new ( st ); // can return null if st==0 + else + _table[id].st = (char*)st; + + _table[id].data = data; + _table[id].next = -1; + _table[id].dynamic = dynamic; + } + +void SrHashTableBase::_clear_entry ( int id ) + { + if ( _table[id].dynamic ) delete _table[id].st; + + _table[id].st = 0; + _table[id].data = 0; + _table[id].next = -1; + _table[id].dynamic = 0; + } + +//============================== end of file =============================== + diff --git a/source/dcdt/se/sr_hash_table.h b/source/dcdt/se/sr_hash_table.h new file mode 100644 index 0000000000..39244a5cdd --- /dev/null +++ b/source/dcdt/se/sr_hash_table.h @@ -0,0 +1,155 @@ + +/** \file sr_hash_table.h + * Hash table functions */ + +# ifndef SR_HASH_TABLE_H +# define SR_HASH_TABLE_H + +# include "sr_array.h" + +//================================ SrHashTableBase =============================== + +/*! \class SrHashTableBase sr_hash_table.h + Stores user data associated with string keys in a hash table. + Note1: this is a rather specific implementation: colliding elements + are appended to the same array of entries, this is very efficient for inserting + elements but less practical for removing elements (removal was not implemented). + Note2: the user is responsible for allocation/deallocation of the appended user data, + which is merely stored as given void pointers, more info about that in the SrHashTable class. */ +class SrHashTableBase + { protected: + struct Entry { char* st; // the string id of this entry, or null if empty entry + void* data; // the user data associated or null if none + int next; // the index of the next colliding item + char dynamic; // 1 if st was created with new operator, 0 if st is static + }; + + SrArray _table; + SrArray _free; + int _hash_size; + int _elements; + int _last_id; + + public: + + /*! Default constructor creates a table of given hash size. If the hash size + is 0 (the default value), init() must be properly called aftwerwards. */ + SrHashTableBase ( int hsize=0 ); + + /*! Destructor */ + ~SrHashTableBase (); + + /*! Destroy actual table and builds a new empty one with the given hash size. + A value of zero will make the hash table empty and unusable. */ + void init ( int hsize ); + + /*! Rebuilds the table with a different hash size */ + void rehash ( int new_hsize ); + + /*! Returns the table size, which can be greater than the initial + given hash size in case there were collisions */ + int size () const { return _table.size(); } + + /*! Returns the initial (minimum) size of the table */ + int hash_size () const { return _hash_size; } + + /*! Returns the number of colliding elements in the table */ + int collisions () const { return _table.size()-_free.size()-_hash_size; } + + /*! Calculates and returns the maximum number of comparisons + that can happen when searching for a string in the table, + ie, the longest list size associated with a same entry in + the hash table. If there are no collisions, 1 is returned. */ + int longest_entry () const; + + /*! Total number of elements inserted in the table */ + int elements () const { return _elements; } + + /*! Returns the next index of an entry colliding with index id. + -1 is returned in case there is no further "colliding index" */ + int collidingid ( int id ) const { return _table[id].next; } + + /*! Returns the string key associated with the given id (can be null). + No validity checkings in the index are done! */ + const char* key ( int id ) const { return _table[id].st; } + + /*! Returns the user data associated with the given id (can be null). + No validity checkings in the index are done! */ + void* data ( int id ) const { return _table[id].data; } + + /*! Returns the valid index entry (>=0) relative to the given string key, + or -1 if the string key does not exist in the table (or if st==0)*/ + int lookup_index ( const char *st ) const; + + /*! Returns the user data associated with the given string key, + or null if the string was not found */ + void* lookup ( const char* st ) const; + + /*! Inserts a string key and user data to the table and returns true in case of success. + False is returned in case the string key already exists (or if st==0) meaning + that the entry was not added to the table. + If the string already exists, its index can be retrieved with lastid(). + The given st pointer is copied in an internal allocated string. + (note: if not set, the hash size is automatically initialized with 256) */ + bool insert ( const char *st, void* data ) { return _insert(st,data,1); } + + /*! Same as the insert() method, however the given st pointer is considered to + be a pointer to a static string and is not internally allocated. */ + bool insertstat ( const char *st, void* data ) { return _insert(st,data,0); } + + /*! Returns the id involved during the last call to the insert methods: + it will be 1) the index of the new added entry, or 2) the index of the found + duplicated entry, or 3) -1 is the string key was null. */ + int lastid () const { return _last_id; } + + /*! Removes and returns the data associated with key st. Returns 0 if + the key was not found. */ + void* remove ( const char *st ); + + private : + bool _insert ( const char *st, void* data, char dynamic ); + void _set_entry ( int id, const char *st, void* data, char dynamic ); + void _clear_entry ( int id ); + }; + +//================================ SrHasTable =============================== + +/*! \class SrHasTable sr_hash_table.h + Template version for usefull typecasts to a user type X. + For generality, there is no destructor and the user is responsible to + delete any attached pointers, using or not methods delete_data() and unref_data(). */ +template +class SrHashTable : public SrHashTableBase + { public: + /*! This constructor simply calls the constructor of the base class */ + SrHashTable ( int hsize ) : SrHashTableBase(hsize) {} + + /*! Simple type cast to the base class method */ + X data ( int id ) const { return (X)SrHashTableBase::data(id); } + + /*! Simple type cast to the base class method */ + X lookup ( const char* st ) const { return (X)SrHashTableBase::lookup(st); } + + /*! Will delete and set to zero all attached data in the hash table using + the delete operator after a type cast to (X*). */ + void delete_data () + { int i; + for (i=0; i<_table.size(); i++) + { delete (X*)_table[i].data; _table[i].data=0; } // delete 0 is ok + } + + /*! Will call unref() and set to zero all attached data in the hash table. It assumes + that X derives SrSharedClass, and therefore X* is type-casted to SrSharedClass*. */ + void unref_data () + { int i; + for (i=0; i<_table.size(); i++) + { if (_table[i].data) { ((SrSharedClass*)_table[i].data)->unref(); _table[i].data=0; } } + } + + /*! Simple type cast to the base class method */ + X remove ( const char *st ) { return (X)SrHashTableBase::remove(st); } + }; + +//============================== end of file =============================== + +# endif // SR_HASH_TABLE_H diff --git a/source/dcdt/se/sr_heap.h b/source/dcdt/se/sr_heap.h new file mode 100644 index 0000000000..50955af433 --- /dev/null +++ b/source/dcdt/se/sr_heap.h @@ -0,0 +1,119 @@ + +# ifndef SR_HEAP_H +# define SR_HEAP_H + +/** \file sr_heap.h + * Template for a heap based on SrArray. */ + +# include "sr_array.h" + +/*! \class SrHeap sr_heap.h + \brief Heap based on a SrArray + + SrHeap implements a heap ordered binary tree based on a SrArray object. + Because of performance issues, SrHeap does not honor constructors or + destructors of its elements X, so user-managed pointers should be used + in case of more complex classes, in the same design line as SrArray. + The heap is ordered according to its associated cost. The remove() + method will always remove the minimum cost element in the heap. + class X is the element type, and Y is the cost type. Y will usually + be int, float or double. */ +template +class SrHeap + { private : + struct Elem { X e; Y c; }; + SrArray _heap; + + public : + + /*! Default constructor. */ + SrHeap () {} + + /*! Copy constructor. */ + SrHeap ( const SrHeap& h ) : _heap(h._heap) {} + + /*! Set the capacity of the internal array */ + void capacity ( int c ) { _heap.capacity(c); } + + /*! Returns true if the heap is empty, false otherwise. */ + bool empty () const { return _heap.empty(); } + + /*! Returns the number of elements in the queue. */ + int size () const { return _heap.size(); } + + /*! Initializes as an empty heap */ + void init () { _heap.size(0); } + + /*! Compress the internal heap array */ + void compress () { _heap.compress(0); } + + /*! Make the heap have the given size, by removing the worst elements. + Only applicable when s < size(). */ + void size ( int s ) + { if ( s<=0 ) { init(); return; } + if ( s>=size() ) return; + SrArray tmp(s,s); + while ( tmp.size()0 ) + { insert ( tmp.top().e, tmp.top().c ); + tmp.pop(); + } + } + + /*! Insert a new element with the given cost */ + void insert ( const X& elem, Y cost ) + { // insert at the end: + _heap.push(); + _heap.top().e = elem; + _heap.top().c = cost; + // swim up: (parent of elem k is k/2) + Elem tmp; + int k=_heap.size(); + while ( k>1 && _heap[k/2-1].c>_heap[k-1].c ) + { SR_SWAP ( _heap[k/2-1], _heap[k-1] ); + k = k/2; + } + } + + /*! Removes the element in the top of the heap, which is always + the element with lowest cost. */ + void remove () + { // put last element in top: + _heap[0] = _heap.pop(); + // sink down: (children of node k are 2k and 2k+1) + int j; + int k=1; + int n=_heap.size(); + Elem tmp; + while ( 2*k<=n ) + { j=2*k; + if ( j_heap[j].c ) j++; + if ( !(_heap[k-1].c>_heap[j-1].c) ) break; + SR_SWAP ( _heap[k-1], _heap[j-1] ); + k=j; + } + } + + /*! Get a reference to the top element of the the heap, + which is always the element with lowest cost. */ + const X& top () const { return _heap[0].e; } + + /*! Get the lowest cost in the heap, + which is always the cost of the top element. */ + Y lowest_cost () const { return _heap[0].c; } + + /*! Returns elem i (0<=i + +# include "sr_image.h" + +//=========================== SrImage ============================ + +SrImage::SrImage () + { + _w = _h = _tw = 0; + _data = 0; + } + +SrImage::~SrImage () + { + delete _data; + } + +void SrImage::alloc ( int w, int h ) + { + int newsize = w*h*3; + + if ( newsize!=_tw*_h ) + { + delete _data; + + if ( w<=0 || h<=0 ) w=h=0; + + if ( w ) + _data = new srbyte [ newsize ]; // to hold rgb values + else + _data = 0; + } + + _tw = 3*w; + _w = w; + _h = h; + } + +void SrImage::vertical_mirror () + { + int i, ie, mid; + mid = _h/2; + + srbyte* buf = new srbyte [ _tw ]; + + for ( i=0,ie=_h-1; i=0; h-- ) + { scanline = line(h); + for ( w=0; w<_tw; w+=3 ) + { fputc ( scanline[w+2], f ); // B + fputc ( scanline[w+1], f ); // G + fputc ( scanline[w], f ); // R + } + for ( w=0; w +# include +# include + +# include "sr_input.h" +# include "sr_string.h" +# include "sr_array.h" + +//# define SR_USE_TRACE1 //Parser +//# define SR_USE_TRACE2 //Init +# include "sr_trace.h" + +# define ISNULL _type==(srbyte)TypeNull +# define ISFILE _type==(srbyte)TypeFile +# define ISSTRING _type==(srbyte)TypeString + +//=============================== SrInput ================================= + +struct SrInput::UngetData + { struct Token { char* string; srbyte type; }; + SrArray character; // unget buffer for char reading + SrArray token; // unget buffer for token reading + void init (); + }; + +void SrInput::UngetData::init () + { + character.size(0); + while ( token.size() ) delete token.pop().string; + } + +void SrInput::_init ( char c ) + { + _size = 0; + _curline = 0; + _comment_style = c; + _type = (srbyte) TypeNull; + _last_error = (srbyte) NoError; + _last_token_type = 0; + _max_token_size = 256; + _lowercase_tokens = 1; // true + _skipped_spaces = 0; + _unget = new UngetData; + _filename = 0; + } + +SrInput::SrInput ( char com ) + { + SR_TRACE2 ("Default Constructor"); + _init ( com ); + } + +SrInput::SrInput ( const char *buff, char com ) + { + SR_TRACE2 ("String Constructor"); + _init ( com ); + if ( buff ) + { _cur.s = buff; + _ini.s = buff; + _size = strlen ( buff ); + _curline = 1; + _type = (srbyte) TypeString; + } + } + +// static utility function: +static void get_size ( FILE *fp, int &size, int &start ) + { + start = (int)ftell(fp); + fseek ( fp, 0, SEEK_END ); + size = (int)ftell(fp) - start; + fseek ( fp, start, SEEK_SET ); + } + +SrInput::SrInput ( FILE *file, char com ) + { + SR_TRACE2 ("File Constructor"); + _init ( com ); + if ( file ) + { _cur.f = file; + _curline = 1; + _type = (srbyte) TypeFile; + get_size ( file, _size, _ini.f ); + } + } + +SrInput::SrInput ( const char* filename, const char* mode, char com ) + { + SR_TRACE2 ("File2 Constructor"); + _init ( com ); + FILE* file = fopen ( filename, mode ); + sr_string_set ( _filename, filename ); + if ( file ) + { _cur.f = file; + _curline = 1; + _type = (srbyte) TypeFile; + get_size ( file, _size, _ini.f ); + } + } + +SrInput::~SrInput () + { + close (); // close frees all data inside _unget, and frees _filename + delete _unget; + } + +void SrInput::init ( const char *buff ) + { + SR_TRACE2 ("Init with string"); + close (); + if ( buff ) + { _cur.s = buff; + _ini.s = buff; + _size = strlen ( buff ); + _curline = 1; + _type = (srbyte) TypeString; + } + } + +void SrInput::init ( FILE *file ) + { + SR_TRACE2 ("Init with file"); + close (); + if ( file ) + { _cur.f = file; + _curline = 1; + _type = (srbyte) TypeFile; + get_size ( file, _size, _ini.f ); + } + } + +void SrInput::init ( const char* filename, const char* mode ) + { + SR_TRACE2 ("Init with file2"); + FILE* file = fopen ( filename, mode ); + init ( file ); + sr_string_set ( _filename, filename ); + } + +void SrInput::close () + { + if ( ISFILE ) fclose ( _cur.f ); + _size = 0; + _curline = 0; + _type = (srbyte) TypeNull; + _last_error = (srbyte) NoError; + _last_token = ""; + _last_token_type = 0; + _unget->init (); + sr_string_set ( _filename, 0 ); + } + +void SrInput::leave_file () + { + _type = (srbyte) TypeNull; + close (); + } + +FILE* SrInput::filept () + { + return _type==TypeFile? _cur.f:0; + } + +bool SrInput::valid () const + { + return (ISNULL)? false : true; + } + +bool SrInput::finished () + { + if ( _unget->character.size()>0 || _unget->token.size()>0 ) return false; + else if ( ISFILE ) return pos()>=_size? true:false; + else if ( ISSTRING ) return *(_cur.s)? false:true; + else return true; + } + +void SrInput::getall ( SrString& buf ) + { + if ( ISFILE ) + { int s = size()-pos(); + buf.capacity ( s+2 ); // need +2 to cope with pc text files + fread ( (void*)(const char*)buf, sizeof(char), (size_t)s, _cur.f ); + buf [ s+1 ] = 0; + } + else if ( ISSTRING ) + { + buf.set ( _cur.s ); + _cur.s = _ini.s+_size; + } + } + +int SrInput::getline ( SrString& buf ) + { + int c; + + buf.len(0); + do { c = get(); + buf << (char)c; + } while ( c!='\n' && c!=EOF ); + + return c; + } + +int SrInput::getchar () + { + int c = EOF; + + if ( _unget->character.size()>0 ) + { c = _unget->character.pop(); + } + else + { if ( ISFILE ) c=fgetc(_cur.f); + else if ( ISSTRING ) c = *_cur.s? *_cur.s++:EOF; + } + + if ( c=='\n' ) _curline++; + return c; +} + +static void skip_c_comment ( SrInput *p ) + { + int c, d; + while ( true ) + { c = p->getchar(); + if ( c=='/' ) // for nested comments + { d = p->getchar(); + if ( d=='*' ) skip_c_comment(p); else p->unget(d); + } + else if ( c=='*' ) + { d = p->getchar(); + if ( d=='/' ) return; else p->unget(d); + } + else if ( c<0 ) return; // EOF found in the middle of a comment will not cause an error. + } + } + +int SrInput::get () + { + int c = getchar(); + + if ( _comment_style==0 ) + { return c; + } + else if ( _comment_style=='C' && c=='/' ) + { int d = getchar(); + if ( d=='*' ) + { skip_c_comment(this); + return get(); + } + else if ( d=='/' ) + { skip_line (); + return get(); + } + else unget(d); + } + else if ( c==_comment_style ) + { skip_line (); + return get(); + } + return c; + } + +void SrInput::unget ( char c ) + { + if ( c=='\n' && _curline>0 ) _curline--; + _unget->character.push() = c; + } + +void SrInput::advance ( int n ) + { + while ( n-->0 ) if ( getchar()<0 ) break; + } + +void SrInput::rewind () + { + if ( ISNULL ) return; + _unget->init(); + _curline = 1; + if ( ISSTRING ) _cur.s = _ini.s; + else fseek ( _cur.f, _ini.f, SEEK_SET ); + } + +int SrInput::pos () + { + if ( ISFILE ) return ((int)ftell(_cur.f)) - _ini.f; + else if ( ISSTRING ) return ((sruint)_cur.s) - ((sruint)_ini.s); + else return 0; + } + +void SrInput::pos ( int pos ) + { + _unget->init(); + if ( ISFILE ) fseek ( _cur.f, pos+_ini.f, SEEK_SET ); + else if ( ISSTRING ) _cur.s = _ini.s+pos; + } + +void SrInput::set_pos_and_update_cur_line ( int pos ) + { + _unget->init(); + rewind (); + advance ( pos ); + } + +void SrInput::skip_line () + { + int c; + do { c = getchar(); + } while ( c!=EOF && c!='\n' ); + } + +void SrInput::unget_token ( const char *token, SrInput::TokenType type ) + { + UngetData::Token& t = _unget->token.push(); + t.string = 0; + t.type = (srbyte) type; + sr_string_set ( t.string, token ); + } + +void SrInput::unget_token () + { + if ( _last_token_type!=EndOfFile ) + unget_token ( _last_token, (TokenType)_last_token_type ); + } + +bool SrInput::has_unget_data () const + { + return _unget->token.size()==0 && _unget->character.size()==0? false:true; + } + +static char get_escape_char ( char c ) + { + switch ( c ) + { case 'n' : return '\n'; + case 't' : return '\t'; + case '\n': return 0; // Just skip line + default : return c; + } + } + +SrInput::TokenType SrInput::get_token ( SrString &buf ) + { + # define UNGET(c) if(c>0)unget(c) + + int i; + int c = ' '; + int size = _max_token_size; + TokenType ret; + + _last_error = (srbyte) NoError; + + buf.capacity(size); + + if ( _unget->token.size()>0 ) + { UngetData::Token& t = _unget->token.pop(); + buf.set ( t.string ); + delete t.string; + return (TokenType) t.type; + } + + buf[0] = buf[1] = 0; + + _skipped_spaces = 0; + while ( c && isspace(c) ) // skip initial spaces; + { c=get(); _skipped_spaces++; } + + if ( c==EOF ) + { SR_TRACE1 ( "Got the End Of File!" ); + ret = EndOfFile; + } + else if ( strchr(SR_INPUT_DELIMITERS,c) && c!='.' ) // '.' will be detected after checking a real + { buf[0]=c; ret=Delimiter; + SR_TRACE1 ( "Got a Delimiter: "< "< "<'? true:false; + } + +bool SrInput::read_field ( SrString& name ) + { + name = ""; + if ( getd()!='<' ) return false; + name = gets(); + if ( name=="" ) return false; + if ( getd()!='>' ) return false; + return true; + } + +bool SrInput::close_field ( const SrString& name ) + { + if ( getd()!='<' ) return false; + if ( getd()!='/' ) return false; + if ( gets()!=name ) return false; + if ( getd()!='>' ) return false; + return true; + } + +bool SrInput::skip_field ( const SrString& name ) + { + while ( true ) + { if ( close_field(name) ) return true; + if ( _last_token_type==EndOfFile ) return false; + } + } + +//================================= operators ================================== + +SrInput& operator>> ( SrInput& in, int& i ) + { + SrString& s = in.getn(); + i = s.atoi(); + return in; + } + +SrInput& operator>> ( SrInput& in, sruint& i ) + { + SrString& s = in.getn(); + i = (sruint)s.atoi(); + return in; + } + +SrInput& operator>> ( SrInput& in, srbyte& c ) + { + SrString& s = in.getn(); + c = (srbyte)s.atoi(); + return in; + } + +SrInput& operator>> ( SrInput& in, float& f ) + { + SrString& s = in.getn(); + f = s.atof(); + return in; + } + +SrInput& operator>> ( SrInput& in, double& d ) + { + SrString& s = in.getn(); + d = s.atod(); + return in; + } + +SrInput& operator>> ( SrInput& in, char* st ) + { + SrString& s = in.gets(); + strcpy ( st, s ); + return in; + } + +//============================ End of File ============================== diff --git a/source/dcdt/se/sr_input.h b/source/dcdt/se/sr_input.h new file mode 100644 index 0000000000..a57dd70a18 --- /dev/null +++ b/source/dcdt/se/sr_input.h @@ -0,0 +1,369 @@ + +# ifndef SR_INPUT_H +# define SR_INPUT_H + +/** \file sr_input.h + * parses input file or string */ + +// sr_array.h cannot be included here because of circular references +# include +# include "sr_string.h" + +/*! Defines which characters are considered as delimiters. */ +# define SR_INPUT_DELIMITERS "{}[]();,.=+-/^*%:&|!~<>#?@\\\'" + +/*! \class SrInput sr_input.h + \brief Parses a file or string buffer + + SrInput reads data from a string buffer or from an open file. It can + be used to read data byte per byte, or by parsing tokens that are + recognized as names, delimiters, integers, reals or strings. + Comments can be specified to be skiped and error messages are returned. */ +class SrInput + { public : + + /*! Indicates the type of the current input. It can be TypeFile, TypeString, + or TypeNull. TypeNull indicates that the SrInput is not valid, and need + to be connected to a file or string to become valid (by calling init()). */ + enum Type { TypeFile, //!< Input from a file + TypeString, //!< Input from a string buffer + TypeNull //!< Input not initialized, valid() will return false + }; + + /*! Indicates the type of the token returned in get_token() */ + enum TokenType { Name, //!< A sequence of chars without spaces and delimiters + Delimiter, //!< Any delimiter defined in macro SR_INPUT_DELIMITERS + Integer, //!< An int type number + Real, //!< A real type number + String, //!< A seq inside "". Returns content converting c-like escape chars. + EndOfFile, //!< End of file encountered + Error //!< There was an error when parsing the last token. + }; + + /*! Indicates the error occured in the last get operation. See the comments in each + function to know the types of error that can be generated. */ + enum ErrorType { NoError, //!< No error occured in the last operation + UndefChar, //!< Unrecognized char was found + UnexpectedToken, //!< Parsed token is of an unexpected type + TooBig, //!< token is bigger than the given buffer + OpenString, //!< EOF found before end of a string + InvalidPoint //!< Misplaced decimal point found in a real number + }; + + private : + + struct UngetData; + union { FILE *f; const char *s; } _cur; // the current input position + union { int f; const char *s; } _ini; // the beginning of the buffer + int _size; // the size of the input in bytes + int _curline; // keeps track of the current line + char _comment_style; // 0:none, 'C':C/C++ like, otherwise skip line + srbyte _type; // the enumerator Type + srbyte _last_error; // last error, if any. + SrString _last_token; // buffer with the last token read + srbyte _last_token_type; // buffer with the last token type read + int _max_token_size; // max size allowed for parsed tokens + char _lowercase_tokens; // if tokens should be converted to lowercase + int _skipped_spaces; // number of spaces skipped during last get_token + UngetData* _unget; // unget char and token information + char* _filename; // optional file name of the open file + void _init ( char c ); + public : + + /*! Construct an input of type SrInput::TypeNull. + SrInput will only be operational when linked to a file or string by calling + an init() function later. While SrInput is of type Null, the valid() method will + return false. The parameter com is the comment style for this input and is 0 by + default. See comment_style() for a description of possible styles. */ + SrInput ( char com=0 ); // init as a null input + + /*! Construct an input of type SrInput::TypeString. + In this way SrInput will read bytes from the given null-terminated string buffer. + If buff is null, SrInput will be initialized as TypeNull, and not as a String type. + Com is the comment style for this input and is 0 by default. + See comment_style() for a description of possible styles. */ + SrInput ( const char *buff, char com=0 ); // init as a string input + + /*! Construct an input of type SrInput::File + In this way, SrInput will read bytes from the given C-style file stream. If the file pointer + is null, SrInput will be initialized as TypeNull, and not as a File type. The current + position pointed by file is considered to be the start, and so the size is measured + since this given position. The parameter com is the comment style for this input and + is 0 by default. See comment_style() for a description of possible styles */ + SrInput ( FILE *file, char com=0 ); // init as a file input + + /*! Construct an input of type SrInput::File, from a given filename. + This is similar to the previous constructor, but it receives the fopen() parameters + directly (file name and mode strings) and stores internally the given file name. */ + SrInput ( const char* filename, const char* mode, char com=0 ); // init as a file input + + /*! Closes the associated file if it is a TypeFile input. + If it is required to leave the associated input file open, call leave_file() before. */ + ~SrInput (); + + /*! Closes actual input, and init it as TypeString. + If buff is null, SrInput will be initialized as TypeNull, and not as a String type. + The comment style is not changed, but unget data are freed. */ + void init ( const char *buff ); + + /*! Closes actual input, and init it as a File type. + If file is null, SrInput will be initialized as TypeNull, and not as a File type. + The comment style is not changed, but unget data are freed. */ + void init ( FILE *file ); + + /*! Closes actual input, and init it as a File type, opening given file name in given mode. + If file could not be open, SrInput is initialized as TypeNull, and not as a File type. + Filename is stored, the comment style is not changed, but unget data are freed. */ + void init ( const char* filename, const char* mode ); + + /*! Closes actual input and set it as TypeNull. + If SrInput is of type file, the associated file is closed. In all cases, + unget data is freed, filename is set to null, the last error is cleaned, + and size and curline are set to zero. */ + void close (); + + /*! Puts SrInput into TypeNull mode but without closing the current file. + If SrInput is not of File type, the effect is the same as close(). */ + void leave_file (); + + /*! If the input is done from a file, return the FILE pointer associated, otherwise + will return 0 */ + FILE* filept (); + + /*! Returns the file name used for opening a file input, or null if not available */ + const char* filename () const { return _filename; } + + /*! Associates with the input a file name. The string is stored but not used by SrInput. */ + void filename ( const char* s ) { sr_string_set(_filename,s); } + + /*! Returns true if the input is not of TypeNull type. */ + bool valid () const; + + /*! Returns the type of the SrInput. */ + Type type () const { return (Type) _type; } + + /*! Returns the size of the current input. Size is calculated as the number in bytes from + the beginning of the input to the end of the input. The beginning is considered to be + the pointer passed to the constructor or init() method, when it is calculated. */ + int size () const { return _size; } + + /*! Returns the current line of the input. When SrInput is created or initialized, the + current line is set to 1. Afterwards, the current line is updated internally each + time a newline char is read. Note that when pos(int) method is used, the current + line is no more valid. */ + int curline () const { return _curline; } + + /*! Defines the comment style used for all "get methods", with exception to get_byte(). + The style can be set to 0 if no comments are allowed, can be set to 'C' to have + standard C and C++ nested comments parsed, or otherwise when the specified + character is encountered the whole line is skiped. */ + void comment_style ( char style ) { _comment_style=style; } // style can be 0,'C','anychar' + + /*! Returns the current comment style being used. */ + char comment_style () const { return _comment_style; } + + /*! Returns true if the current input is pointing after the end of the file. If the + input source is empty but there is data to unget, the input is not considered finished.*/ + bool finished (); + + /*! Gets all bytes of the input, starting from the current position, and put them + in the given buffer. The input will point to its end. If input is of TypeNull, + nothing is done. Note: the input is not parsed, just copied, therefore comments + are also copied. */ + void getall ( SrString& buf ); + + /*! Gets all bytes of the current line, i.e. untill a '\n' is read, + and put them in the given buffer (inlcuding the '\n'). + The input will point to the first byte after the new line character. + The last character read is returned and will be either '\n' or EOF. + Uses method get(), and therefore comments are parsed. */ + int getline ( SrString& buf ); + + /*! Get current character and advances pointer, will return -1 if end was reached. + Will read next char from the associated string or file. If SrInput is of + TypeNull, -1 is always returned. Comments are not considered, but unget + chars are correctly handled. */ + int getchar (); // comments not handled + + /*! Get current byte and advance, will return 0 if finish is reached. + This is for ascii inputs where comments are skipped according to + the current style. */ + int get (); // comments handled + + /*! Puts a byte in the unget stack. */ + void unget ( char c ); + + /*! Reads the next n bytes of the current input. + Note that unget data are considered, but comments are not. */ + void advance ( int n=1 ); + + /*! puts the pointer in the beginning of the input. + Any unget data is cleared. Nothing is done if SrInput is of type Null. */ + void rewind (); + + /*! Offset in bytes from the begining of the input. */ + int pos (); + + /*! Puts the input pointer to the position pos, clearing all unget data. + After calling it, curline() becomes invalid. Alternatively, method + set_pos_and_update_cur_line() can be used */ + void pos ( int pos ); + + /*! Puts the input pointer to the position pos, clears all unget data and + determines the line number using rewind() and advance(pos). */ + void set_pos_and_update_cur_line ( int pos ); + + /*! Reads all the current line (with getchar), stopping one byte after the + first newline encountered. */ + void skip_line (); + + /*! Sets the maximum allowed size for parsed tokens. Default is 256. */ + void max_token_size ( int s ) { _max_token_size=s; } + + /*! Returns the current maximum allowed size for parsed tokens. */ + int max_token_size () const { return _max_token_size; } + + /*! Determines if parsed tokens are transformed to lowercase or not. Default is true. */ + void lowercase_tokens ( bool b ) { _lowercase_tokens = b; } + + /*! Retrieves the state if parsed tokens are transformed to lowercase or not. */ + bool lowercase_tokens () const { return _lowercase_tokens? true:false; } + + /*! Returns the number of leading spaces skipped during the + last call to get_token() */ + int skipped_spaces () const { return _skipped_spaces; } + + /*! Affects the next call to all methods using get_token(). + The token is copied and stored within SrInput, leaving the user buffer untouched. */ + void unget_token ( const char *token, TokenType type ); + + /*! Affects the next call to all methods using get_token(). + The last token parsed is ungetted, if its type is not EndOfFile */ + void unget_token (); + + /*! Returns true if any data (token or byte) was put in the unget stacks */ + bool has_unget_data () const; + + /*! Puts a token in buf, returning its type, skipping comments, and considering + unget() and unget_token() data. When EOF is reached, buf[0] is set to 0. + Names are converted to lower case according to the state set by lowercase_tokens(). + The maximum length permitted for buf is determined by max_token_size(). + When SrInput::Error is returned, last_error() will return the description of + the error occured. + Note: get_token() will parse an input like "-3" as having two tokens, a + delimiter and an integer (use getn() for reading numbers). */ + TokenType get_token ( SrString& buf ); + + /* Does the same as get_token(SrString&), but the parsed token and token type + are kept in an internal buffer, to be later retrieved by last_token() and + last_token_type() */ + TokenType get_token (); + + /* Returns a reference to the internal buffer containing the last token parsed + with get_token(void). */ + SrString& last_token() { return _last_token; } + + /* Returns the type of the last token parsed with get_token(void). */ + TokenType last_token_type() const { return (TokenType)_last_token_type; } + + /*! Returns the last error occured, if any. Errors are generated by methods + using get_token() methods. */ + ErrorType last_error () const { return (ErrorType)_last_error; } + + /*! Returns true if there an error occured, and false otherwise. */ + bool had_error () const { return ((ErrorType)_last_error)==NoError? false:true; } + + /*! Sets the last error to NoError. */ + void reset_error () { _last_error=(srbyte)NoError; } + + /*! This static method will return a describing error message if t is one + of the following: UndefChar, UnexpectedToken, TooBig, OpenString, or InvalidPoint. + Otherwise it will return 0 : */ + static char* error_desc ( ErrorType t ); + + /*! Gets the next token of type string or name using get_token(void), + and returns the token, which is kept in the internal last_token() string. + If the returned string has length 0 (ie equal to ""), it means that + the parsed token is not a string, neithert a name, or EOF was reached. + In any of these cases, last_error() will return UnexpectedToken. */ + SrString& gets (); + + /*! Same as gets(void), but without using the internal buffer of last_token(), + the given buffer buf is used instead. A reference to buf is retorned. */ + SrString& gets ( SrString& buf ); + + /*! Gets the next token, considering it is a number (type int or real) + preceeded or not by delimiters + or -, and returns the token, which + is kept in the internal last_token() string. + The type in last_token_type() will tell if it was an integer or real. + If the returned string has length 0 (ie equal to ""), it means that + the parsed token is not a number or EOF was reached. + In any of these cases, last_error() will return UnexpectedToken. */ + SrString& getn (); + + /*! Reads the next token using get_token(), and tests if it is a delimiter. + Returns the delimiter char if a delimiter was found, otherwise returns 0. + When 0 is returned, last_error() will be set to UnexpectedToken. */ + char getd (); + + /*! Skips next n tokens, by calling get_token(). n has a default value of 1. + False is returned if any error occurred or EOF is encountered. + Otherwise true is returned. */ + bool skip ( int n=1 ); + + /*! Reads tokens using get_token() until the given name is read as type Name. + skipto() uses the sr_compare() function that is case-insensitive. + Returns false if EOF was reached before, otherwise returns true. */ + bool skipto ( const char *name ); + + /*! Returns true if there is a field like in the current position, + or otherwise false is returned. All parsed tokens are ungetted, so that + the effect is as the current input position is not changed. */ + bool has_field (); + + /*! Parse the input expecting to encounter a field like , and puts + field_name in the string name. If unexpected tokens are read, an error occurs + and false is returned. If EOF is encountered, false is returned. */ + bool read_field ( SrString& name ); + + /*! Checks if the following tokens in input is , returning true or false. */ + bool close_field ( const SrString& name ); + + /*! Reads the input until reading . If the field is not found, false is + returned. Otherwise true is returned and the input will point after . */ + bool skip_field ( const SrString& name ); + + /*! Operator to read an integer using method getn() and function atoi(). + Errors can be tracked using last_error(). */ + friend SrInput& operator>> ( SrInput& in, int& i ); + + /*! Operator to read an unsigned integer using method getn() and function atoi(). + Errors can be tracked using last_error(). */ + friend SrInput& operator>> ( SrInput& in, sruint& i ); + + /*! Operator to read an unsigned char using method getn() and function atoi(). + Errors can be tracked using last_error(). */ + friend SrInput& operator>> ( SrInput& in, srbyte& c ); + + /*! Operator to read a float using method getn() and function atof(). + Errors can be tracked using last_error(). */ + friend SrInput& operator>> ( SrInput& in, float& f ); + + /*! Operator to read a double using method getn() and function atof(). + Errors can be tracked using last_error(). */ + friend SrInput& operator>> ( SrInput& in, double& d ); + + /*! Operator to read a string using method gets(). st must point to a buffer + with enought space to receive the input. The input is read using the + internal buffer, and the result is copied to st using simply strcpy(). + Errors can be tracked using last_error(). */ + friend SrInput& operator>> ( SrInput& in, char* st ); + + /*! Operator to read a SrString using method gets(SrString&). + Errors can be tracked using last_error(). Implemented inline. */ + friend SrInput& operator>> ( SrInput& in, SrString& s ) { in.gets(s); return in; } + }; + +//============================== end of file =============================== + +# endif // SR_INPUT_H diff --git a/source/dcdt/se/sr_light.cpp b/source/dcdt/se/sr_light.cpp new file mode 100644 index 0000000000..1d64914b89 --- /dev/null +++ b/source/dcdt/se/sr_light.cpp @@ -0,0 +1,29 @@ +#include "precompiled.h" +# include "sr_light.h" + +//===================================== SrLight ================================ + + +SrLight::SrLight () + { + init (); + } + +void SrLight::init () + { + spot_exponent = 0; + spot_direction.set ( 0, 0, -1.0f ); + spot_cutoff = 180; + constant_attenuation = 1.0f; + linear_attenuation = 0; + quadratic_attenuation = 0; + ambient = SrColor::black; + diffuse = SrColor::white; + specular = SrColor::black; + position = SrVec::k; + directional = true; + } + + + +//================================ End of File ================================================= diff --git a/source/dcdt/se/sr_light.h b/source/dcdt/se/sr_light.h new file mode 100644 index 0000000000..3c9a462e08 --- /dev/null +++ b/source/dcdt/se/sr_light.h @@ -0,0 +1,82 @@ + +# ifndef SR_LIGHT_H +# define SR_LIGHT_H + +/** \file sr_light.h + * Keeps light parameters + */ + +# include "sr_vec.h" +# include "sr_color.h" + +/*! \class SrLight sr_light.h + \brief Keeps light parameters + + SrLight Keeps light parameters following the OpenGL specifications. + Note however that this class only stores data and has no dependency on + OpenGL functions. */ +class SrLight + { public : + + /*! Higher spot exponents result in a more focused light source. The default + spot exponent is 0, resulting in uniform light distribution. Values must + be in the range of [0,128]. */ + float spot_exponent; + + /*! The direction of the light in homogeneous object coordinates. The spot + direction is transformed by the inverse of the modelview matrix when + glLight is called (just as if it was a normal), and it is stored in + eye coordinates. It is significant only when GL_SPOT_CUTOFF is not + 180, which it is by default. The default direction is (0,0,-1). */ + SrVec spot_direction; + + /*! Specifies the maximum spread angle. Only values in the range [0,90], + and the special value 180, are accepted. If the angle between the + direction of the light and the direction from the light to the vertex + being lighted is greater than the spot cutoff angle, then the light + is completely masked. Otherwise, its intensity is controlled by the + spot exponent and the attenuation factors. The default spot cutoff + is 180, resulting in uniform light distribution. */ + float spot_cutoff; + + /*! If the light is positional, rather than directional, its intensity + is attenuated by the reciprocal of the sum of: the constant factor, + the linear factor multiplied by the distance between the light and + the vertex being lighted, and the quadratic factor multiplied by + the square of the same distance. The default attenuation factors + are cte=1, linear=0, quad=0, resulting in no attenuation. Only + nonnegative values are accepted. */ + float constant_attenuation; + float linear_attenuation; //!< See constant_attenuation. + float quadratic_attenuation; //!< See constant_attenuation. + + SrColor ambient; //!< Default is black + SrColor diffuse; //!< Default is white + SrColor specular; //!< Default is black + + /*! The position is transformed by the modelview matrix when glLight is + called (just as if it was a point), and it is stored in eye + coordinates. If directional, diffuse and specular lighting calculations + take the lights direction, but not its actual position, into account, + and attenuation is disabled. Otherwise, diffuse and specular lighting + calculations are based on the actual location of the light in eye + coordinates, and attenuation is enabled. The default position is (0,0,1). */ + SrVec position; + + /*! When true means that the position w coord is 0, otherwise 1. Default is true. */ + bool directional; + + public : + + /*! Initialize the camera with the default parameters. */ + SrLight (); + + /*! Sets the default parameters. */ + void init (); + }; + + +//================================ End of File ================================================= + +# endif // SR_LIGHT_H + diff --git a/source/dcdt/se/sr_line.cpp b/source/dcdt/se/sr_line.cpp new file mode 100644 index 0000000000..6ad32cfc23 --- /dev/null +++ b/source/dcdt/se/sr_line.cpp @@ -0,0 +1,177 @@ +#include "precompiled.h" +# include +# include "sr_box.h" +# include "sr_line.h" +# include "sr_input.h" +# include "sr_output.h" + +//=========================================================================== + +// Important: Static initializations cannot use other static initialized +// variables ( as SrVec::i, ... ), as we cannot know the order that they +// will be initialized by the compiler. + +const SrLine SrLine::x ( SrVec(0,0,0), SrVec(1.0f,0,0) ); +const SrLine SrLine::y ( SrVec(0,0,0), SrVec(0,1.0f,0) ); +const SrLine SrLine::z ( SrVec(0,0,0), SrVec(0,0,1.0f) ); + +//============================== SrLine ==================================== + +#define EPSILON 0.00001 // floats have 7 decimals + +// Original code from : http://www.acm.org/jgt/papers/MollerTrumbore97/ +bool SrLine::intersects_triangle ( const SrVec &v0, const SrVec &v1, const SrVec &v2, + float &t, float &u, float &v ) const + { + SrVec dir, edge1, edge2, tvec, pvec, qvec; + float det, inv_det; + + dir = p2 - p1; + edge1 = v1 - v0; // find vectors for two edges sharing v0 + edge2 = v2 - v0; + pvec = cross ( dir, edge2 ); // begin calculating determinant - also used to calculate U parameter + det = dot ( edge1, pvec ); // if determinant is near zero, ray lies in plane of triangle + // printf("det=%f\n",det); + + if ( SR_NEXTZ(det,EPSILON) ) + { //sr_out.warning("det in ray_triangle fails => %f",(float)det); + return false; + } + inv_det = 1.0f / det; + + tvec = p1 - v0; // calculate distance from v0 to ray origin + u = dot(tvec, pvec) * inv_det; // calculate U parameter and test bounds + if ( u<0.0 || u>1.0 ) return false; + + qvec = cross ( tvec, edge1 ); // prepare to test V parameter + v = dot(dir, qvec) * inv_det; // calculate V parameter and test bounds + if ( v<0.0 || u+v>1.0 ) return false; + t = dot(edge2,qvec) * inv_det; // calculate t, ray intersects triangle + + return true; + } + +bool SrLine::intersects_square ( const SrVec &v1, const SrVec &v2, + const SrVec &v3, const SrVec &v4, float& t ) const + { + float u, v; + if ( intersects_triangle ( v1, v2, v3, t, u, v ) ) return true; + if ( intersects_triangle ( v1, v3, v4, t, u, v ) ) return true; + return false; + } + +int SrLine::intersects_box ( const SrBox& box, float& t1, float& t2, SrVec* vp ) const + { + SrVec p1, p2, p3, p4, p; + float t[6]; + int side[6]; + int tsize=0; + + # define INTERSECT(s) if ( intersects_square(p1,p2,p3,p4,t[tsize]) ) { side[tsize]=s; tsize++; } + + box.get_side ( p1, p2, p3, p4, 0 ); + INTERSECT(0); + box.get_side ( p1, p2, p3, p4, 1 ); + INTERSECT(1); + box.get_side ( p1, p2, p3, p4, 2 ); + INTERSECT(2); + box.get_side ( p1, p2, p3, p4, 3 ); + INTERSECT(3); + box.get_side ( p1, p2, p3, p4, 4 ); + INTERSECT(4); + box.get_side ( p1, p2, p3, p4, 5 ); + INTERSECT(5); + + # undef INTERSECT + + if ( tsize==0 ) + { t1=t2=0; } + else if ( tsize==1 ) + { t1=t2=t[0]; } + else if ( tsize==2 ) + { float tmpf; + int tmpi; + if ( t[1]0 && vp) box.get_side ( vp[0], vp[1], vp[2], vp[3], side[0] ); + return tsize; + } + +int SrLine::intersects_sphere ( const SrPnt& center, float radius, SrPnt* vp ) const + { + // set up quadratic Q(t) = a*t^2 + 2*b*t + c + SrVec dir = p2-p1; + SrVec kdiff = p1 - center; + float a = dir.norm2(); + float b = dot ( kdiff, dir ); + float c = kdiff.norm2() - radius*radius; + + float aft[2]; + float discr = b*b - a*c; + + if ( discr < 0.0f ) + { return 0; + } + else if ( discr > 0.0f ) + { float root = sqrtf(discr); + float inva = 1.0f/a; + aft[0] = (-b - root)*inva; + aft[1] = (-b + root)*inva; + if ( vp ) + { vp[0] = p1 + aft[0]*dir; + vp[1] = p1 + aft[1]*dir; + if ( dist2(vp[1],p1)1.0f ) // closest point does not fall within the line segment + if ( k ) *k=u; + + return p1 + u*v; + } + +//============================== friends ==================================== + +SrOutput& operator<< ( SrOutput& o, const SrLine& l ) + { + return o << l.p1 <<" "<< l.p2; + } + +SrInput& operator>> ( SrInput& in, SrLine& l ) + { + return in >> l.p1 >> l.p2; + } + +//============================= End of File =========================================== diff --git a/source/dcdt/se/sr_line.h b/source/dcdt/se/sr_line.h new file mode 100644 index 0000000000..84ee6f6cff --- /dev/null +++ b/source/dcdt/se/sr_line.h @@ -0,0 +1,95 @@ +/** \file sr_line.h + * Three dimensional line */ + +# ifndef SR_LINE_H +# define SR_LINE_H + +# include "sr_vec.h" + +class SrBox; +class SrInput; +class SrOutput; + +/*! \class SrLine sr_line.h + \brief Three dimensional line + + SrLine defines a line by keeping two points in the three dimensional + space. These two points are p1 and p2 of type SrVec(==SrPnt). When the line + is considered as a segement, p1 and p2 will delimit the segment, and + when the line is considered as a ray, the source of the ray is p1 and + the ray direction is defined as p2-p1. */ +class SrLine + { public : + SrPnt p1, p2; + public : + static const SrLine x; //!< (0,0,0)--(1,0,0) line + static const SrLine y; //!< (0,0,0)--(0,1,0) line + static const SrLine z; //!< (0,0,0)--(0,0,1) line + public : + + /*! Initializes SrLine as the x axis (0,0,0)--(1,0,0) line. */ + SrLine () : p1(SrPnt::null), p2(SrPnt::i) {} + + /*! Copy constructor. */ + SrLine ( const SrLine &l ) : p1(l.p1), p2(l.p2) {} + + /*! Initializes with the given endpoints. */ + SrLine ( const SrPnt &v1, const SrPnt &v2 ) : p1(v1), p2(v2) {} + + /*! Set endpoints. */ + void set ( const SrPnt &v1, const SrPnt &v2 ) { p1=v1; p2=v2; } + + /*! Same as copy operator. */ + void set ( const SrLine &l ) { p1=l.p1; p2=l.p2; } + + /*! Copy operator from another SrLine. */ + void operator = ( const SrLine &l ) { p1=l.p1; p2=l.p2; } + + /*! Calculates the intersection of SrLine with the triangle [v0,v1,v2]. + If the line intercepts the triangle, true is returned, otherwise + false is returned. The triangle can be given in any orientation. + When true is returned the return values t,u,v will satisfy: + (1-u-v)v0 + uv1 + vv2 == (1-t)p1 + (t)p2 == interception point. + In this way, u and v indicate a parametric distance from the vertices + and t is a parametric distance that can be used to determine if only + the segment [p1,p2] intersects in fact the triangle. */ + bool intersects_triangle ( const SrPnt &v0, const SrPnt &v1, const SrPnt &v2, + float &t, float &u, float &v ) const; + + /*! Returns the number of intersections between the line and the square (v1,v2,v3,v4). + In case true is returned, the intersection point is + defined by (1-t)p1 + (t)p2, so that if t is between 0 and 1, the point + is also inside the segment determined by SrLine. */ + bool intersects_square ( const SrPnt& v1, const SrPnt& v2, + const SrPnt& v3, const SrPnt& v4, float& t ) const; + + /*! Returns 1 or 2 if the line intersects the box, otherwise 0 is returned. + In case 2 is returned, there are two intersection points defined by + (1-t1)p1 + (t1)p2, and (1-t2)p1 + (t2)p2 (t1> ( SrInput& in, SrLine& l ); + + }; + +//============================== end of file =============================== + +# endif // SR_LINE_H diff --git a/source/dcdt/se/sr_lines.cpp b/source/dcdt/se/sr_lines.cpp new file mode 100644 index 0000000000..fe9d6f9164 --- /dev/null +++ b/source/dcdt/se/sr_lines.cpp @@ -0,0 +1,265 @@ +#include "precompiled.h" +# include "sr_box.h" +# include "sr_mat.h" +# include "sr_vec2.h" +# include "sr_lines.h" + +//# define SR_USE_TRACE1 // Constructor and Destructor +# include "sr_trace.h" + +//======================================= SrLines ==================================== + +const char* SrLines::class_name = "Lines"; + +SrLines::SrLines () + { + SR_TRACE1 ( "Constructor" ); + } + +SrLines::~SrLines () + { + SR_TRACE1 ( "Destructor" ); + } + +void SrLines::init () + { + V.size(0); + C.size(0); + I.size(0); + } + +void SrLines::compress () + { + V.compress(); + C.compress(); + I.compress(); + } + +void SrLines::push_line ( const SrVec &p1, const SrVec &p2 ) + { + V.push()=p1; + V.push()=p2; + } + +void SrLines::push_line ( const SrVec2 &p1, const SrVec2 &p2 ) + { + V.push().set(p1.x,p1.y,0); + V.push().set(p2.x,p2.y,0); + } + +void SrLines::push_line ( float ax, float ay, float az, float bx, float by, float bz ) + { + V.push().set(ax,ay,az); + V.push().set(bx,by,bz); + } + +void SrLines::push_line ( float ax, float ay, float bx, float by ) + { + V.push().set(ax,ay,0); + V.push().set(bx,by,0); + } + +void SrLines::begin_polyline () + { + I.push() = V.size(); + } + +void SrLines::end_polyline () + { + I.push() = V.size()-1; + } + +void SrLines::push_vertex ( const SrVec& p ) + { + V.push() = p; + } + +void SrLines::push_vertex ( const SrVec2& p ) + { + V.push().set(p.x,p.y,0); + } + +void SrLines::push_vertex ( float x, float y, float z ) + { + V.push().set(x,y,z); + } + +void SrLines::push_color ( const SrColor &c ) + { + I.push() = V.size(); + C.push() = c; + I.push() = -C.size(); + } + +void SrLines::push_cross ( SrVec2 c, float r ) + { + SrVec2 p(r,r); + push_line ( c-p, c+p ); p.x*=-1; + push_line ( c-p, c+p ); + } + +void SrLines::push_axis ( const SrPnt& orig, float len, int dim, const char* let, + bool rule, SrBox* box ) + { + float r, mr; + float a, b, c, k; + const float z = 0.0f; + + r = box? box->max_size()/2.0f : len; + mr = -r; + a=r/25.0f; b=a/2.0f; c=a*3.0f; k=a/3.0f; + + bool letx=false, lety=false, letz=false; + int vi = V.size(); + + if ( let ) + while ( *let ) + { char c = SR_UPPER(*let); + if ( c=='X' ) letx=true; + else if ( c=='Y' ) lety=true; + else if ( c=='Z' ) letz=true; + let++; + } + + if ( dim>=1 ) + { if ( box ) { mr=box->a.x; r=box->b.x; } + push_color ( SrColor::red ); + push_line ( mr, z, z, r, z, z ); // X axis + if ( letx && r>0 ) + { push_line ( r, -a, z, r-a, -c, z ); // Letter X + push_line ( r-a, -a, z, r, -c, z ); + } + if ( rule && r>1.0 ) + { int i; int mini=SR_CEIL(mr); int maxi=SR_FLOOR(r); + push_color ( SrColor::red ); + for ( i=mini; i=2 ) + { if ( box ) { mr=box->a.y; r=box->b.y; } + push_color ( SrColor::green ); + push_line ( z, mr, z, z, r, z ); // Y axis + if ( lety && r>0 ) + { push_line ( a, r, z, a+b, r-a, z ); // Letter Y + push_line ( a+a, r, z, a, r-a-a, z ); + } + if ( rule && r>1.0 ) + { int i; int mini=SR_CEIL(mr); int maxi=SR_FLOOR(r); + push_color ( SrColor::green ); + for ( i=mini; i=3 ) + { if ( box ) { mr=box->a.z; r=box->b.z; } + push_color ( SrColor::blue ); + push_line ( z, z, mr, z, z, r ); // Z axis + if ( letz && r>0 ) + { begin_polyline (); + push_vertex ( z, -a, r-a ); // Letter Z + push_vertex ( z, -a, r ); + push_vertex ( z, -c, r-a ); + push_vertex ( z, -c, r ); + end_polyline (); + } + if ( rule && r>1.0 ) + { int i; int mini=SR_CEIL(mr); int maxi=SR_FLOOR(r); + push_color ( SrColor::blue ); + for ( i=mini; i& a ) + { + int i; + if ( a.size()<2 ) return; + I.push() = V.size(); + for ( i=0; i& a ) + { + int i; + if ( a.size()<2 ) return; + I.push() = V.size(); + for ( i=0; i& a ) + { + int i; + for ( i=0; i0 ) + { push_vertex ( center + x ); + x = x*mr; + } + push_vertex ( center+x1st ); // to make it close exactly + end_polyline (); + } + +void SrLines::get_bounding_box ( SrBox& b ) const + { + int i; + b.set_empty (); + for ( i=0; i=i will be C[-j-1]. + The indices i appearing in the sequence of pairs (i,j) must appear ordered, so that + 'I' is just read once when drawing the lines. */ +class SrLines + { public : + SrArray V; // C; // I; //& a ); + + /*! Push an array of points forming a polygon */ + void push_polygon ( const SrArray& a ); + + /*! Push an array of points assumin each 2 consecutive points denote one line */ + void push_lines ( const SrArray& a ); + + /*! Calls push_polygon() or push_polyline() according to p.open() */ + void push_polygon ( const SrPolygon& p ) + { if (p.open()) push_polyline( (const SrArray&)p ); + else push_polygon( (const SrArray&)p ); + } + + /*! Approximates a circle with a polyline, where: + center is the center point, center+radius gives the first point, + normal is orthogonal to radius, and nvertices gives the number of + vertices in the polyline */ + void push_circle_approximation ( const SrPnt& center, const SrVec& radius, + const SrVec& normal, int nvertices ); + + /*! Returns the bounding box of all vertices used. The returned box can be empty. */ + void get_bounding_box ( SrBox &b ) const; + }; + + +//================================ End of File ================================================= + +# endif // SR_LINES_H + diff --git a/source/dcdt/se/sr_list.cpp b/source/dcdt/se/sr_list.cpp new file mode 100644 index 0000000000..747283ebce --- /dev/null +++ b/source/dcdt/se/sr_list.cpp @@ -0,0 +1,292 @@ +#include "precompiled.h" +# include "sr_list.h" + +//============================ SrListBase ================================= + +SrListBase::SrListBase ( SrClassManagerBase* m ) + { + _first = 0; + _cur = 0; + _man = m; + _man->ref(); + _elements = 0; + } + +SrListBase::SrListBase ( const SrListBase& l ) + { + _first = 0; + _cur = 0; + _man = l._man; + _man->ref(); + _elements = 0; + insert_list ( l ); + } + +SrListBase::SrListBase ( SrListNode* n, SrClassManagerBase* m ) + { + _first = _cur = n; + _man = m; + _man->ref(); + _elements = 0; + } + +SrListBase::~SrListBase () + { + init (); + _man->unref(); + } + +void SrListBase::init () + { + while (_cur) remove (); + } + +void SrListBase::elements ( int e ) + { + if ( e>=0 ) { _elements=e; return; } + + _elements = 0; + if ( !_first ) return; + _cur = _first; + do { _elements++; + _cur = _cur->next(); + } while ( _cur!=_first ); + } + +void SrListBase::insert_next ( SrListNode *n ) + { + if (!n) return; + + _elements++; + if ( _cur ) + { _cur->insert_next(n); + _cur = n; + } + else + { n->init(); _first = _cur = n; } + } + +SrListNode* SrListBase::insert_next () + { + insert_next ( (SrListNode*)_man->alloc() ); + return _cur; + } + +void SrListBase::insert_prior ( SrListNode* n ) + { + if (!n) return; + + _elements++; + if ( _cur ) + { _cur->insert_prior(n); + _cur = n; + } + else + { n->init(); _first = _cur = n; } + } + +SrListNode* SrListBase::insert_prior () + { + insert_prior ( (SrListNode*)_man->alloc() ); + return _cur; + } + +void SrListBase::insert_list ( const SrListNode* l ) + { + if ( !l ) return; + + SrListBase nl (_man); + const SrListNode* lcur = l; + + do { nl.insert_next ( (SrListNode*)_man->alloc(lcur) ); + lcur = lcur->next(); + } while ( lcur!=l ); + + if ( _cur ) + _cur->splice ( nl.last() ); // join the two lists + else + _cur = _first = nl._cur->next(); + + nl.leave_data(); // so that elements are not deleted by nl destructor + + _elements += nl._elements; + } + +SrListNode *SrListBase::replace ( SrListNode *n ) + { + if (!_cur) return 0; + if ( _first==_cur ) _first=n; + SrListNode *ret = _cur->replace(n); + _cur = n; + return ret; + } + +SrListNode *SrListBase::extract () + { + if (!_cur) return 0; + + SrListNode *origcur = _cur; + _elements--; + + if ( _first->next()!=_first ) + { _cur = _cur->remove()->next(); + if ( _first==origcur ) _first=_cur; + } + else _first = _cur = 0; + + return origcur; + } + +void SrListBase::remove () + { + void* pt = extract(); + if ( pt ) _man->free(pt); + } + +void SrListBase::sort () + { + _cur = _first; + if ( !_cur || _cur->next()==_cur ) return; + + SrListNode *pos, *min; + while ( _cur->next() != _first ) + { min = pos = _cur; + + do { _cur = _cur->next(); + if ( _man->compare(_cur,min)<0 ) min=_cur; + } while ( _cur!=_first->prior() ); + + if (min!=pos) + { if (pos==_first) _first=min; + pos->swap(min); + } + + _cur = min->next(); + } + + _cur = _first; + } + +int SrListBase::insort ( SrListNode *n ) + { + int c=1; + + if ( _cur ) + { _cur = _first; + do { c = _man->compare ( n, _cur ); + if ( c<=0 ) break; + _cur = _cur->next(); + } while ( _cur!=_first ); + } + + if ( _cur==_first && c<=0 ) _first=n; + + insert_prior(n); + + return c; + } + +bool SrListBase::search ( const SrListNode *n ) + { + _cur = _first; + if ( !_cur || !n ) return false; + int c; + + do { c = _man->compare ( n, _cur ); + if ( c==0 ) return true; + if ( c<0 ) return false; + _cur = _cur->next(); + } while ( _cur!=_first ); + + return false; + } + +bool SrListBase::search_all ( const SrListNode *n ) + { + _cur = _first; + if ( !_cur || !n ) return false; + + do { if ( _man->compare(n,_cur)==0 ) return true; + _cur = _cur->next(); + } while ( _cur!=_first ); + + return false; + } + +void SrListBase::take_data ( SrListBase& l ) + { + if ( this == &l ) return; + init (); + _first = l._first; l._first = 0; + _cur = l._cur; l._cur = 0; + _elements = l._elements; l._elements = 0; + } + +void SrListBase::take_data ( SrListNode* n ) + { + if ( _cur==n || _first==n ) return; + init (); + _first = _cur = n; + } + +SrListNode* SrListBase::leave_data () + { + SrListNode* n = _first; + _first = _cur = 0; + _elements = 0; + return n; + } + +SrOutput& operator<< ( SrOutput& o, const SrListBase& l ) + { + o<<'['; + SrListIteratorBase it(l); + for ( it.first(); it.inrange(); it.next() ) + { l._man->output ( o, it.get() ); + if ( !it.inlast() ) o << ' '; + } + return o<<']'; + } + +//=========================== SrListIteratorBase ================================ + +SrListIteratorBase::SrListIteratorBase ( const SrListBase& l ) + { + _node = 0; + _list = &l; + reset (); + } + +SrListIteratorBase::SrListIteratorBase ( SrListNode* n ) + { + _list = 0; + _node = n; + reset (); + } + +void SrListIteratorBase::reset () + { + if ( _list ) + { _cur = _list->cur(); + _first = _list->first(); + _last = _list->last(); + } + else + { _cur = _first = _last = _node; + if ( _first ) _last = _first->prior(); + } + _rcode = 0; + } + +bool SrListIteratorBase::inrange () + { + if ( _rcode==3 || !_first ) return false; + + if ( _rcode==1 ) { _rcode=2; if ( _first==_last ) _rcode=3; } + else + if ( _rcode==2 && (inlast()||infirst()) ) _rcode=3; + + return true; + } + +//============================== end of file =============================== diff --git a/source/dcdt/se/sr_list.h b/source/dcdt/se/sr_list.h new file mode 100644 index 0000000000..a20ab7b22a --- /dev/null +++ b/source/dcdt/se/sr_list.h @@ -0,0 +1,329 @@ + +# ifndef SR_LIST_H +# define SR_LIST_H + +/** \file sr_list.h + * Manages a circular linked list */ + +# include "sr_class_manager.h" +# include "sr_list_node.h" +# include "sr_output.h" + +/*! \class SrListBase sr_list.h + \brief Base class for SrList + + SrListBase implements methods for managing a double linked + circular list. The user should however use the template + class SrList for an implementation that includes automatic + type casts for the user type. A manager to the user + data is required, which must be a class deriving SrListNode. */ +class SrListBase + { private : + SrListNode* _first; // first element + SrListNode* _cur; // current element + SrClassManagerBase* _man; // manager of user data, that derives SrListNode + int _elements; // the number of elements in the list + + public : + + /*! Initiates an empty list. The class manager must manage a user class + deriving from SrListNode. */ + SrListBase ( SrClassManagerBase* m ); + + /*! Copy constructor. The class manager of l is shared. */ + SrListBase ( const SrListBase& l ); + + /*! Constructor from a given node. the list takes control of the nodes + headed by n. The number of elements is set to zero, but it can be + adjusted with method elements() if required. + The class manager must be compatible with the derived type of n. */ + SrListBase ( SrListNode* n, SrClassManagerBase* m ); + + /*! The destructor deletes all nodes from memory. */ + ~SrListBase (); + + /*! Returns the manager of the user data */ + SrClassManagerBase* class_manager() const { return _man; } + + /*! Deletes all elements of the list. */ + void init (); + + /*! Returns true if there is no elements in the list. */ + bool empty () const { return _first? false:true; } + + /*! Changes the internal elements counter maintained during list manipulations. + If e<0, will count all nodes to correctly update the internal counter, + otherwise will just take the new amount e. Having a wrong value has no + consequences. */ + void elements ( int e ); + + /*! Returns the current number of elements in the list. */ + int elements () const { return _elements; } + + /*! Returns the current element being pointed by the SrListBase internal pointer. */ + SrListNode* cur () const { return _cur; } + + /*! Sets the internal current element pointer to n. It is the user responsability + to ensure that n makes part of the list controlled by SrListBase. */ + void cur ( SrListNode* n ) { _cur = n; } + + /*! Returns the first element of the list. */ + SrListNode* first () const { return _first; } + + /*! Sets the internal first element pointer to n. Its the user responsability + to ensure that n makes part of the list controlled by SrListBase */ + void first ( SrListNode* n ) { _first = n; } + + /*! Returns the last element of the list. */ + SrListNode* last () const { return _first? _first->prior():0; } + + /*! Sets the last element of the list to be n by adjusting the internal first pointer. + It is the user responsability to ensure that n makes part of the list controlled + by SrListBase. (as the list is circular, only the first pointer is mantained). */ + void last ( SrListNode* n ) { _first = n->next(); } + + /*! Puts the current position cur() pointing to the first node. */ + void gofirst () { _cur=_first; } + + /*! Puts the current position cur() pointing to the last node. */ + void golast () { if (_first) _cur=_first->prior(); } + + /*! Puts the current position cur() pointing to the next node curnext(); + Attention: this method cannot be called if the list is empty! */ + void gonext () { _cur=_cur->next(); } + + /*! Puts the current position cur() pointing to the prior node curprior(); + Attention: this method cannot be called if the list is empty! */ + void goprior () { _cur=_cur->prior(); } + + /*! Returns the next element of the current position cur(). + Attention: this method cannot be called if the list is empty! */ + SrListNode* curnext () const { return _cur->next(); } + + /*! Returns the prior element of the current position cur(). + Attention: this method cannot be called if the list is empty! */ + SrListNode* curprior () const { return _cur->prior(); } + + /*! This method calls gonext(), and returns true iff the last element was not reached. + Use this to iterate over a hole list, like : \code + if ( !l.empty() ) + { l.gofirst(); + do { l.cur()->do_something(); + } while ( l.notlast() ); + } \endcode + Attention: this method cannot be called if the list is empty! */ + bool notlast () { gonext(); return _cur==_first? false:true; } + + /*! This method calls goprior(), and returns true iff the first element was not reached. + Use this to iterate over a hole list in backwards, like : \code + if ( !l.empty() ) + { l.golast(); + do { l.cur()->do_something(); + } while ( l.notfirst() ); + } \endcode + Attention: this method cannot be called if the list is empty! */ + bool notfirst () { goprior(); return _cur->next()==_first? false:true; } + + /*! Returns true iff the current position is pointing to the last element. + Attention: this method cannot be called if the list is empty! */ + bool inlast () const { return _cur->next()==_first? true:false; } + + /*! Returns true iff the current position is pointing to the first element, + or if the list is empty. */ + bool infirst () const { return _cur==_first? true:false; } + + /*! Extracts the first element of the list, the cur position is set to be + the next element. */ + SrListNode* pop_front () { _cur=_first; return extract(); } + + /*! Extracts the last element of the list, the cur position is left as the + first element. */ + SrListNode* pop_back () { golast(); return extract(); } + + /*! Inserts n before the first element and makes n be the first element of + the list and also the current one. */ + void push_front ( SrListNode* n ) { _cur=_first; insert_prior(n); _first=_cur; } + + /*! Inserts n after the last element and makes n be the last element of the list + and also the current one. */ + void push_back ( SrListNode* n ) { golast(); insert_next(n); } + + /*! Inserts n after the current element. The current element becomes n. */ + void insert_next ( SrListNode* n ); + + /*! Allocates a new element, inserting it after the current one. + The current element becomes the new one, and is returned. */ + SrListNode* insert_next (); + + /*! Inserts n prior to the current element. The current element becomes n. */ + void insert_prior ( SrListNode* n ); + + /*! Allocates a new element, inserting it after the current one. + The current element becomes the new one, and is returned. */ + SrListNode* insert_prior (); + + /*! Inserts a copy of list l after the cur position, list l stays unchanged. */ + void insert_list ( const SrListBase& l ) { insert_list(l._first); } + + /*! Inserts a copy of the list pointed by l after the cur position, list l stays unchanged. */ + void insert_list ( const SrListNode *l ); + + /*! Replaces the current element by n, only swaping their pointers. + The original current element is not deleted but returned. */ + SrListNode* replace ( SrListNode* n ); + + /*! Extract the current element and return it (without deleting it). If the list + is empty, 0 is returned. The current element becomes the next one, and the + same for the first element if it is removed. */ + SrListNode* extract (); + + /*! Removes the current element calling extract() and deletes it. */ + void remove (); + + /*! Does a selection sort. The current position stays at the first element. */ + void sort (); + + /*! Inserts the node in its sorted position. Return the last comparison result: + >0 if it was inserted as the last element in the list, <0 if it was inserted + in the middle of the list, and 0 if it was inserted just before a duplicated + element. */ + int insort ( SrListNode* n ); + + /*! Linear search considering that the list is sorted. + Current position will point the found element if true is returned. */ + bool search ( const SrListNode *n ); + + /*! Linear search that will test all elements in the list, case needed for + when the list is not sorted. + Current position will point the found element if true is returned. */ + bool search_all ( const SrListNode *n ); + + /*! Get control of the nodes in list l, and set l to be an empty list. + The data manager of l and SrList must be of the same type. */ + void take_data ( SrListBase& l ); + + /*! Get control of the nodes headed by n. The number of elements is set to + zero, but it can be adjusted with method elements() if required. + The data manager of SrList must be compatible with the derived n type. */ + void take_data ( SrListNode* n ); + + /*! Returns the first element of the list, and set the list to be empty. */ + SrListNode* leave_data (); + + /*! Outputs the list in the format: [e1 e2 en ]. */ + friend SrOutput& operator<< ( SrOutput& o, const SrListBase& l ); + }; + +/*! \class SrList sr_list.h + \brief Manages a circular linked list of derived classes X of SrLink + + SrList defines automatic type casts to the user type, which must + derive SrListNode. For documentation of the methods + see the documentation of the base class SrListBase methods. */ +template +class SrList : public SrListBase + { public : + /*! Default constructor that automatically creates a SrClassManager. */ + SrList () : SrListBase ( new SrClassManager ) {} + + /*! Constructor with a given class manager. */ + SrList ( SrClassManagerBase* m ) : SrListBase ( m ) {} + + /*! Copy constructor. Inititates the list as a copy of l, + duplicating all elements and sharing the class manager. */ + SrList ( const SrList& l ) : SrListBase ( l ) {} + + /*! Constructor from a given node. The list takes control of the nodes + headed by n. The number of elements is set to zero, but it can be + adjusted later with method elements() if required. */ + SrList ( X* n ) : SrListBase ( n, new SrClassManager ) {} + + X* cur () const { return (X*)SrListBase::cur(); } + void cur ( X* n ) { SrListBase::cur((SrListNode*)n); } + X* first () const { return (X*)SrListBase::first(); } + void first ( X* n ) { SrListBase::first((SrListNode*)n); } + X* last () { return (X*) SrListBase::last(); } + void last ( X* n ) { SrListBase::last((SrListNode*)n); } + X* curnext () const { return (X*)SrListBase::curnext(); } + X* curprior () const { return (X*)SrListBase::curprior(); } + X* pop_front () { return (X*)SrListBase::pop_front(); } + X* pop_back () { return (X*)SrListBase::pop_back(); } + void push_front ( X* n ) { SrListBase::push_front(n); } + void push_back ( X* n ) { SrListBase::push_back(n); } + void insert_next ( X* n ) { SrListBase::insert_next(n); } + X* insert_next () { return (X*)SrListBase::insert_next(); } + void insert_prior ( X* n ) { SrListBase::insert_prior(n); } + X* insert_prior () { return (X*)SrListBase::insert_prior(); } + X* replace ( X* n ) { return (X*)SrListBase::replace(n); } + X* extract () { return (X*)SrListBase::extract(); } + void operator= ( const SrList& l ) { init(); insert_list(l); } + }; + +/*! Base class for iterating over lists. */ +class SrListIteratorBase + { private : + SrListNode* _cur; + SrListNode* _first; + SrListNode* _last; + char _rcode; + const SrListBase* _list; + SrListNode* _node; + + public : + /*! Constructor */ + SrListIteratorBase ( const SrListBase& l ); + + /*! Constructor */ + SrListIteratorBase ( SrListNode* n ); + + /*! Returns the current element being pointed by the iterator */ + SrListNode* get () const { return _cur; } + + /*! Returns the first element being pointed by the iterator */ + SrListNode* getfirst () const { return _first; } + + /*! Returns the last element being pointed by the iterator */ + SrListNode* getlast () const { return _last; } + + /*! Must be called each time the associate list is changed */ + void reset (); + + /*! Points the iterator to the first element. */ + void first () { _cur=_first; _rcode=1; } + + /*! Points the iterator to the last element. */ + void last () { _cur=_last; _rcode=1; } + + /*! Advances the current position of the iterator of one position */ + void next () { _cur=_cur->next(); } + + /*! Walk back the current position of the iterator of one position */ + void prior () { _cur=_cur->prior(); } + + /*! This method only makes sense when called in a loop, like in: the following + for ( it.first(); it.inrange(); it.next() ) { ... } + or for ( it.last(); it.inrange(); it.prior() ) { ... } */ + bool inrange (); + + /*! Returns true if the current position is pointing to the last element. */ + bool inlast () const { return _cur==_last; } + + /*! Returns true if the current position is pointing to the first element, + or if the list is empty. */ + bool infirst () const { return _cur==_first; } + }; + +/*! Derives SrListIteratorBase providing correct type casts for the user type */ +template +class SrListIterator : public SrListIteratorBase + { public : + SrListIterator ( const SrList& l ) : SrListIteratorBase(l) {} + SrListIterator ( X* n ) : SrListIteratorBase((SrListNode*)n) {} + X* get () { return (X*)SrListIteratorBase::get(); } + X* operator-> () { return (X*)SrListIteratorBase::get(); } + }; + +//============================== end of file =============================== + +# endif // SR_LIST_H + diff --git a/source/dcdt/se/sr_list_node.cpp b/source/dcdt/se/sr_list_node.cpp new file mode 100644 index 0000000000..24a5c19e50 --- /dev/null +++ b/source/dcdt/se/sr_list_node.cpp @@ -0,0 +1,85 @@ +#include "precompiled.h" +# include "sr_list_node.h" + +//=============================== SrListNode ================================ + +SrListNode *SrListNode::remove_next () + { + SrListNode *n = _next; + _next = _next->_next; + _next->_next->_prior = this; + return n; + } + +SrListNode *SrListNode::remove_prior () + { + SrListNode *n = _prior; + _prior = _prior->_prior; + _prior->_prior->_next = this; + return n; + } + +SrListNode *SrListNode::remove () + { + _next->_prior = _prior; + _prior->_next = _next; + return this; + } + +SrListNode *SrListNode::replace ( SrListNode *n ) + { + _next->_prior = n; + _prior->_next = n; + n->_next = _next; + n->_prior = _prior; + return this; + } + +SrListNode *SrListNode::insert_next ( SrListNode *n ) + { + n->_next = _next; + n->_next->_prior = n; + n->_prior = this; + _next = n; + return n; + } + +SrListNode *SrListNode::insert_prior ( SrListNode *n ) + { + n->_prior = _prior; + _prior->_next = n; + n->_next = this; + _prior = n; + return n; + } + +void SrListNode::splice ( SrListNode *n ) + { + _next->_prior = n; + n->_next->_prior = this; + SrListNode *nxt=_next; + _next = n->_next; + n->_next = nxt; + +/* This has the same result as: + SrListNode *tmp; + SR_SWAP ( _next->_prior, n->_next->_prior ); + SR_SWAP ( _next, n->_next ); +*/ + } + +void SrListNode::swap ( SrListNode *n ) + { + SrListNode *nn=n->_next, *p=_prior; + + if ( _next!=n ) { _prior=n->_prior; n->_next=_next; } else { _prior=n; n->_next=this; } + if ( nn!=this ) { _next=nn; n->_prior=p; } else { _next=n; n->_prior=this; } + + _prior->_next=this; + _next->_prior=this; + n->_prior->_next=n; + n->_next->_prior=n; + } + +//============================== end of file =============================== + diff --git a/source/dcdt/se/sr_list_node.h b/source/dcdt/se/sr_list_node.h new file mode 100644 index 0000000000..801f58e6b2 --- /dev/null +++ b/source/dcdt/se/sr_list_node.h @@ -0,0 +1,82 @@ + +# ifndef SR_LIST_NODE_H +# define SR_LIST_NODE_H + +/** \file sr_list_node.h + * A double linked circular list node */ + +# include "sr.h" + +/*! \class SrListNode sr_list_node.h + \brief Elements of SrList must derive SrListNode + + SrListNode is a node of a double linked circular list. + It does not have virtual destructors + in order to save the space overhead of virtual functions. + + We do not use splice() to insert/remove, and this implies: + 1. A node that is inserted must not be inside any list. + 2. A node that is removed do not form a single circular list (call init() after for this) + 3. But insert / remove methods are faster. + + \code + splice guide : + l->remove_next() == l->splice(l->next()); + l->remove_prior() == l->prior->splice(l->prior()->prior()); + l->remove() == l->splice(l->prior()); + splice(a,b) implementation : + { swap(a->next()->prior(),b->next()->prior()); swap(a->next(),b->next()); } + Example of a splice usage: + l1: A->B->C, l2: X->Y->Z <=> splice(A,X) <=> l3: A->Y->Z->X->B->C + \endcode */ +class SrListNode + { + private : + SrListNode* _next, *_prior; + + public : + + /*! Constructs a node n where the next and prior pointers refers to n. */ + SrListNode () { _next=_prior=this; } + + /*! Returns the next node of the list. */ + SrListNode* next () const { return _next; } + + /*! Returns the prior node of the list. */ + SrListNode* prior () const { return _prior; } + + /*! Makes the next and prior pointers refers to itself. */ + void init () { _next=_prior=this; } + + /*! Returns true if the node is only, ie, iff next==this. */ + bool only () const { return _next==this? true:false; } + + /*! Removes the next node from the list and return it. */ + SrListNode* remove_next (); + + /*! Removes the prior node from the list and return it. */ + SrListNode* remove_prior (); + + /*! Removes itself from the list and returns itself. */ + SrListNode* remove (); + + /*! Replaces itself by n on the list, and returns itself. */ + SrListNode* replace ( SrListNode* n ); + + /*! Inserts n in the list after itself and returns n. */ + SrListNode* insert_next ( SrListNode* n ); + + /*! Inserts n in the list before itself and returns n. */ + SrListNode* insert_prior ( SrListNode* n ); + + /*! Operator splice(), does the same as: + swap(a->next->prior,b->next->prior); swap(a->next,b->next); */ + void splice ( SrListNode* n ); + + /*! Swaps the position of itself and n in the list by readjusting their pointers. */ + void swap ( SrListNode* n ); + }; + +//============================== end of file =============================== + +# endif // SR_LIST_NODE_H diff --git a/source/dcdt/se/sr_mat.cpp b/source/dcdt/se/sr_mat.cpp new file mode 100644 index 0000000000..a2fd8d1190 --- /dev/null +++ b/source/dcdt/se/sr_mat.cpp @@ -0,0 +1,591 @@ +#include "precompiled.h" +# include + +# include "sr_mat.h" +//# include + +//================================== Static Data =================================== + +const SrMat SrMat::null ( 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0 ); + +const SrMat SrMat::id ( 1.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0 ); + +# define E11 e[0] +# define E12 e[1] +# define E13 e[2] +# define E14 e[3] +# define E21 e[4] +# define E22 e[5] +# define E23 e[6] +# define E24 e[7] +# define E31 e[8] +# define E32 e[9] +# define E33 e[10] +# define E34 e[11] +# define E41 e[12] +# define E42 e[13] +# define E43 e[14] +# define E44 e[15] + +//==================================== SrMat ======================================== + + +SrMat::SrMat ( const float *p ) + { + SR_ASSERT ( p ); + set ( p ); + } + +SrMat::SrMat ( const double *p ) + { + SR_ASSERT ( p ); + set ( p ); + } + +SrMat::SrMat ( float a, float b, float c, float d, + float e, float f, float g, float h, + float i, float j, float k, float l, + float m, float n, float o, float p ) + { + setl1 ( a, b, c, d ); + setl2 ( e, f, g, h ); + setl3 ( i, j, k, l ); + setl4 ( m, n, o, p ); + } + +void SrMat::set ( const float *p ) + { + SR_ASSERT ( p ); + setl1 ( p[0], p[1], p[2], p[3] ); + setl2 ( p[4], p[5], p[6], p[7] ); + setl3 ( p[8], p[9], p[10], p[11] ); + setl4 ( p[12], p[13], p[14], p[15] ); + } + +void SrMat::set ( const double *p ) + { + SR_ASSERT ( p ); + setl1 ( (float)p[0], (float)p[1], (float)p[2], (float)p[3] ); + setl2 ( (float)p[4], (float)p[5], (float)p[6], (float)p[7] ); + setl3 ( (float)p[8], (float)p[9], (float)p[10], (float)p[11] ); + setl4 ( (float)p[12], (float)p[13], (float)p[14], (float)p[15] ); + } + +void SrMat::round ( float epsilon ) + { + int i; + for ( i=0; i<16; i++ ) + if ( e[i]>=-epsilon && e[i]<=epsilon ) e[i]=0; + } + +void SrMat::transpose () + { + float tmp; + SR_SWAP ( E12, E21 ); + SR_SWAP ( E13, E31 ); + SR_SWAP ( E14, E41 ); + SR_SWAP ( E23, E32 ); + SR_SWAP ( E24, E42 ); + SR_SWAP ( E34, E43 ); + } + +void SrMat::transpose3x3 () + { + float tmp; + SR_SWAP ( E12, E21 ); + SR_SWAP ( E13, E31 ); + SR_SWAP ( E23, E32 ); + SR_SWAP ( E34, E43 ); + } + +void SrMat::translation ( float tx, float ty, float tz ) + { + setl1 ( 1.0, 0.0, 0.0, 0.0 ); + setl2 ( 0.0, 1.0, 0.0, 0.0 ); + setl3 ( 0.0, 0.0, 1.0, 0.0 ); + setl4 ( tx, ty, tz, 1.0 ); + } + +void SrMat::left_combine_translation ( const SrVec &v ) + { + E41 = v.x*E11 + v.y*E21 + v.z*E31; + E42 = v.x*E12 + v.y*E22 + v.z*E32; + E43 = v.x*E13 + v.y*E23 + v.z*E33; + } + +void SrMat::right_combine_translation ( const SrVec &v ) + { + E41 += v.x; + E42 += v.y; + E43 += v.z; + } + +void SrMat::combine_scale ( float sx, float sy, float sz ) + { + E11*=sx; E12*=sy; E13*=sy; + E21*=sx; E22*=sy; E23*=sy; + E31*=sx; E32*=sy; E33*=sy; + } + +void SrMat::scale ( float sx, float sy, float sz ) + { + setl1 ( sx, 0.0, 0.0, 0.0 ); + setl2 ( 0.0, sy, 0.0, 0.0 ); + setl3 ( 0.0, 0.0, sz, 0.0 ); + setl4 ( 0.0, 0.0, 0.0, 1.0 ); + } + +void SrMat::rotx ( float sa, float ca ) + { + setl1 ( 1.0, 0.0, 0.0, 0.0 ); + setl2 ( 0.0, ca, sa, 0.0 ); + setl3 ( 0.0, -sa, ca, 0.0 ); + setl4 ( 0.0, 0.0, 0.0, 1.0 ); + } + +void SrMat::roty ( float sa, float ca ) + { + setl1 ( ca, 0.0, -sa, 0.0 ); + setl2 ( 0.0, 1.0, 0.0, 0.0 ); + setl3 ( sa, 0.0, ca, 0.0 ); + setl4 ( 0.0, 0.0, 0.0, 1.0 ); + } + +void SrMat::rotz ( float sa, float ca ) + { + setl1 ( ca, sa, 0.0, 0.0 ); + setl2 ( -sa, ca, 0.0, 0.0 ); + setl3 ( 0.0, 0.0, 1.0, 0.0 ); + setl4 ( 0.0, 0.0, 0.0, 1.0 ); + } + +void SrMat::rotx ( float radians ) + { + rotx ( sinf(radians), cosf(radians) ); + } + +void SrMat::roty ( float radians ) + { + roty ( sinf(radians), cosf(radians) ); + } + +void SrMat::rotz ( float radians ) + { + rotz ( sinf(radians), cosf(radians) ); + } + +void SrMat::rot ( const SrVec &vec, float sa, float ca ) + { + double x, y, z, norm; + double xx, yy, zz, xy, yz, zx, xs, ys, zs, oc; + + x=vec.x; y=vec.y; z=vec.z; + + norm = x*x + y*y + z*z; + if ( norm!=1 ) + { norm = sqrt(norm); + if (norm!=0) { x/=norm; y/=norm; z/=norm; } + } + + // The following line was added to invert the sign of the given angle + // in order to cope with the v*M OpenGL way, that is contrary to M*v of + // the standard mathematical notation. + sa*=-1.0; + + xx = x * x; yy = y * y; zz = z * z; + xy = x * y; yz = y * z; zx = z * x; + xs = x * sa; ys = y * sa; zs = z * sa; + + oc = 1.0 - ca; + + setl1 ( float(oc * xx + ca), float(oc * xy - zs), float(oc * zx + ys), 0 ); + setl2 ( float(oc * xy + zs), float(oc * yy + ca), float(oc * yz - xs), 0 ); + setl3 ( float(oc * zx - ys), float(oc * yz + xs), float(oc * zz + ca), 0 ); + setl4 ( 0, 0, 0, 1 ); + } + +void SrMat::rot ( const SrVec &vec, float radians ) + { + rot ( vec, sinf(radians), cosf(radians) ); + } + +void SrMat::rot ( const SrVec& from, const SrVec& to ) + { + SrVec axis; + axis = cross ( from, to ); + rot ( cross(from,to), angle(from,to) ); + } + +void SrMat::projxy ( SrVec p1, SrVec p2, SrVec p3 ) + { + # define PROJERR(d) { sr_out.warning ("degenerated input(%d) in SrMat::projxy()!\n",d); return; } + + SrMat m(SrMat::NotInitialized); + float ca, sa, v; + + translation ( -p1.x, -p1.y, -p1.z ); // p1 goes to origin + p2=p2-p1; p3=p3-p1; + + v = sqrtf(p2.y*p2.y + p2.z*p2.z); if (v==0.0) PROJERR(1); + + ca=p2.z/v; sa=p2.y/v; // rotate by x : p1p2 to xz + m.rotx(ca,sa); p2.rotx(ca,sa); p3.rotx(ca,sa); + mult ( *this, m ); + + v = sqrtf(p2.x*p2.x + p2.z*p2.z); if (v==0.0) PROJERR(2); + + ca=p2.x/v; sa=p2.z/v; // rotate by y: p1p2 to x axis + m.roty(ca,sa); p3.roty(ca,sa); + mult ( *this, m ); + + v = sqrtf(p3.y*p3.y + p3.z*p3.z); if (v==0.0) PROJERR(3); + + ca=p3.y/v; sa=p3.z/v; // rotate by x: p3 to xy + m.rotx(ca,-sa); + mult ( *this, m ); + + # undef PROJERR + } + +void SrMat::perspective ( float fovy, float aspect, float znear, float zfar ) + { + float top = znear * tanf ( fovy/2 ); + + float left = -top * aspect; + float right = top * aspect; + + float w = right-left; // width + float h = 2.0f*top; // height + float d = zfar-znear; // depth + + setl1 ( (2*znear)/w, 0, 0, 0 ); + setl2 ( 0, (2*znear)/h, 0, 0 ); + setl3 ( (right+left)/w, 0, (zfar+znear)/-d, -1 ); + setl4 ( 0, 0, (2*zfar*znear)/-d, 0 ); + +/* + GLdouble ymax = (GLdouble)znear * tan ( fovy/2 ); // same as fovy + + glFrustum ( (GLdouble) ((-ymax)*(GLdouble)aspect), // xmin + (GLdouble) (( ymax)*(GLdouble)aspect), // xmax + (GLdouble) (-ymax), // ymin + (GLdouble) ymax, + (GLdouble) znear, + (GLdouble) zfar + ); +*/ + } + +void SrMat::look_at ( const SrVec& eye, const SrVec& center, const SrVec& up ) + { + SrVec z = eye-center; z.normalize(); + SrVec x = cross ( up, z ); + SrVec y = cross ( z, x ); + + x.normalize(); + y.normalize(); + + setl1 ( x.x, y.x, z.x, 0.0 ); + setl2 ( x.y, y.y, z.y, 0.0 ); + setl3 ( x.z, y.z, z.z, 0.0 ); + setl4 ( 0, 0, 0, 1.0f ); + + x = eye*-1.0f; + left_combine_translation ( x ); + } + +void SrMat::inverse ( SrMat &inv ) const + { + float d = det(); + if (d==0.0) return; + d = 1.0f/d; + + float m12 = E21*E32 - E22*E31; + float m13 = E21*E33 - E23*E31; + float m14 = E21*E34 - E24*E31; + float m23 = E22*E33 - E23*E32; + float m24 = E22*E34 - E24*E32; + float m34 = E23*E34 - E24*E33; + inv.E11 = (E42*m34 - E43*m24 + E44*m23) * d; + inv.E21 = (E43*m14 - E41*m34 - E44*m13) * d; + inv.E31 = (E41*m24 - E42*m14 + E44*m12) * d; + inv.E41 = (E42*m13 - E41*m23 - E43*m12) * d; + inv.E14 = (E13*m24 - E12*m34 - E14*m23) * d; + inv.E24 = (E11*m34 - E13*m14 + E14*m13) * d; + inv.E34 = (E12*m14 - E11*m24 - E14*m12) * d; + inv.E44 = (E11*m23 - E12*m13 + E13*m12) * d; + + m12 = E11*E42 - E12*E41; + m13 = E11*E43 - E13*E41; + m14 = E11*E44 - E14*E41; + m23 = E12*E43 - E13*E42; + m24 = E12*E44 - E14*E42; + m34 = E13*E44 - E14*E43; + inv.E12 = (E32*m34 - E33*m24 + E34*m23) * d; + inv.E22 = (E33*m14 - E31*m34 - E34*m13) * d; + inv.E32 = (E31*m24 - E32*m14 + E34*m12) * d; + inv.E42 = (E32*m13 - E31*m23 - E33*m12) * d; + inv.E13 = (E23*m24 - E22*m34 - E24*m23) * d; + inv.E23 = (E21*m34 - E23*m14 + E24*m13) * d; + inv.E33 = (E22*m14 - E21*m24 - E24*m12) * d; + inv.E43 = (E21*m23 - E22*m13 + E23*m12) * d; + } + +SrMat SrMat::inverse () const + { + SrMat inv; + inverse ( inv ); + return inv; + } + +float SrMat::det () const + { + float m12 = E21*E32 - E22*E31; + float m13 = E21*E33 - E23*E31; + float m14 = E21*E34 - E24*E31; + float m23 = E22*E33 - E23*E32; + float m24 = E22*E34 - E24*E32; + float m34 = E23*E34 - E24*E33; + float d1 = E12*m34 - E13*m24 + E14*m23; + float d2 = E11*m34 - E13*m14 + E14*m13; + float d3 = E11*m24 - E12*m14 + E14*m12; + float d4 = E11*m23 - E12*m13 + E13*m12; + return -E41*d1 + E42*d2 - E43*d3 + E44*d4; + } + +float SrMat::det3x3 () const + { + return E11 * (E22*E33-E23*E32) + + E12 * (E23*E31-E21*E33) + + E13 * (E21*E32-E22*E31); + } + +float SrMat::norm2 () const + { + return E11*E11 + E12*E12 + E13*E13 + E14*E14 + + E21*E21 + E22*E22 + E23*E23 + E24*E24 + + E31*E31 + E32*E32 + E33*E33 + E34*E34 + + E41*E41 + E42*E42 + E43*E43 + E44*E44; + } + +float SrMat::norm () const + { + return sqrtf ( norm2() ); + } + +void SrMat::mult ( const SrMat &m1, const SrMat &m2 ) + { + SrMat* m = (this==&m1||this==&m2)? new SrMat(SrMat::NotInitialized) : this; + + m->setl1 ( m1.E11*m2.E11 + m1.E12*m2.E21 + m1.E13*m2.E31 + m1.E14*m2.E41, + m1.E11*m2.E12 + m1.E12*m2.E22 + m1.E13*m2.E32 + m1.E14*m2.E42, + m1.E11*m2.E13 + m1.E12*m2.E23 + m1.E13*m2.E33 + m1.E14*m2.E43, + m1.E11*m2.E14 + m1.E12*m2.E24 + m1.E13*m2.E34 + m1.E14*m2.E44 ); + + m->setl2 ( m1.E21*m2.E11 + m1.E22*m2.E21 + m1.E23*m2.E31 + m1.E24*m2.E41, + m1.E21*m2.E12 + m1.E22*m2.E22 + m1.E23*m2.E32 + m1.E24*m2.E42, + m1.E21*m2.E13 + m1.E22*m2.E23 + m1.E23*m2.E33 + m1.E24*m2.E43, + m1.E21*m2.E14 + m1.E22*m2.E24 + m1.E23*m2.E34 + m1.E24*m2.E44 ); + + m->setl3 ( m1.E31*m2.E11 + m1.E32*m2.E21 + m1.E33*m2.E31 + m1.E34*m2.E41, + m1.E31*m2.E12 + m1.E32*m2.E22 + m1.E33*m2.E32 + m1.E34*m2.E42, + m1.E31*m2.E13 + m1.E32*m2.E23 + m1.E33*m2.E33 + m1.E34*m2.E43, + m1.E31*m2.E14 + m1.E32*m2.E24 + m1.E33*m2.E34 + m1.E34*m2.E44 ); + + m->setl4 ( m1.E41*m2.E11 + m1.E42*m2.E21 + m1.E43*m2.E31 + m1.E44*m2.E41, + m1.E41*m2.E12 + m1.E42*m2.E22 + m1.E43*m2.E32 + m1.E44*m2.E42, + m1.E41*m2.E13 + m1.E42*m2.E23 + m1.E43*m2.E33 + m1.E44*m2.E43, + m1.E41*m2.E14 + m1.E42*m2.E24 + m1.E43*m2.E34 + m1.E44*m2.E44 ); + + if ( m!=this ) { *this=*m; delete m; } + } + +void SrMat::add ( const SrMat &m1, const SrMat &m2 ) + { + setl1 ( m1.E11+m2.E11, m1.E12+m2.E12, m1.E13+m2.E13, m1.E14+m2.E14 ); + setl2 ( m1.E21+m2.E21, m1.E22+m2.E22, m1.E23+m2.E23, m1.E24+m2.E24 ); + setl3 ( m1.E31+m2.E31, m1.E32+m2.E32, m1.E33+m2.E33, m1.E34+m2.E34 ); + setl4 ( m1.E41+m2.E41, m1.E42+m2.E42, m1.E43+m2.E43, m1.E44+m2.E44 ); + } + +void SrMat::sub ( const SrMat &m1, const SrMat &m2 ) + { + setl1 ( m1.E11-m2.E11, m1.E12-m2.E12, m1.E13-m2.E13, m1.E14-m2.E14 ); + setl2 ( m1.E21-m2.E21, m1.E22-m2.E22, m1.E23-m2.E23, m1.E24-m2.E24 ); + setl3 ( m1.E31-m2.E31, m1.E32-m2.E32, m1.E33-m2.E33, m1.E34-m2.E34 ); + setl4 ( m1.E41-m2.E41, m1.E42-m2.E42, m1.E43-m2.E43, m1.E44-m2.E44 ); + } + +//================================= friends ======================================== + +float dist ( const SrMat &a, const SrMat &b ) + { + return sqrtf ( dist2(a,b) ); + } + +float dist2 ( const SrMat &a, const SrMat &b ) + { + return (a-b).norm2(); + } + +//================================= operators ======================================== + +void SrMat::operator *= ( float r ) + { + E11*=r; E12*=r; E13*=r; E14*=r; + E21*=r; E22*=r; E23*=r; E24*=r; + E31*=r; E32*=r; E33*=r; E34*=r; + E41*=r; E42*=r; E43*=r; E44*=r; + } + +void SrMat::operator *= ( const SrMat &m ) + { + SrMat* t = (this==&m)? new SrMat(SrMat::NotInitialized) : this; + + t->setl1 ( E11*m.E11 + E12*m.E21 + E13*m.E31 + E14*m.E41, + E11*m.E12 + E12*m.E22 + E13*m.E32 + E14*m.E42, + E11*m.E13 + E12*m.E23 + E13*m.E33 + E14*m.E43, + E11*m.E14 + E12*m.E24 + E13*m.E34 + E14*m.E44 ); + + t->setl2 ( E21*m.E11 + E22*m.E21 + E23*m.E31 + E24*m.E41, + E21*m.E12 + E22*m.E22 + E23*m.E32 + E24*m.E42, + E21*m.E13 + E22*m.E23 + E23*m.E33 + E24*m.E43, + E21*m.E14 + E22*m.E24 + E23*m.E34 + E24*m.E44 ); + + t->setl3 ( E31*m.E11 + E32*m.E21 + E33*m.E31 + E34*m.E41, + E31*m.E12 + E32*m.E22 + E33*m.E32 + E34*m.E42, + E31*m.E13 + E32*m.E23 + E33*m.E33 + E34*m.E43, + E31*m.E14 + E32*m.E24 + E33*m.E34 + E34*m.E44 ); + + t->setl4 ( E41*m.E11 + E42*m.E21 + E43*m.E31 + E44*m.E41, + E41*m.E12 + E42*m.E22 + E43*m.E32 + E44*m.E42, + E41*m.E13 + E42*m.E23 + E43*m.E33 + E44*m.E43, + E41*m.E14 + E42*m.E24 + E43*m.E34 + E44*m.E44 ); + + if ( t!=this ) { *this=*t; delete t; } + +/* old non optimized version: + SrMat mat(SrMat::NotInitialized); + mat.mult ( *this, m ); + *this = mat; +*/ + } + +void SrMat::operator += ( const SrMat &m ) + { + E11+=m.E11; E12+=m.E12; E13+=m.E13; E14+=m.E14; + E21+=m.E21; E22+=m.E22; E23+=m.E23; E24+=m.E24; + E31+=m.E31; E32+=m.E32; E33+=m.E33; E34+=m.E34; + E41+=m.E41; E42+=m.E42; E43+=m.E43; E44+=m.E44; + + } + +SrMat operator * ( const SrMat &m, float r ) + { + return SrMat ( m.E11*r, m.E12*r, m.E13*r, m.E14*r, + m.E21*r, m.E22*r, m.E23*r, m.E24*r, + m.E31*r, m.E32*r, m.E33*r, m.E34*r, + m.E41*r, m.E42*r, m.E43*r, m.E44*r ); + } + +SrMat operator * ( float r, const SrMat &m ) + { + return SrMat ( m.E11*r, m.E12*r, m.E13*r, m.E14*r, + m.E21*r, m.E22*r, m.E23*r, m.E24*r, + m.E31*r, m.E32*r, m.E33*r, m.E34*r, + m.E41*r, m.E42*r, m.E43*r, m.E44*r ); + } + +SrVec operator * ( const SrMat &m, const SrVec &v ) + { + SrVec r ( m.E11*v.x + m.E12*v.y + m.E13*v.z + m.E14, + m.E21*v.x + m.E22*v.y + m.E23*v.z + m.E24, + m.E31*v.x + m.E32*v.y + m.E33*v.z + m.E34 ); + + float w = m.E41*v.x + m.E42*v.y + m.E43*v.z + m.E44; + if ( w!=0.0 && w!=1.0 ) r/=w; + return r; + } + +SrVec operator * ( const SrVec &v, const SrMat &m ) + { + SrVec r ( m.E11*v.x + m.E21*v.y + m.E31*v.z + m.E41, + m.E12*v.x + m.E22*v.y + m.E32*v.z + m.E42, + m.E13*v.x + m.E23*v.y + m.E33*v.z + m.E43 ); + + float w = m.E14*v.x + m.E24*v.y + m.E34*v.z + m.E44; + if ( w!=0.0 && w!=1.0 ) r/=w; + return r; + } + +SrMat operator * ( const SrMat &m1, const SrMat &m2 ) + { + SrMat mat(SrMat::NotInitialized); + mat.mult ( m1, m2 ); + return mat; + } + +SrMat operator + ( const SrMat &m1, const SrMat &m2 ) + { + SrMat mat(SrMat::NotInitialized); + mat.add ( m1, m2 ); + return mat; + } + +SrMat operator - ( const SrMat &m1, const SrMat &m2 ) + { + SrMat mat(SrMat::NotInitialized); + mat.sub ( m1, m2 ); + return mat; + } + +bool operator == ( const SrMat &m1, const SrMat &m2 ) + { + return m1.E11==m2.E11 && m1.E12==m2.E12 && m1.E13==m2.E13 && m1.E14==m2.E14 && + m1.E21==m2.E21 && m1.E22==m2.E22 && m1.E23==m2.E23 && m1.E24==m2.E24 && + m1.E31==m2.E31 && m1.E32==m2.E32 && m1.E33==m2.E33 && m1.E34==m2.E34 && + m1.E41==m2.E41 && m1.E42==m2.E42 && m1.E43==m2.E43 && m1.E44==m2.E44 ? true:false; + } + +bool operator != ( const SrMat &m1, const SrMat &m2 ) + { + return m1.E11!=m2.E11 || m1.E12!=m2.E12 || m1.E13!=m2.E13 || m1.E14!=m2.E14 || + m1.E21!=m2.E21 || m1.E22!=m2.E22 || m1.E23!=m2.E23 || m1.E24!=m2.E24 || + m1.E31!=m2.E31 || m1.E32!=m2.E32 || m1.E33!=m2.E33 || m1.E34!=m2.E34 || + m1.E41!=m2.E41 || m1.E42!=m2.E42 || m1.E43!=m2.E43 || m1.E44!=m2.E44 ? true:false; + } + +SrOutput& operator<< ( SrOutput& o, const SrMat& m ) + { + o << m.E11 <<' '<< m.E12 <<' '<< m.E13 <<' '<< m.E14 << srnl; + o << m.E21 <<' '<< m.E22 <<' '<< m.E23 <<' '<< m.E24 << srnl; + o << m.E31 <<' '<< m.E32 <<' '<< m.E33 <<' '<< m.E34 << srnl; + o << m.E41 <<' '<< m.E42 <<' '<< m.E43 <<' '<< m.E44 << srnl; + return o; + } + +SrInput& operator>> ( SrInput& in, SrMat& m ) + { + return in >> m.E11 >> m.E12 >> m.E13 >> m.E14 + >> m.E21 >> m.E22 >> m.E23 >> m.E24 + >> m.E31 >> m.E32 >> m.E33 >> m.E34 + >> m.E41 >> m.E42 >> m.E43 >> m.E44; + } + + +/* Almost included at some point: + / The matrix keeps a flag saying if it represents a rigid transformation. + This flag is used to speed up some operations. + void rigid_transformation ( bool b ) { _rigid_transf=b?1:0; } + + / Returns if the matrix represents a rigid transformation + bool rigid_transformation () const { return _rigid_transf? true:false; } +*/ + +//================================== End of File =========================================== diff --git a/source/dcdt/se/sr_mat.h b/source/dcdt/se/sr_mat.h new file mode 100644 index 0000000000..6ab4facbed --- /dev/null +++ b/source/dcdt/se/sr_mat.h @@ -0,0 +1,316 @@ + +# ifndef SR_MAT_H +# define SR_MAT_H + +/** \file sr_mat.h + * A 4x4 matrix */ + +# include "sr_vec.h" + +/*! \class SrMat sr_mat.h + \brief A 4x4 matrix + + The transformation methods consider that the matrix will be + applied to a line vector at the left side of the matrix. So + that in all matrix transformations M should multiply v as vM. + When using the matrix as a geometric transformation, the + first three elements of the fourth line represent the + translation vector. This is to follow the OpenGL design. */ +class SrMat + { private : + float e[16]; + public : + static const SrMat null; //!< A null matrix + static const SrMat id; //!< An identity matrix + enum InitializeMode { NotInitialized }; + public : + + /*! Default constructor initializes SrMat as identity. */ + SrMat () { set(id.e); } + + /*! Copy constructor. */ + SrMat ( const SrMat &m ) { set(m.e); } + + /*! Constructor without initialization. SrMat values are simply not + initialized when declaring SrMat m(SrMat::NotInitialized) */ + SrMat ( InitializeMode /*m*/ ) {} + + /*! Constructor from a float[16] pointer. */ + SrMat ( const float *p ); + + /*! Constructor from a double[16] pointer. */ + SrMat ( const double *p ); + + /*! Constructor from 16 values. */ + SrMat ( float a, float b, float c, float d, float e, float f, float g, float h, + float i, float j, float k, float l, float m, float n, float o, float p ); + + /*! Copy operator. */ + void operator= ( const SrMat& m ) { set(m.e); } + + /*! float pointer type cast operator */ + operator float*() { return &e[0]; } + + /*! const float pointer type cast operator */ + operator const float*() const { return &e[0]; } + + /*! Returns a float pointer to the SrMat element with given index. */ + float* pt ( int i ) { return &e[i]; } + + /*! Returns a const float pointer to the SrMat element with given index. */ + const float* pt_const ( int i ) const { return &e[i]; } + + /*! Permits access to an element of the matrix as a vector of 16 elements. + The given index must be from 0 to 15, no chekings are done. + Implemented inline. */ + float& operator[] ( int i ) { return e[i]; } + + /*! const version of [] operator */ + float operator[] ( int i ) const { return e[i]; } + + /*! const version of operator[], implemented inline */ + float get ( int i ) const { return e[i]; } + + /*! set element i in [0,16], implemented inline */ + void set ( int i, float v ) { e[i]=v; } + + /*! Permits access to an element of the matrix using a line and column + indices. The given indices can be from 0 to 3. No indices chekings + are done. Implemented inline. */ + float& operator() ( int i, int j ) { return e[i*4+j]; } + + /*! const version of operator(), implemented inline */ + float get ( int i, int j ) const { return e[i*4+j]; } + + float& e11 () { return e[0]; } //!< Returns a reference to the element [0] + float& e12 () { return e[1]; } //!< Returns a reference to the element [1] + float& e13 () { return e[2]; } //!< Returns a reference to the element [2] + float& e14 () { return e[3]; } //!< Returns a reference to the element [3] + float& e21 () { return e[4]; } //!< Returns a reference to the element [4] + float& e22 () { return e[5]; } //!< Returns a reference to the element [5] + float& e23 () { return e[6]; } //!< Returns a reference to the element [6] + float& e24 () { return e[7]; } //!< Returns a reference to the element [7] + float& e31 () { return e[8]; } //!< Returns a reference to the element [8] + float& e32 () { return e[9]; } //!< Returns a reference to the element [9] + float& e33 () { return e[10]; } //!< Returns a reference to the element [10] + float& e34 () { return e[11]; } //!< Returns a reference to the element [11] + float& e41 () { return e[12]; } //!< Returns a reference to the element [12] + float& e42 () { return e[13]; } //!< Returns a reference to the element [13] + float& e43 () { return e[14]; } //!< Returns a reference to the element [14] + float& e44 () { return e[15]; } //!< Returns a reference to the element [15] + + /*! Sets all elements of SrMat from the given float[16] pointer. */ + void set ( const float *p ); + + /*! Sets all elements of SrMat from the given double[16] pointer. */ + void set ( const double *p ); + + /*! Sets the four elements of line 1. Implemented inline. */ + void setl1 ( float x, float y, float z, float w ) { e[0]=x; e[1]=y; e[2]=z; e[3]=w; } + + /*! Sets the four elements of line 2. Implemented inline. */ + void setl2 ( float x, float y, float z, float w ) { e[4]=x; e[5]=y; e[6]=z; e[7]=w; } + + /*! Sets the four elements of line 3. Implemented inline. */ + void setl3 ( float x, float y, float z, float w ) { e[8]=x; e[9]=y; e[10]=z; e[11]=w; } + + /*! Sets the four elements of line 4. Implemented inline. */ + void setl4 ( float x, float y, float z, float w ) { e[12]=x; e[13]=y; e[14]=z; e[15]=w; } + + /*! Sets the first three elements of line 1. Implemented inline. */ + void setl1 ( const SrVec &v ) { e[0]=v.x; e[1]=v.y; e[2]=v.z; } + + /*! Sets the first three elements of line 2. Implemented inline. */ + void setl2 ( const SrVec &v ) { e[4]=v.x; e[5]=v.y; e[6]=v.z; } + + /*! Sets the first three elements of line 3. Implemented inline. */ + void setl3 ( const SrVec &v ) { e[8]=v.x; e[9]=v.y; e[10]=v.z; } + + /*! Sets the first three elements of line 4. Implemented inline. */ + void setl4 ( const SrVec &v ) { e[12]=v.x; e[13]=v.y; e[14]=v.z; } + + /*! Returns true if all elements are equal to 0.0, false otherwise. */ + bool isnull () const { return *this==null; } + + /*! Returns true if the matrix is identical to SrMat::id, false otherwise. */ + bool isid () const { return *this==id; } + + /*! Makes SrMatrix be a null matrix. */ + void zero () { *this=null; } + + /*! Make elements in interval [-epsilon,epsilon] to become 0 */ + void round ( float epsilon ); + + /*! Makes SrMatrix be an identity matrix. */ + void identity () { *this=id; } + + /*! Transpose SrMatrix. */ + void transpose (); + + /*! Transpose the 3x3 sub matrix. */ + void transpose3x3 (); + + /*! Makes SrMat be a translation transformation, that can be applied to a vector x as xM. */ + void translation ( float tx, float ty, float tz ); + + /*! Makes SrMat be a translation transformation, that can be applied to a vector x as xM. */ + void translation ( const SrVec &v ) { translation(v.x,v.y,v.z); } + + /*! Pre-multiplies SrMat with a translation matrix constructed with the + vector v. The multiplication is optimized considering that SrMat has + values only on its 3x3 sub matrix. */ + void left_combine_translation ( const SrVec &v ); + + /*! Pos-multiplies SrMat with a translation matrix constructed with the + vector v. Here the first three values of the 4th line are added by v. */ + void right_combine_translation ( const SrVec &v ); + + /*! Combines in SrMat a scale factor of sx, sy and sz on each coordinate. */ + void combine_scale ( float sx, float sy, float sz ); + + /*! Combines in SrMat a global scale factor of s. */ + void combine_scale ( float s ) { combine_scale(s,s,s); } + + /*! Makes SrMat be a scale transformation, that can be applied to a vector x as xM. */ + void scale ( float sx, float sy, float sz ); + + /*! Makes SrMat be a scale transformation, that can be applied to a vector x as xM. */ + void scale ( float s ) { scale(s,s,s); } + + /*! Makes SrMat be a rotation transformation around x axis. The given parameters + are the sine and cosine of the desired angle. The transformation can then be + applied to a vector x as xM. */ + void rotx ( float sa, float ca ); + + /*! Makes SrMat be a rotation transformation around y axis. The given parameters + are the sine and cosine of the desired angle. The transformation can then be + applied to a vector x as xM. */ + void roty ( float sa, float ca ); + + /*! Makes SrMat be a rotation transformation around z axis. The given parameters + are the sine and cosine of the desired angle. The transformation can then be + applied to a vector x as xM. */ + void rotz ( float sa, float ca ); + + /*! Makes SrMat be a rotation transformation around x of the given angle in radians. */ + void rotx ( float radians ); + + /*! Makes SrMat be a rotation transformation around y of the given angle in radians. */ + void roty ( float radians ); + + /*! Makes SrMat be a rotation transformation around z of the given angle in radians. */ + void rotz ( float radians ); + + /*! Rotation around an axis. The transformation is obtained with vM (Mesa version). */ + void rot ( const SrVec &vec, float sa, float ca ); + + /*! Rotation around an axis given an angle in radians. */ + void rot ( const SrVec &vec, float radians ); + + /*! Gives the rotation matrix that rotates one vector to another. */ + void rot ( const SrVec& from, const SrVec& to ); + + /*! Sets SrMat to be the rigid transformation matrix that maps the three + given vertices into the plane xy. The transformation can then be applied + to each vertex with vM. After transformed, v1 will go to the origin, + and v2 will lye in the X axis. The most time consuming operations are + 3 square roots and 3 matrix multiplication. */ + void projxy ( SrVec v1, SrVec v2, SrVec v3 ); + + /*! Sets SrMat to the OpenGL glut-like camera transformation. Note: fovy + parameter must be set in radians. */ + void perspective ( float fovy, float aspect, float znear, float zfar ); + + /*! Sets SrMat to the OpenGL gluLookAt like camera transformation. */ + void look_at ( const SrVec& eye, const SrVec& center, const SrVec& up ); + + /*! Fast invertion by direct calculation, no loops, no gauss, no pivot searching, + but with more numerical errors. The result is put in the given SrMat parameter. */ + void inverse ( SrMat& inv ) const; + + /*! Fast invertion by direct calculation, no loops, no gauss, no pivot searching, + but with more numerical errors. The result is a new matrix returned by value. */ + SrMat inverse () const; + + /*! Makes SrMat to be its inverse, calling the inverse() method. */ + void invert () { *this=inverse(); } + + /*! Fast 4x4 determinant by direct calculation, no loops, no gauss. */ + float det () const; + + /*! Fast 3x3 determinant by direct calculation, no loops, no gauss. */ + float det3x3 () const; + + /*! Considers the matrix as a 16-dimensional vector and returns its norm raised + to the power of two. */ + float norm2 () const; + + /*! Considers the matrix as a 16-dimensional vector and returns its norm. */ + float norm () const ; + + /*! Returns true if dist2(*this,m)<=ds*ds, and false otherwise. Implemented inline. */ + bool isnext ( const SrMat &m, float ds ) { return dist2(*this,m)<=ds*ds? true:false; } + + /*! Sets SrMat as the multiplication of m1 with m2. This method needs + to use a temporary matrix to perform the calculations if m1 or m2 + is equal to to SrMat */ + void mult ( const SrMat &m1, const SrMat &m2 ); + + /*! Sets SrMat to be the addition of m1 with m2. */ + void add ( const SrMat &m1, const SrMat &m2 ); + + /*! Sets SrMat to be the difference m1-m2. */ + void sub ( const SrMat &m1, const SrMat &m2 ); + + /*! Distance between two matrices, considering them as a 16-dimensional vector. */ + friend float dist ( const SrMat &a, const SrMat &b ); + + /*! Distance between two matrices raised to two, considering them as a 16-dimensional vector. */ + friend float dist2 ( const SrMat &a, const SrMat &b ); + + /*! Operator to multiply SrMat by a scalar. */ + void operator *= ( float r ); + + /*! Operator to (right) multiply SrMat by another SrMat m. */ + void operator *= ( const SrMat &m ); + + /*! Operator to add to SrMat another SrMat. */ + void operator += ( const SrMat &m ); + + /*! Operator to multiply a SrMat to a scalar, returning another SrMat. */ + friend SrMat operator * ( const SrMat &m, float r ); + + /*! Operator to multiply a scalar to a SrMat, returning another SrMat. */ + friend SrMat operator * ( float r, const SrMat &m ); + + /*! Operator to multiply a SrMat to a SrVec, returning another SrMat. */ + friend SrVec operator * ( const SrMat &m, const SrVec &v ); + + /*! Operator to multiply a SrVec to a SrMat, returning another SrMat. */ + friend SrVec operator * ( const SrVec &v, const SrMat &m ); + + /*! Operator to multiply two SrMat, returning another SrMat. */ + friend SrMat operator * ( const SrMat &m1, const SrMat &m2 ); + + /*! Operator to add two SrMat, returning another SrMat. */ + friend SrMat operator + ( const SrMat &m1, const SrMat &m2 ); + + /*! Operator to compute the difference two SrMat, returning another SrMat. */ + friend SrMat operator - ( const SrMat &m1, const SrMat &m2 ); + + /*! Comparison operator to check if two SrMat are equal. */ + friend bool operator == ( const SrMat &m1, const SrMat &m2 ); + + /*! Comparison operator to check if two SrMat are different. */ + friend bool operator != ( const SrMat &m1, const SrMat &m2 ); + + /*! Outputs 4 elements per line. */ + friend SrOutput& operator<< ( SrOutput& o, const SrMat& m ); + + /*! Reads 16 float numbers from the input. */ + friend SrInput& operator>> ( SrInput& in, SrMat& m ); + }; + +//============================== end of file =============================== + +# endif // SR_MAT_H diff --git a/source/dcdt/se/sr_material.cpp b/source/dcdt/se/sr_material.cpp new file mode 100644 index 0000000000..b79708a0cd --- /dev/null +++ b/source/dcdt/se/sr_material.cpp @@ -0,0 +1,61 @@ +#include "precompiled.h" +# include "sr_material.h" + +//# define SR_USE_TRACE1 +# include "sr_trace.h" + +SrMaterial::SrMaterial () : + ambient ( 51, 51, 51, 255 ), + diffuse ( 204, 204, 204, 255 ), + specular ( 0, 0, 0, 255 ), + emission ( 0, 0, 0, 255 ) + { + shininess = 0; + } + +void SrMaterial::init () + { + ambient.set ( 51, 51, 51, 255 ); + diffuse.set ( 204, 204, 204, 255 ); + specular.set ( 0, 0, 0, 255 ); + emission.set ( 0, 0, 0, 255 ); + shininess = 0; + } + +bool operator == ( const SrMaterial& m1, const SrMaterial& m2 ) + { + return ( m1.ambient==m2.ambient && + m1.diffuse==m2.diffuse && + m1.specular==m2.specular && + m1.emission==m2.emission && + m1.shininess==m2.shininess )? true:false; + } + +bool operator != ( const SrMaterial& m1, const SrMaterial& m2 ) + { + return ( m1.ambient==m2.ambient && + m1.diffuse==m2.diffuse && + m1.specular==m2.specular && + m1.emission==m2.emission && + m1.shininess==m2.shininess )? false:true; + } + +SrOutput& operator<< ( SrOutput& o, const SrMaterial& m ) + { + return o << m.ambient <<' '<< + m.diffuse <<' '<< + m.specular <<' '<< + m.emission <<' '<< + m.shininess; + } + +SrInput& operator>> ( SrInput& in, SrMaterial& m ) + { + return in >> m.ambient >> + m.diffuse >> + m.specular >> + m.emission >> + m.shininess; + } + +//================================ End of File ================================================= diff --git a/source/dcdt/se/sr_material.h b/source/dcdt/se/sr_material.h new file mode 100644 index 0000000000..38f7b48935 --- /dev/null +++ b/source/dcdt/se/sr_material.h @@ -0,0 +1,49 @@ + +/** \file sr_material.h + * specifies a material + */ + +# ifndef SR_MATERIAL_H +# define SR_MATERIAL_H + +# include "sr_input.h" +# include "sr_output.h" + +# include "sr_color.h" + +/*! \class SrMaterial sr_material.h + \brief specifies a material + + Defines a material. */ +class SrMaterial + { public : + SrColor ambient; //!< default in float coords: 0.2, 0.2, 0.2, 1.0 + SrColor diffuse; //!< default in float coords: 0.8, 0.8, 0.8, 1.0 + SrColor specular; //!< default in float coords: 0.0, 0.0, 0.0, 1.0 + SrColor emission; //!< default in float coords: 0.0, 0.0, 0.0, 1.0 + srbyte shininess; //!< default: 0, can be in : [0,128] + public : + + /*! Initializes with the default values. */ + SrMaterial (); + + /*! Set again the default values. Note that .2 is mapped + to 51, and .8 to 204 in the SrColor format. */ + void init (); + + /*! Exact comparison operator == */ + friend bool operator == ( const SrMaterial& m1, const SrMaterial& m2 ); + + /*! Exact comparison operator != */ + friend bool operator != ( const SrMaterial& m1, const SrMaterial& m2 ); + + /*! Outputs in format: "ar ag ab aa dr dg db da sr sg sb sa er eg eb ea s". */ + friend SrOutput& operator<< ( SrOutput& o, const SrMaterial& m ); + + /*! Inputs from format: "ar ag ab aa dr dg db da sr sg sb sa er eg eb ea s". */ + friend SrInput& operator>> ( SrInput& in, SrMaterial& m ); + }; + +//================================ End of File ================================================= + +# endif // SR_MATERIAL_H diff --git a/source/dcdt/se/sr_matn.cpp b/source/dcdt/se/sr_matn.cpp new file mode 100644 index 0000000000..60e0f8440e --- /dev/null +++ b/source/dcdt/se/sr_matn.cpp @@ -0,0 +1,606 @@ +#include "precompiled.h" +# include +# include + +# include "sr_random.h" +# include "sr_output.h" +# include "sr_matn.h" + +//# define SR_USE_TRACE1 // const/dest +# include "sr_trace.h" + +# define MAT(m,n) _data[_col*m+n] + +SrMatn::SrMatn () : _lin(0), _col(0), _data(0) + { + SR_TRACE1 ("Default Constructor"); + } + +SrMatn::SrMatn ( const SrMatn &m ) : _lin(m._lin), _col(m._col) + { + _data = new double[_lin*_col]; + // reminder: memcpy does not work with overlap, but memmove yes. + memcpy ( _data, m._data, sizeof(double)*_lin*_col ); + SR_TRACE1 ("Copy Constructor\n"); + } + +SrMatn::SrMatn ( int m, int n ) : _lin(m), _col(n) + { + _data = new double[_lin*_col]; + SR_TRACE1 ("Size Constructor\n"); + } + +SrMatn::SrMatn ( double *data, int m, int n ) : _lin(m), _col(n) + { + _data = data; + SR_TRACE1 ("Take Data Constructor\n"); + } + +SrMatn::~SrMatn () + { + delete [] _data; + SR_TRACE1 ("Destructor\n"); + } + +void SrMatn::size ( int m, int n ) + { + SR_POS(m); + SR_POS(n); + int s = m*n; + + if ( s==0 ) + { delete [] _data; + _data=0; + m = n = 0; + } + else if ( !_data ) + { _data = new double[s]; + } + else if ( s!=_lin*_col ) + { delete [] _data; + _data = new double[s]; + } + + _lin=m; _col=n; + } + +void SrMatn::resize ( int m, int n ) + { + SrMatn mat ( m, n ); + int lin = SR_MIN(m,_lin); + int col = SR_MIN(n,_col); + + for ( int i=0; isize(l,c); + + klast = SR_MIN(m1._col,m2._lin); + for ( i=0; iset(i,j,sum); + } + + if ( m!=this ) { *this=*m; delete m; } + } + +void SrMatn::leave_data ( double*& m ) + { + m = _data; + _data = 0; + _lin = _col = 0; + } + +void SrMatn::take_data ( SrMatn &m ) + { + if ( this==&m ) return; + if ( _data ) delete []_data; + _data = m._data; m._data=0; + _lin = m._lin; m._lin=0; + _col = m._col; m._col=0; + } + +//============================ Operators ========================================= + +SrMatn& SrMatn::operator = ( const SrMatn& m ) + { + if ( this != &m ) + { size ( m._lin, m._col ); + memcpy ( _data, m._data, sizeof(double)*_lin*_col ); + } + return *this; + } + +SrMatn& SrMatn::operator += ( const SrMatn& m ) + { + int s = size(); + for ( int i=0; ibig ) big=tmp; } + if ( big==0.0 ) { sr_out.warning("Singular matrix in routine ludcmp"); return 0; } + vv[i]=1.0/big; // save the scaling + } + + for ( j=0; j=big) { big=tmp; imax=i; } + } + + if ( pivoting ) + { if ( j!=imax ) // interchange rows if needed + { for ( k=0; kj ) { l(i,j)=u(i,j); u(i,j)=0.0; } + else { l(i,j) = i==j? 1.0:0.0; } + } + + return true; + } + +void lubksb ( const SrMatn &a, SrMatn &b, const int *indx ) + { + int i, ii=-1, ip, j; + double sum; + int n=a.lin(); + + for ( i=0; i=0) { for ( j=ii; j<=i-1; j++ ) sum -= a.get(i,j)*b[j]; } + else if (sum) ii=i; + b[i]=sum; + } + + for ( i=n-1; i>=0; i-- ) + { sum = b[i]; + for ( j=i+1; j=big) { big=tmp; irow=j; icol=k; } + } + else if ( ipiv[k]>0 ) + { sr_out.warning("gaussj: Singular Matrix-1"); return false; } + } + ++(ipiv[icol]); + if ( irow!=icol ) + { for ( l=0; l=0; l-- ) + { if ( indxr[l]==indxc[l] ) continue; + for ( k=0; kpiv_val ) { piv_lin=k; piv_val=tmp; } + } + if ( i!=piv_lin ) m.swap_lines(i,piv_lin); + + tmp = m(i,i); + if ( tmp==0.0 ) { sr_out.warning("singular matrix in gauss\n"); return false; } + for ( k=i+1; k=i; j-- ) + m(k,j) = m(k,j)-m(i,j)*m(k,i)/tmp; + } + + for ( i=n-1; i>=0; i-- ) + { tmp = 0.0; + for ( k=i+1; k=0; j-- ) u[j] -= gam[j+1]*u[j+1]; + } +*/ + +/* From my old libraries, should work. +static void gauss_band ( SrMatn& A, SrMatn& B ) + { + int i, j, k, c; + int band=(A.col()-2)/2; + int diag=band+1; // diagonal + int last=col-1; // last column of band TMatrix + real fact, div; + + if (col%2!=0 || col>lin || pointer==null) return gaBadSize; + if ( !v.setSize(lin,1) ) return gaNoMemory; + + for ( i=1; i<=lin; i++ ) + { c=col; + div = MAT(i,band+1); + if ( div==0.0 ) return gaZeroDivision; + for ( j=i-band; j<=i+band && j<=lin; j++ ) + { c--; + if (j<=i || j<1) continue; + fact = MAT(j,c)/div; + MAT(j,col) = MAT(j,col)-MAT(i,col)*fact; + for ( k=last; k>=1; k-- ) + { if (k+j-i>last) continue; + MAT(j,k) = MAT(j,k)-MAT(i,k+j-i)*fact; + } + } + } + for ( j=lin; j>=1; j-- ) + { + fact = 0.0; c=diag; + for ( k=j+1; k<=j+band && k<=lin; k++ ) + { ++c; fact += MAT(j,c)*v.get(k); } + if ( MAT(j,diag)==0.0 ) return gaZeroDivision; + v(j) = ( (MAT(j,col)-fact) / MAT(j,diag) ); + } + return gaOk; + } +*/ + +//================================= End Of File ================================== diff --git a/source/dcdt/se/sr_matn.h b/source/dcdt/se/sr_matn.h new file mode 100644 index 0000000000..685d3e8fdd --- /dev/null +++ b/source/dcdt/se/sr_matn.h @@ -0,0 +1,148 @@ + +/** \file sr_matn.h + * n dimensional matrix */ + +# ifndef SR_MATN_H +# define SR_MATN_H + +class SrInput; +class SrOutput; + +/*! \class SrMatn sr_matrix.h + \brief resizeable, n-dimensional matrix of double elements + + All coordinates are considered starting from index 0. */ +class SrMatn + { private : + double *_data; + int _lin, _col; + public : + SrMatn (); + SrMatn ( const SrMatn &m ); + SrMatn ( int m, int n ); + SrMatn ( double *data, int m, int n ); + ~SrMatn (); + + /*! Old elements are not preserved when realloc is done. */ + void size ( int m, int n ); + + /*! returns the size of the matirx, ie, lin*col. */ + int size () const { return _lin*_col; } + + /*! resize also copies the old values to the new resized matrix. */ + void resize ( int m, int n ); + + /*! resizes SrMatn and copy the contents of the submatrix [li..le,ci..ce] + of m to SrMatn. */ + void set_as_sub_mat ( const SrMatn &m, int li, int le, int ci, int ce ); + + /*! Copies the submatrix [li..le,ci..ce] of m to the SrMatn, without + reallocation, and putting the submatrix starting at coordinates + [l,c] of SrMatn. */ + void set_sub_mat ( int l, int c, const SrMatn &m, int li, int le, int ci, int ce ); + + /*! Resizes SrMatn as a column vector containing the data of the + s column of m. */ + void set_as_column ( const SrMatn &m, int s ); + + /*! Copies the data of the column mc of m to the column c of SrMat. */ + void set_column ( int c, const SrMatn &m, int mc ); + + int lin () const { return _lin; } + int col () const { return _col; } + void identity (); + void transpose (); + void swap_lines ( int l1, int l2 ); + void swap_columns ( int c1, int c2 ); + void set_all ( double r ); + void set_random ( double inf, double sup ); + + /*! Returns the (lin*col)-dimensional vector norm. */ + double norm () const; + + operator const double* () const { return _data; } + + /*! indices start from 0. */ + double& operator [] ( int p ) { return _data[p]; } + + /*! indices start from 0. */ + double& operator () ( int i, int j ) { return _data[_col*i+j]; } + + void set ( int p, double r ) { _data[p]=r; } + void set ( int i, int j, double r ) { _data[_col*i+j]=r; } + + double get ( int p ) const { return _data[p]; } + double get ( int i, int j ) const { return _data[_col*i+j]; } + + void add ( const SrMatn& m1, const SrMatn& m2 ); + void sub ( const SrMatn& m1, const SrMatn& m2 ); + + /*! Makes SrMatn be m1*m2. This method works properly with calls like + a.mult(a,b), a.mult(b,a), or a.mult(a,a). */ + void mult ( const SrMatn& m1, const SrMatn& m2 ); + + void leave_data ( double *&m ); + void take_data ( SrMatn &m ); + + SrMatn& operator = ( const SrMatn& m ); + SrMatn& operator += ( const SrMatn& m ); + SrMatn& operator -= ( const SrMatn& m ); + + friend SrOutput& operator << ( SrOutput &o, const SrMatn &m ); + + /*! Returns the norm of the matrix a-b. */ + friend double dist ( const SrMatn &a, const SrMatn &b ); + + /* Transforms a into its encoded LU decomposition. The L and U are returned + in the same matrix a. L can be retrieved by making L=a and then setting + all elements of the diagonal to 1 and those above the diagonal to 0. U is + retrieved by making U=a, and then setting all elements below the diagonal + to 0. Then the multiplication LU will give a', where a' is a with some + row permutations. The row permutation history is stored in the returned + int array, that tells, beginning from the first row, the other row index + to swap sequentially the rows. This swap sequence will transform a to a'. + If pivoting is set to false, then no pivoting is done and a' will be equal + to a. Parameter d returns +1 or -1, depending on whether the number of + row interchanges was even or odd, respectively. The function returns null + if a is singular. See Numerical Recipes page 46. */ + friend const int* ludcmp ( SrMatn &m, double *d=0, bool pivoting=true ); + + /*! Returns the explicit LU decomposition of a. Here we decompose the result + of the encoded ludcmp version (without pivoting), returning the exact + l and u matrices so that lu=a.*/ + friend bool ludcmp ( const SrMatn &a, SrMatn &l, SrMatn &u ); + + /*! Solves the linear equations ax=b. Here a is a n dimension square matrix, + given in its LU encoded decomposition, b is a n dimensional column vector, + that will be changed to return the solution vector x. indx is the row + permutation vector returned by ludcmp. */ + friend void lubksb ( const SrMatn &a, SrMatn &b, const int *indx ); + + /*! Solve the system of linear equations ax=b using a LU decomposition. + Matrix a is changed to its encoded LU decomposition, and matrix b + will be changed to contain the solution x. */ + friend bool lusolve ( SrMatn &a, SrMatn &b ); + + /*! Same as the other lusolve(), but here const qualifiers are respected, + what implies some extra temporary buffers allocation deallocation. */ + friend bool lusolve ( const SrMatn &a, const SrMatn &b, SrMatn &x ); + + /*! Will change a to its encoded LU decomposition and return in inva + the inverse matrix of a. */ + friend bool inverse ( SrMatn &a, SrMatn &inva ); + + /*! Will put in a its inverse. */ + friend bool invert ( SrMatn &a ); + + /* Returns the determinant of a using the LU decomposition method. The + given matrix a is transformed into the result of the ludcmp() function. + See Numerical Recipes page 49. */ + friend double det ( SrMatn &a ); + + /*! Solve the system ax=b, using the Gauss Jordan method. */ + friend bool gauss ( const SrMatn &a, const SrMatn &b, SrMatn &x ); + }; + +//============================== end of file =============================== + +# endif // SR_MATN_H diff --git a/source/dcdt/se/sr_matrix_window.h b/source/dcdt/se/sr_matrix_window.h new file mode 100644 index 0000000000..4fb3db55a8 --- /dev/null +++ b/source/dcdt/se/sr_matrix_window.h @@ -0,0 +1,54 @@ + +# ifndef SR_MATRIX_WINDOW_H +# define SR_MATRIX_WINDOW_H + +/** \file sr_matrix_window.h + * Fltk window for matrix edition. */ + +class Fl_Window; +class Fl_Browser; +class SrMat; +class SrMWin; + +/*! \class SrMatrixWindow sr_matrix_window.h + \brief Fltk window for matrix edition. */ +class SrMatrixWindow + { private : + SrMWin* _ui; + bool _ok_pressed; + public : + + /*! Constructor */ + SrMatrixWindow (); + + /*! Destructor */ + ~SrMatrixWindow (); + + /*! Set window title */ + void title ( const char* s ); + + Fl_Window* window (); + + /*! Put a new matrix in the window. Method matrix_changed() is called. */ + void mat ( const SrMat& m ); + + const SrMat& mat () const; + + bool ok_was_pressed () const { return _ok_pressed; } + + /* Called when the ok button is pressed, and here it closes the + window and set _ok_pressed to true; */ + virtual void ok_button ( const SrMat& m ); + + /* Called when the cancel button is pressed, and here it closes the + window and set _ok_pressed to false; */ + virtual void cancel_button (); + + /* Called each time the matrix is changed */ + virtual void matrix_changed ( const SrMat& m ) {}; + }; + +//================================ End of File ================================================= + +# endif // SR_MATRIX_WINDOW_H + diff --git a/source/dcdt/se/sr_mem_control.cpp b/source/dcdt/se/sr_mem_control.cpp new file mode 100644 index 0000000000..d2b9baf62b --- /dev/null +++ b/source/dcdt/se/sr_mem_control.cpp @@ -0,0 +1,131 @@ +#include "precompiled.h" +# include +# include +# include + +# include "sr_mem_control.h" +# include "sr_output.h" + +//=== Memory Control ======================================================================= + +// the folowing structure is used only internally +// I could have used a balanced tree, but since this is only for debug +// it is better to maintain it as simple as possible and as a stand alone file. +struct srMemData + { const char* type; + const char* file; + int line; + int size; + void* addr; + void set ( const char* c, char* f, int l, int s, void* a ) + { type=c; file=f; line=l; size=s; addr=a; } + }; + +// it is not nice to use such static things intead of classes, +// but this is the only way to let the macro mechanism work. +static bool changed=false; +static srMemData *mem_data=0; +static int mem_data_size=0; +static int mem_data_capacity=0; +static unsigned mem_bytes_used=0; +static SrOutput& output=sr_out; + +static int cmp_func ( const void* pt1, const void* pt2 ) + { + const srMemData *a = (const srMemData*)pt1; + const srMemData *b = (const srMemData*)pt2; + + int c = strcmp ( a->file, b->file ); + if ( c!=0 ) return c; + + c = strcmp ( a->type, b->type ); + if ( c!=0 ) return c; + + return b->size-a->size; + } + +static void sort () + { + qsort ( mem_data, mem_data_size, sizeof(srMemData), cmp_func ); + } + +void *sr_mem_control_alloc ( const char* type, char* file, int line, int size, void* addr ) + { + if ( !addr ) + { //lineout (); + output<<"\nsr_control_malloc: Zero Pointer Allocated!\n" << + "File:"<0 && *file!='\\' && *file!='/' ) { file--; i--; } + if ( i>0 ) file++; + + changed=true; + mem_data[mem_data_size++].set ( type, file, line, size, addr ); + mem_bytes_used += size; + return addr; + } + +bool sr_mem_control_free ( char* file, int line, void* addr ) + { + if ( !addr ) return true; + + for ( int i=0; i0 ) + output.put ( " File Type Line Size Addr\n"); + + for ( int i=0; i + +/** \file sr_mem_control.h + * Functions and macros to control memory allocation. + * The idea of usage is similar to sr_trace.h: only inside + * files that testing memory allocation is required that the activation + * of macros should be defined. */ + +/*! This function saves in an internal buffer the given parameters + in order to keep track of allocated memory. + It may be used by replacing new/malloc calls to: + \code sr_mem_control_alloc("X",__FILE__,__LINE__,sizeof(X),new X) \endcode + Then sr_memory_report() can be called to see all allocated data that was not + freed. It is better to use this function with the macros SR_NEW and SR_MALLOC. */ +void* sr_mem_control_alloc ( const char* type, char* file, int line, int size, void* addr ); + +/*! This function erases from the internal buffer the given address + of memory, that was previously registered with sr_mem_control_alloc(). + It is to be used replacing delete/free calls in the following way: + \code sr_mem_control_free(__FILE__,__LINE__,X); delete X; \endcode + It is better to use this function with the macros SR_DEL, SR_DELA and SR_FREE. */ +bool sr_mem_control_free ( char* file, int line, void* addr ); + +/*! This function performs a call to realloc(addr,size), and returns the new + pointer. But it also updates the pointers saved in the internal buffer in + order to keep track of allocated memory. + It may be used replacing realloc calls in the following way: + \code pt=sr_mem_control_realloc(__FILE__,__LINE__,newsize,pt) \endcode + It is better to use this function with the macro SR_REALLOC. */ +void* sr_mem_control_realloc ( char* file, int line, int size, void* addr ); + +/*! This function gives a report about all allocated data that was not freed + using sr_out. But to make it work, the user needs to change all + new/malloc/delete/free calls by calls to the macros SR_NEW, SR_MALLOC, + SR_REALLOC, SR_DEL, SR_DELA, SR_FREE or functions + sr_mem_control_alloc/free/realloc. */ +void sr_memory_report (); + +/*! Returns the total of memory allocated. Only the memory allocated using + sr_mem_control_alloc/free/realloc, or the related macros are taken into account. */ +unsigned sr_memory_allocated (); + +class SrOutput; + +/*! Changes the output used for the reports. */ +void sr_memory_report_output ( SrOutput& o ); + +# ifdef SR_USE_MEM_CONTROL +# define SR_DEL(X) if (sr_mem_control_free(__FILE__,__LINE__,X)) delete X; +# define SR_DELA(X) if (sr_mem_control_free(__FILE__,__LINE__,X)) delete[] X; +# define SR_FREE(p) if (sr_mem_control_free(__FILE__,__LINE__,p)) free(p); +# define SR_NEW(X) ((X*)sr_mem_control_alloc(#X,__FILE__,__LINE__,sizeof(X),new X)) +# define SR_NEW1(X,A) ((X*)sr_mem_control_alloc(#X,__FILE__,__LINE__,sizeof(X),new X(A))) +# define SR_NEW2(X,A,B) ((X*)sr_mem_control_alloc(#X,__FILE__,__LINE__,sizeof(X),new X(A,B))) +# define SR_NEWA(X,s) ((X*)sr_mem_control_alloc(#X,__FILE__,__LINE__,sizeof(X),new X[s])) +# define SR_MALLOC(s) sr_mem_control_alloc("malloc",__FILE__,__LINE__,s,malloc(s)) +# define SR_REALLOC(p,s) sr_mem_control_realloc(__FILE__,__LINE__,s,p) +# else +# define SR_DEL(X) delete X +# define SR_DELA(X) delete[] X +# define SR_FREE(p) free(p) +# define SR_NEW(X) new X +# define SR_NEW1(X,A) new X(A) +# define SR_NEW2(X,A,B) new X(A,B) +# define SR_MALLOC(s) malloc(s) +# define SR_REALLOC(p,s) realloc(p,s) +# endif + +/*! \def SR_DEL(X) + The user needs to use SR_DEL in place of delete, in order to be able to + use sr_memory_report() for obtaining a report of memory that was allocated + but not freed. + But to activate this define, the user must define SR_USE_MEM_CONTROL before + including the header sr_mem_control.h. Otherwise this define will just + directly call delete, with no overhead to the compiled code. + Use: SR_DEL(X); instead of delete X; */ + +/*! \def SR_DELA(X) + The user needs to use SR_DELA in place of delete[], in order to be able to + use sr_memory_report() for obtaining a report of memory that was allocated + but not freed. + But to activate this define, the user must define SR_USE_MEM_CONTROL before + including the header sr_mem_control.h. Otherwise this define will just + directly call delete[], with no overhead to the compiled code. + Use: SR_DELA(X); instead of delete[] X; */ + +/*! \def SR_FREE(p) + The user needs to use SR_FREE in place of free(), in order to be able to use + sr_memory report() for obtaining a report of memory that was allocated but + not freed. + But to activate this define, the user must define SR_USE_MEM_CONTROL before + including the header sr_mem_control.h. Otherwise this define will just directly + call free(), with no overhead to the compiled code. + Use: SR_FREE(p); instead of free(p); */ + +/*! \def SR_NEW(X) + The user needs to use SR_NEW in place of new, in order to be able to use + sr_memory report() for obtaining a report of memory that was allocated but + not freed. + But to activate this define, the user must define SR_USE_MEM_CONTROL before + including the header sr_mem_control.h. Otherwise this define will just + directly call new, with no overhead to the compiled code. + Use: pt=SR_NEW(X); instead of pt=new X; */ + +/*! \def SR_NEW1(X,A) + Equivalent to macro SR_NEW, but allows to set one initialisation parameter for + the constructor of class X. + Use: pt=SR_NEW(X,A); instead of pt=new X(A); */ + +/*! \def SR_NEW2(X,A,B) + Equivalent to macro SR_NEW, but allows to set two initialisation parameters for + the constructor of class X. + Use: pt=SR_NEW(X,A,B); instead of pt=new X(A,B); */ + +/*! \def SR_MALLOC(s) + The user needs to use SR_MALLOC in place of malloc(), in order to be able + to use sr_memory report() for obtaining a report of memory that was allocated + but not freed. + But to activate this define, the user must define SR_USE_MEM_CONTROL before + including the header sr_mem_control.h. Otherwise this define will just directly + call malloc(), with no overhead to the compiled code. + Use: pt=SR_MALLOC(s); instead of pt=malloc(s); */ + +/*! \def SR_REALLOC(p,s) + The user needs to use SR_REALLOC in place of realloc(), in order to be able to + use sr_memory report() for obtaining a report of memory that was allocated but + not freed. + But to activate this define, the user must define SR_USE_MEM_CONTROL before + including the header sr_mem_control.h. Otherwise this define will just directly + call realloc(), with no overhead to the compiled code. + Use: pt=SR_REALLOC(p,s); instead of pt=realloc(p,s); */ + +//============================= end of file ========================== + +# endif // SR_MEM_CONTROL_H diff --git a/source/dcdt/se/sr_model.cpp b/source/dcdt/se/sr_model.cpp new file mode 100644 index 0000000000..24d9f6396d --- /dev/null +++ b/source/dcdt/se/sr_model.cpp @@ -0,0 +1,1039 @@ +#include "precompiled.h" +# include + +# include "sr_model.h" +# include "sr_tree.h" +# include "sr_sphere.h" +# include "sr_cylinder.h" +# include "sr_string_array.h" + +//# define SR_USE_TRACE1 // IO +//# define SR_USE_TRACE2 // Validation of normals materials, etc +# include "sr_trace.h" + +//=================================== SrModel ================================================= + +const char* SrModel::class_name = "Model"; + +SrModel::SrModel () + { + culling = true; + } + +SrModel::~SrModel () + { + } + +void SrModel::init () + { + culling = true; + M.capacity ( 0 ); + V.capacity ( 0 ); + N.capacity ( 0 ); + T.capacity ( 0 ); + F.capacity ( 0 ); + Fm.capacity ( 0 ); + Fn.capacity ( 0 ); + Ft.capacity ( 0 ); + mtlnames.capacity ( 0 ); + name = ""; + } + +void SrModel::compress () + { + M.compress(); + V.compress(); + N.compress(); + T.compress(); + F.compress(); + Fm.compress(); + Fn.compress(); + Ft.compress(); + mtlnames.compress(); + name.compress(); + } + +void SrModel::validate () + { + int i, j; + SrArray iarray; + SrStringArray sarray; + + int fsize = F.size(); + + // check if the model is empty + if ( fsize==0 || V.size()==0 ) + { init (); + compress (); + return; + } + + // check size of Fn + if ( Fn.size()!=fsize || N.size()==0 ) + { Fn.size(0); + N.size(0); + } + + // check size of Ft + if ( Ft.size()!=fsize || T.size()==0 ) + { Ft.size(0); + T.size(0); + } + + // check size of Fm + if ( M.size()==0 ) + { Fm.size(0); + } + else if ( Fm.size()!=fsize ) + { j = Fm.size(); + Fm.size ( fsize ); + for ( i=j; i iarray; + + int fsize = F.size(); + + if ( M.size()==0 ) + { Fm.size(0); + } + else + { j = Fm.size(); + Fm.size ( fsize ); + for ( i=j; i=0 && Fm[i]0 ) material_names[k-1]=0; // remove the last space + + for ( i=0; i iarray; + + int fsize = F.size(); + + if ( N.size()==0 || Fn.size()!=fsize ) + { N.size(0); + Fn.size(0); + } + else + { // remove references to duplicated normals + int nsize = N.size(); + for ( i=0; i iarray; + iarray.size ( vsize ); + for ( i=0; i> s; + if ( s!="SrModel" ) return false; + + // clear arrays and set culling to true + init (); + + while ( !in.finished() ) + { + if ( !in.read_field(s) ) + { if ( in.finished() ) break; // EOF reached + return false; + } + + SR_TRACE1 ( '<' << s << "> Field found...\n" ); + + if ( s=="culling" ) // read culling state + { in >> i; + culling = i? true:false; + if ( !in.close_field(s) ) return false; + } + else if ( s=="name" ) // read name (is a SrInput::String type ) + { in.get_token(); + name = in.last_token(); + if ( !in.close_field(s) ) return false; + } + else if ( s=="vertices" ) // read vertices: x y z + { in >> i; V.size(i); + for ( i=0; i> V[i]; + if ( !in.close_field(s) ) return false; + } + else if ( s=="vertices_per_face" ) // read F: a b c + { in >> i; F.size(i); + for ( i=0; i> F[i]; + F[i].validate(); + } + if ( !in.close_field(s) ) return false; + } + else if ( s=="normals_per_face" ) // read Fn: a b c + { in >> i; Fn.size(i); + for ( i=0; i> Fn[i]; + if ( !in.close_field(s) ) return false; + } + else if ( s=="textcoords_per_face" ) // read Ft: a b c + { in >> i; Ft.size(i); + for ( i=0; i> Ft[i]; + if ( !in.close_field(s) ) return false; + } + else if ( s=="materials_per_face" ) // read Fm: i + { in >> i; Fm.size(i); + for ( i=0; i> Fm[i]; + if ( !in.close_field(s) ) return false; + } + else if ( s=="normals" ) // read N: x y z + { in >> i; N.size(i); + for ( i=0; i> N[i]; + if ( !in.close_field(s) ) return false; + } + else if ( s=="textcoords" ) // read T: x y + { in >> i; T.size(i); + for ( i=0; i> T[i]; + if ( !in.close_field(s) ) return false; + } + else if ( s=="materials" ) // read M: mtls + { in >> i; M.size(i); + for ( i=0; i> M[i]; + if ( !in.close_field(s) ) return false; + } + else if ( s=="material_names" ) // read materials + { SrString buf1, buf2; + mtlnames.capacity ( 0 ); // clear all + mtlnames.size ( M.size() ); // realloc + while ( 1 ) + { if ( in.get_token()!=SrInput::Integer ) { in.unget_token(); break; } + i = atoi ( in.last_token() ); + in.get_token(); + mtlnames.set ( i, in.last_token() ); + } + if ( !in.close_field(s) ) return false; + } + else // unknown field, just skip it: + { if ( !in.skip_field(s) ) return false; + } + } + + SR_TRACE1 ( "OK.\n" ); + + return true; + } + +bool SrModel::save ( SrOutput &o ) const + { + int i; + + // save header as a comment + o << "# SR - Simulation and Representation Toolkit\n" << + "# Marcelo Kallmann 1996-2004\n\n"; + + // save signature + o << "SrModel\n\n"; + + // save name + if ( name.len()>0 ) + { o << " \"" << name << "\" \n\n"; } + + // save culling state + if ( !culling ) + { o << " 0 \n\n"; } + + // save vertices (V) + if ( V.size() ) + { o << " " << V.size() << srnl; + for ( i=0; i\n\n"; + } + + // save faces (F) + if ( F.size() ) + { o << " " << F.size() << srnl; + for ( i=0; i\n\n"; + } + + // save normals (N) + if ( N.size() ) + { o << " " << N.size() << srnl; + for ( i=0; i\n\n"; + } + + // save normals per face (Fn) + if ( Fn.size() ) + { o << " " << Fn.size() << srnl; + for ( i=0; i\n\n"; + } + + // save texture coordinates (T) + /* Not yet supported so not saved + if ( T.size() ) + { o << " " << N.size() << srnl; + for ( i=0; i\n\n"; + } */ + + // save texture coordinates per face (Ft) + /* Not yet supported so not saved + if ( Ft.size() ) + { o << " " << Ft.size() << srnl; + for ( i=0; i\n\n"; + } */ + + // save materials (M) + if ( M.size() ) + { o << " " << M.size() << srnl; + for ( i=0; i\n\n"; + } + + // save materials per face (Fm) + if ( Fm.size() ) + { o << " " << Fm.size() << srnl; + for ( i=0; i\n\n"; + } + + // save material names if there is one: + bool savemtl=false; + for ( i=0; i " << srnl; + for ( i=0; i\n\n"; + } + + // done. + return true; + } + +struct EdgeNode : public SrTreeNode // EdgeNode is only internally used : + { int a, b; + EdgeNode ( int x, int y ) : a(x), b(y) {} + EdgeNode () : a(0), b(0) {} + EdgeNode ( const EdgeNode& e ) : a(e.a), b(e.b) {} + ~EdgeNode () {} + friend SrOutput& operator<< ( SrOutput& out, const EdgeNode& e ) { return out; }; + friend SrInput& operator>> ( SrInput& inp, EdgeNode& e ) { return inp; } + friend int sr_compare ( const EdgeNode* e1, const EdgeNode* e2 ) + { return e1->a!=e2->a ? e1->a-e2->a : e1->b-e2->b; } + }; + +void SrModel::make_edges ( SrArray &E ) + { + int i; + + SrTree t; + + E.size(0); + if ( F.empty() ) return; + + for ( i=0; ia; + E.push() = t.cur()->b; + } + } + +float SrModel::count_mean_vertex_degree () + { + int i; + if ( F.empty() ) return 0.0f; + + SrArray vi(V.size()); + for ( i=0; i t; + + for ( i=0; i> ( SrInput& inp, VertexNode& v ) { return inp; } + friend int sr_compare ( const VertexNode* v1, const VertexNode* v2 ) + { return v1->v!=v2->v ? v1->v-v2->v // vertices are different + : v1->i-v2->i; // vertices are equal: use index i + } + }; + +static void insertv ( SrTree& t, SrArray& vi, int v, int f ) + { + // array vi is only used to generated a suitable tree key sorting the vertices. + VertexNode *n = new VertexNode(v,++vi[v],f); + if ( !t.insert(n) ) sr_out.fatal_error("Wrong faces in SrModel::smooth ()!\n"); + } + +int SrModel::common_vertices_of_faces ( int i1, int i2 ) + { + int i, j, c=0; + int *f1 = &(F[i1].a); + int *f2 = &(F[i2].a); + for ( i=0; i<3; i++ ) + { for ( j=0; j<3; j++ ) + { if ( f1[i]==f2[j] ) c++; //sr_out<& vec, SrArray& vi, SrModel *self, float crease_angle ) + { + int i, j, tmp; + float ang; + + vec.size(vi.size()); + + //sr_out<<"original:\n"; + //for ( i=0; iF[vi[i]].a<<","<F[vi[i]].b<<","<F[vi[i]].c<common_vertices_of_faces(vi[i],vi[j])==2 ) // share an edge + { SR_SWAP(vi[i+1],vi[j]); + break; + } + } + } + + // gen normals for each face around v: + for ( i=0; iface_normal ( vi[i] ); } + + // search for the first edge with a big angle and rearrange array, so + // that the array starts with a "crease angled edge": + bool angfound = false; + for ( i=0; icrease_angle ) + { for ( j=0; j<=i; j++ ) { vec.push()=vec[j]; vi.push()=vi[j]; } + vec.remove ( 0, i+1 ); + vi.remove ( 0, i+1 ); + angfound = true; + break; + } + } + if ( !angfound ) return; // no crease angles in this face cluster + + // Finally set the normals: + SrVec n; + float x=1.0f; + int init=0; + SrArray& N = self->N; + for ( i=0; icrease_angle ) + { n = SrVec::null; + x = 0.0f; + for ( j=init; j<=i; j++ ) { n+=vec[j]; x=x+1.0f; } + n /= x; // n is the mean normal of the previous set of smoothed faces around v + + for ( j=init; j<=i; j++ ) + { SrModel::Face &fn=self->Fn[vi[j]]; + /*if ( f.n<0 ) + { f.n=N.size(); N.insert(N.size(),3); N[f.n]=N[f.a]; N[f.n+1]=N[f.b]; N[f.n+2]=N[f.c]; } + if ( v==f.a ) N[f.n]=n; + else if ( v==f.b ) N[f.n+1]=n; + else N[f.n+2]=n; + */ + if ( v==fn.a ) fn.a = N.size(); + else if ( v==fn.b ) fn.b = N.size(); + else fn.c = N.size(); + N.push() = n; + } + + init = i+1; + } + } + } + +void SrModel::smooth ( float crease_angle ) + { + int v, i; + SrTree t; + SrArray vi; + SrArray vec; // this is just a buffer to be used in gen_normal() + + if ( !V.size() || !F.size() ) return; + + Fn.size ( F.size() ); + + vi.size(V.size()); + for ( i=0; iv; + vi.push() = t.cur()->f; + t.gonext(); + if ( t.cur()==SrTreeNode::null || v!=t.cur()->v ) + { SrVec n = SrVec::null; + for ( i=0; iv; + vi.push() = t.cur()->f; + t.gonext(); + if ( t.cur()==SrTreeNode::null || v!=t.cur()->v ) + { gen_normal ( v, vec, vi, this, crease_angle ); + vi.size(0); + } + } + + remove_redundant_normals (); + + compress (); + } + +SrVec SrModel::face_normal ( int f ) const + { + SrVec n; + const Face& fac = F[f]; + n.cross ( V[fac.b]-V[fac.a], V[fac.c]-V[fac.a] ); + n.normalize(); + return n; + } + +void SrModel::invert_faces () + { + int i, tmp; + for ( i=0; i0 ) + { N.size ( orign+m.N.size() ); + for ( i=0; i0 ) + { M.size ( origm+m.M.size() ); + for ( i=0; i170 ) + vr = cross ( SrVec::j, va ); + else + vr = cross ( SrVec::i, va ); + + vr.len ( c.radius ); // radial vector + + SrMat rot; + rot.rot ( va, dang ); + + SrPnt a1 = c.a+vr; + SrPnt b1 = c.b+vr; + SrPnt a2 = a1 * rot; + SrPnt b2 = a2 + vaxis; + + int i=1; + + do { if ( smooth ) + { N.push()=(a1-c.a)/c.radius; //normalized normal + } + V.push()=a1; V.push()=b1; + if ( i==nfaces ) break; + a1=a2; b1=b2; a2=a1*rot; b2=a2+vaxis; + i++; + } while ( true ); + + // make sides: + int n1, n=0; + int i1, i2, i3; + int size = V.size(); + for ( i=0; i faces; + faces.capacity(16); + for ( i=0; i=0 ) return (int)faces[closest]; + return closest; + } + +//================================ End of File ================================================= diff --git a/source/dcdt/se/sr_model.h b/source/dcdt/se/sr_model.h new file mode 100644 index 0000000000..a278a98e57 --- /dev/null +++ b/source/dcdt/se/sr_model.h @@ -0,0 +1,192 @@ + +# ifndef SR_MODEL_H +# define SR_MODEL_H + +/** \file sr_model.h + * 3d model based on a triangle list + */ + +class SrCylinder; +class SrSphere; + +# include "sr_box.h" +# include "sr_vec.h" +# include "sr_mat.h" +# include "sr_vec2.h" +# include "sr_line.h" +# include "sr_string.h" +# include "sr_material.h" +# include "sr_string_array.h" +# include "sr_shared_class.h" + +/*! \class SrModel sr_model.h + \brief a model composed of triangular faces + + SrModel keeps arrays of vertices, normals, texture coordinates, + materials, and face elements. + These arrays are public and can be directly manipulated by the user. + However please note that is the user responsability to maintain indices + in a coherent way. + Texture support is not finished. */ +class SrModel : public SrSharedClass + { public : + /*! A static string containing the name "model" */ + static const char* class_name; + + /*! The Face structure keeps 3 indices used to associate the + information in V, N, and T to each face */ + struct Face + { int a, b, c; + void set ( int i, int j, int k ) { a=i; b=j; c=k; } + void validate () { if (a<0) a=-a; if (b<0) b=-b; if (c<0) c=-c; } + friend SrOutput& operator<< ( SrOutput& o, const Face& f ) { return o<> ( SrInput& i, Face& f ) { return i>>f.a>>f.b>>f.c; } + }; + + SrArray M; //!< Used materials + SrArray V; //!< Vertices coordinates + SrArray N; //!< Normals table + SrArray T; //!< Texture coordinates + SrArray F; //!< Triangular faces indices to V + SrArray Fm; //!< Indices to the materials in M (size can be Fn; //!< Indices to the normals in N (size can be Ft; //!< Indices to the texture coordinates in T + + /*! Will be set to true (the default) if back face culling + should be applied, and false othrwise */ + bool culling; + + /*! Stores a name description for the materials in M. + Will have size 0 if no names defined, otherwise will have size M.size() */ + SrStringArray mtlnames; + + /*! May contain any desired name for the model. */ + SrString name; + + private : + + //SrImage* _texture; // to do... + + public : + + /*! Constructor lets all internal arrays as empty and culling is set to true */ + SrModel (); + + /*! Virtual Destructor */ + virtual ~SrModel (); + + /*! Returns true if the model has no faces, and false otherwise */ + bool empty () { return F.empty(); } + + /*! Sets to an empty model. Internal arrays are not compressed. */ + void init (); + + /*! Compress all internal array buffers. */ + void compress (); + + /*! Ensures that Fm, Fn, Ft arrays have the same size as F or have size 0. */ + void validate (); + + /*! Removes unreferenced or duplicated materials. */ + void remove_redundant_materials (); + + /*! Removes redundant normals, which are closer than the given angular distance. */ + void remove_redundant_normals ( float dang=SR_TORAD(0.5f) ); + + /*! Check and remove redundant vertices */ + void merge_redundant_vertices ( float prec ); + + /*! Reads a SrModel format. */ + bool load ( SrInput &in ); + + /*! Save in the SrModel format. */ + bool save ( SrOutput &o ) const; + + /*! Imports .obj file format. If the import is succesfull, true + is returned, and otherwise false is returned. */ + bool import_obj ( const char* file ); + + /*! Exports .iv file format. If the export is succesfull, true + is returned, and otherwise false is returned. */ + bool export_iv ( const char* file ); + + /*! Makes E to be an array containing the indices of the model edges. */ + void make_edges ( SrArray& E ); + + /*! Count and return the mean number of edges adjacent to a vertex in the model. */ + float count_mean_vertex_degree (); + + /*! Calculates the bounding box of this model. */ + void get_bounding_box ( SrBox &box ) const; + + /*! Translate the position of each vertex of the model. */ + void translate ( const SrVec &tr ); + + /*! Scale each vertex of the model. */ + void scale ( float factor ); + + /*! Translates so that the bounding box center becomes (0,0,0). */ + void centralize (); + + /*! Centralizes and scale to achieve maxcoord. */ + void normalize ( float maxcoord ); + + /*! Returns the number of common vertices between the two faces indices. */ + int common_vertices_of_faces ( int i, int j ); + + /*! Clear the N and Fn arrays, with compression. */ + void flat (); + + /*! Clear materials and then set M and Fm so that all triangles use the + same material m, with compression. */ + void set_one_material ( const SrMaterial& m ); + + /*! Clear material names, and the M and Fm array, with compression. */ + void clear_materials (); + + /*! Clear the T and Ft array (with compression). */ + void clear_textures (); + + /*! Generates normals smoothly, respecting the given crease + angle in radians. Compression is called in the end. + If the crease angle is <0, it is not considered and + only an overall smooth is done. */ + void smooth ( float crease_angle=SR_TORAD(35.0f) ); + + /*! Calculates and returns the normalized normal of the given face index. */ + SrVec face_normal ( int f ) const; + + /*! Inverts faces orientations by swaping b and c indices, + and does the same to the normals. */ + void invert_faces (); + + /*! Multiply all normals in N by -1 */ + void invert_normals (); + + /*! Multiply arrays V and N with the given matrix */ + void apply_transformation ( const SrMat& mat ); + + /*! Add faces, materials and normals of m to SrModel */ + void add_model ( const SrModel& m ); + + /*! Copy operator */ + void operator = ( const SrModel& m ); + + /*! Make a box shape */ + void make_box ( const SrBox& b ); + + /*! Make a sphere shape */ + void make_sphere ( const SrSphere& s, float resolution=1 ); + + /*! Make a cylinder shape */ + void make_cylinder ( const SrCylinder& c, float resolution=1, bool smooth=true ); + + /*! Returns the index of the face intersecting with the line, or -1 if + no face is found. In case several intersections are found, the closest + to line.p1 is returned */ + int pick_face ( const SrLine& line ) const; + }; + +//================================ End of File ================================================= + +# endif // SR_MODEL_H diff --git a/source/dcdt/se/sr_model_export_iv.cpp b/source/dcdt/se/sr_model_export_iv.cpp new file mode 100644 index 0000000000..c5fb04be71 --- /dev/null +++ b/source/dcdt/se/sr_model_export_iv.cpp @@ -0,0 +1,85 @@ +#include "precompiled.h" +# include "sr_model.h" + +//# define SR_USE_TRACE1 // keyword tracking +# include "sr_trace.h" + + +static void wcolor ( SrColor c, SrOutput& o ) + { + float f[4]; + c.get(f); + o << f[0] << srspc << f[1] << srspc << f[2]; + } + +bool SrModel::export_iv ( const char* file ) + { + int i; + SrOutput o ( fopen(file,"wt") ); + if ( !o.valid() ) return false; + + // ===== Header ================================= + o << "#VRML V1.0 ascii\n\n"; + o << "Separator {\n\n"; + o << " Info { string \"Produced by SR Toolkit exporter v0.5, M. Kallmann 2003\" }\n\n"; + + // ===== Hints ================================== + o << " ShapeHints {\n"; + if ( culling ) o << " shapeType SOLID\n"; + o << " vertexOrdering COUNTERCLOCKWISE\n"; + o << " faceType CONVEX\n"; + o << " }\n\n"; + + //===== Materials =============================== + if ( M.size()==1 ) + { o << " Material {\n"; + o << " diffuseColor "; wcolor(M[0].diffuse,o); o<1 ) + { o << " Material {\n"; + o << " diffuseColor [\n"; + for ( i=0; i1 ) + { o << " materialIndex [\n"; + for ( i=0; i>n; if (n>0) n--; else if (n<0) n+=A.size() + +static void get_face ( SrInput& in, SrModel& m, int& vc, int& vt, int& vn ) + { + vc = vt = vn = -1; + GETID(vc,m.V); + + if ( in.get_token()!=SrInput::Delimiter ) // was only: vc + { in.unget_token(); return; } + + if ( in.get_token()==SrInput::Delimiter ) // is: vc//vn + { GETID(vn,m.N); + return; + } + + in.unget_token(); + + GETID(vt,m.T); // is: vc/vt + + if ( in.get_token()==SrInput::Delimiter ) + { GETID(vn,m.N); // is: vc/vt/vn + return; + } + + if ( !in.finished() ) in.unget_token (); // is only: vc/vt + } + +static SrColor read_color ( SrInput& in ) + { + float r, g, b; + in >> r >> g >> b; + SrColor c(r,g,b); + return c; + } + +static void read_materials ( SrArray& M, + SrStringArray& mnames, + const SrString& file, + const SrStringArray& paths ) + { + SrString s; + SrInput in; + + in.init ( fopen(file,"rt") ); + int i=0; + while ( !in.valid() && i> i; + M.top().shininess = i; + } + else if ( in.last_token()=="illum" ) // dont know what is this one + { in >> i; + } + } + } + +static bool process_line ( const SrString& line, + SrModel& m, + SrStringArray& paths, + SrStringArray& mnames, + int& curmtl ) + { + SrInput in (line); + in.get_token(); + + if ( in.last_token().len()==0 ) return true; + + if ( in.last_token()=="v" ) // v x y z [w] + { SR_TRACE1 ( "v" ); + m.V.push(); + in >> m.V.top(); + } + else if ( in.last_token()=="vn" ) // vn i j k + { SR_TRACE1 ( "vn" ); + m.N.push(); + in >> m.N.top(); + } + else if ( in.last_token()=="vt" ) // vt u v [w] + { SR_TRACE1 ( "vt" ); + m.T.push(); + in >> m.T.top(); + } + else if ( in.last_token()=="g" ) + { SR_TRACE1 ( "g" ); + } + else if ( in.last_token()=="f" ) // f v/t/n v/t/n v/t/n (or v/t or v//n or v) + { SR_TRACE1 ( "f" ); + int i=0; + SrArray v, t, n; + v.capacity(8); t.capacity(8); n.capacity(8); + while ( true ) + { if ( in.get_token()==SrInput::EndOfFile ) break; + in.unget_token(); + get_face ( in, m, v.push(), t.push(), n.push() ); + if ( in.last_error()==SrInput::UnexpectedToken ) return false; + } + + if ( v.size()<3 ) return false; + + for ( i=2; i=0 && t[1]>=0 && t[i]>=0 ) + m.Ft.push().set( t[0], t[i-1], t[i] ); + + if ( n[0]>=0 && n[1]>=0 && n[i]>=0 ) + m.Fn.push().set ( n[0], n[i-1], n[i] ); + } + } + else if ( in.last_token()=="s" ) // smoothing groups not supported + { SR_TRACE1 ( "s" ); + in.get_token(); + } + else if ( in.last_token()=="usemap" ) // usemap name/off + { SR_TRACE1 ( "usemap" ); + } + else if ( in.last_token()=="usemtl" ) // usemtl name + { SR_TRACE1 ( "usemtl" ); + in.get_token (); + curmtl = mnames.lsearch ( in.last_token() ); + SR_TRACE1 ( "curmtl = " << curmtl << " (" << in.last_token() << ")" ); + } + else if ( in.last_token()=="mtllib" ) // mtllib file1 file2 ... + { SR_TRACE1 ( "mtllib" ); + SrString token, file; + int pos = line.get_next_string ( token, 0 ); // parse again mtllib + while ( true ) + { pos = line.get_next_string ( token, pos ); + if ( pos<0 ) break; + token.extract_file_name ( file ); + token.replace ( "\\", "/" ); // avoid win-unix problems + SR_TRACE1 ( "new path: "< +# include +# include + +# include "sr_output.h" +# include "sr_string.h" + +//============================== Global data ==================================== + +SrOutput sr_out; + +static char* DefaultIntFmt = "%d"; +static char* DefaultFloatFmt = "g"; +static char* DefaultDoubleFmt = "g"; + +//=============================== SrOutput ====================================== + +void SrOutput::_init () // this can be only called from constructors + { + _type = TypeConsole; + _margin = 0; + _margin_char = '\t'; + _intfmt = DefaultIntFmt; + _floatfmt = DefaultFloatFmt; + _doublefmt = DefaultDoubleFmt; + _func_udata = 0; + _filename = 0; + default_formats(); + } + +SrOutput::SrOutput () + { + _init (); + } + +SrOutput::SrOutput ( FILE *f ) + { + _init (); + _type = TypeFile; + _device.file = f; + } + +SrOutput::SrOutput ( const char* filename, const char* mode ) + { + _init (); + _type = TypeFile; + _device.file = fopen(filename,mode); + sr_string_set(_filename,filename); + } + +SrOutput::SrOutput ( SrString &s ) + { + _init (); + _type = TypeString; + _device.string=&s; + } + +SrOutput::SrOutput ( void(*f)(const char*,void*), void* udata ) + { + _init (); + _type = TypeFunction; + _func_udata = udata; + _device.func = f; + } + +SrOutput::~SrOutput () + { + close (); + default_formats (); // will free pointers only if needed (no defaults used) + } + +void SrOutput::leave_file () + { + _device.file = 0; + _type=TypeConsole; + } + +FILE* SrOutput::filept () + { + return _type==TypeFile? _device.file:0; + } + +void SrOutput::init () + { + close(); + _type=TypeConsole; + } + +void SrOutput::init ( FILE *f ) + { + close(); + _type=TypeFile; + _device.file=f; + } + +void SrOutput::init ( const char* filename, const char* mode ) + { + init ( fopen(filename,mode) ); + sr_string_set(_filename,filename); + } + +void SrOutput::init ( SrString &s ) + { + close(); + _type=TypeString; + _device.string=&s; + } + +void SrOutput::init ( void(*f)(const char*,void*), void* udata ) + { + close(); + _type=TypeFunction; + _device.func=f; + _func_udata = udata; + } + +bool SrOutput::valid () + { + switch ( _type ) + { case TypeConsole : return true; + case TypeFile : return _device.file ? true:false; + case TypeString : return _device.string ? true:false; + case TypeFunction : return _device.func ? true:false; + } + return false; + } + +void SrOutput::close () + { + if ( _type==TypeFile ) + { if ( _device.file ) + { fclose ( _device.file ); + _device.file = 0; + } + } + sr_string_set ( _filename, 0 ); + } + +static void set_fmt ( char*& fmt, char* def, const char* newfmt ) + { + if ( fmt==def ) fmt=0; + else sr_string_set ( fmt, 0 ); + + if ( newfmt==def ) fmt=def; + else sr_string_set ( fmt, newfmt ); + } + +void SrOutput::fmt_int ( const char* s ) + { + set_fmt ( _intfmt, DefaultIntFmt, s ); + } + +void SrOutput::fmt_float ( const char* s ) + { + set_fmt ( _floatfmt, DefaultFloatFmt, s ); + } + +void SrOutput::fmt_double ( const char* s ) + { + set_fmt ( _doublefmt, DefaultDoubleFmt, s ); + } + +void SrOutput::outm () + { + int i; + for ( i=0; i<_margin; i++ ) put ( _margin_char ); + } + +void SrOutput::flush () + { + switch ( _type ) + { case TypeConsole : fflush(stdout); break; + case TypeFile : fflush(_device.file); break; + case TypeString : break; + case TypeFunction : break; + } + } + +void SrOutput::default_formats () + { + fmt_int ( DefaultIntFmt ); + fmt_float ( DefaultFloatFmt ); + fmt_double ( DefaultDoubleFmt ); + } + +void SrOutput::put ( char c ) + { + switch ( _type ) + { case TypeConsole : fputc(c,stdout); break; + case TypeFile : fputc(c,_device.file); break; + case TypeString : { char st[2]; st[0]=c; st[1]=0; _device.string->append(st); } break; + case TypeFunction : { char st[2]; st[0]=c; st[1]=0; _device.func(st,_func_udata); } break; + } + } + +void SrOutput::put ( const char *st ) + { + switch ( _type ) + { case TypeConsole : fputs(st,stdout); break; + case TypeFile : fputs(st,_device.file); break; + case TypeString : _device.string->append(st); break; + case TypeFunction : _device.func(st,_func_udata); break; + } + } + +void SrOutput::putf ( const char *fmt, ... ) + { + char buf[256]; + va_list args; + + buf[0] = 0; + + va_start ( args, fmt ); + vsprintf ( buf, fmt, args ); + va_end ( args ); + + put ( buf ); + } + +void SrOutput::fatal_error ( const char *fmt, ... ) + { + char buf[256]; + va_list args; + + buf[0] = 0; + + va_start ( args, fmt ); + vsprintf ( buf, fmt, args ); + va_end ( args ); + + put ( "\nSR Fatal Error !\n" ); + put ( buf ); + put ( "\nPress any key to exit..." ); + flush (); + getchar(); + put ( "\n" ); + + exit ( 1 ); + } + +void SrOutput::warning ( const char *fmt, ... ) + { + char buf[256]; + va_list args; + + buf[0] = 0; + + va_start ( args, fmt ); + vsprintf ( buf, fmt, args ); + va_end ( args ); + + put ( "\nSR Warning:\n" ); + put ( buf ); + put ( "\n" ); + flush (); + } + +//=============================== friends =============================== + +SrOutput& operator<< ( SrOutput& o, const char *st ) + { + o.put(st); + return o; + } + +SrOutput& operator<< ( SrOutput& o, const SrString& st ) + { + o.put((const char*)st); + return o; + } + +SrOutput& operator<< ( SrOutput& o, char c ) + { + o.put(c); + return o; + } + +SrOutput& operator<< ( SrOutput& o, srbyte c ) + { + o.putf(o._intfmt,(int)c); + return o; + } + +SrOutput& operator<< ( SrOutput& o, int i ) + { + o.putf(o._intfmt,i); + return o; + } + +SrOutput& operator<< ( SrOutput& o, bool b ) + { + o.put ( b? "true":"false" ); + return o; + } + +SrOutput& operator<< ( SrOutput& o, short s ) + { + o.putf(o._intfmt,(int)s); + return o; + } + +SrOutput& operator<< ( SrOutput& o, sruint i ) + { + o.putf(o._intfmt,(int)i); + return o; + } + +SrOutput& operator<< ( SrOutput& o, float f ) + { + if ( o._floatfmt[0]=='g' ) + { if ( f==int(f) ) + o.putf("%d.0",int(f)); + else + o.putf("%g",f); + } + else if ( o._floatfmt[0]=='i' ) + { if ( f==int(f) ) + o.putf("%d",int(f)); + else + o.putf("%f",f); + } + else o.putf(o._floatfmt,f); + + return o; + } + +SrOutput& operator<< ( SrOutput& o, double d ) + { + if ( o._doublefmt[0]=='g' ) + { o.putf("%g",d); + if ( d==int(d) ) o<<".0"; + } + else if ( o._doublefmt[0]=='i' ) + { if ( d==int(d) ) + o.putf("%d",int(d)); + else + o.putf("%f",d); + } + else o.putf(o._doublefmt,d); + return o; + } + +//============================== end of file =============================== diff --git a/source/dcdt/se/sr_output.h b/source/dcdt/se/sr_output.h new file mode 100644 index 0000000000..fba4e96bf8 --- /dev/null +++ b/source/dcdt/se/sr_output.h @@ -0,0 +1,206 @@ + +# ifndef SR_OUTPUT_H +# define SR_OUTPUT_H + +/** \file sr_output.h + * Output to file, function, string or stdout */ + +# include +# include "sr.h" + +class SrString; + +/*! \class SrOutput sr_output.h + \brief Generic output class + + Outputs to a file, function, string or stdout. SrOutput has all basic + functionalities of ostream, but with the ability to easily redirect the + output and using the old C standard library ouput functions. */ +class SrOutput + { public : + /*! The type of the ouput, that can be connected to a file, function, string or stdout. */ + enum Type { TypeConsole, //!< Output is directed to the console + TypeFile, //!< Output is directed to a file + TypeString, //!< Output is directed to a SrString + TypeFunction //!< Output is directed to a function + }; + + private : + Type _type; + int _margin; + char _margin_char; + union { SrString *string; + void (*func) (const char *,void*); + FILE *file; + } _device; + + void* _func_udata; // used only when the output is a function + char* _intfmt; // format used to print integers + char* _floatfmt; // format used to print floats + char* _doublefmt; // format used to print doubles + char* _filename; // optional file name of the open file + void _init (); + + public : + /*! Default constructor: inits the output to the console. */ + SrOutput (); + + /*! Constructor that inits the output to a file. */ + SrOutput ( FILE *f ); + + /*! Constructor to a file, which is open with fopen(filename,mode). + The filename is stored. */ + SrOutput ( const char* filename, const char* mode ); + + /*! Constructor that inits the output to a SrString. */ + SrOutput ( SrString &s ); + + /*! Constructor that inits the output to a function. The function receives + the string to output and a void pointer to use as user data. */ + SrOutput ( void(*f)(const char*,void*), void* udata ); + + /*! The destructor will close the associated output device. In fact, + only in the case of a file ouput that closing the device is done. + The destructor is implemented inline by calling the close() method. */ + ~SrOutput (); + + /*! If output is done to a file, do not close it, and redirects current ouput + to the console. */ + void leave_file (); + + /*! If the output is done to a file, return the FILE pointer associated, otherwise + will return 0 */ + FILE* filept (); + + /*! Closes current ouput and redirects it to the console. */ + void init (); + + /*! Closes current ouput and redirects it to a file. */ + void init ( FILE *f ); + + /*! Closes current ouput and redirects it to a file, which is open with + fopen(filename,mode). The filename is stored and can be retrieved with filename. */ + void init ( const char* filename, const char* mode ); + + /*! Closes current ouput and redirects it to a SrString. */ + void init ( SrString &s ); + + /*! Closes current ouput and redirects it to a function. The function + receives the string to output and a user data void pointer. */ + void init ( void(*f)(const char*,void*), void* udata ); + + /*! Returns the type of the output. Implemented inline. */ + Type type () const { return _type; } + + /*! Returns the file name used for opening a file output, or null if not available */ + const char* filename () const { return _filename; } + + /*! Associates with the output a file name. The string is stored but not used by SrOutput. */ + void filename ( const char* s ) { sr_string_set(_filename,s); } + + /*! Returns the current margin being used. The number returned represents + the number of tabs or spaces to be used as current margin */ + int margin () const { return _margin; } + + /*! Changes the current margin being used. */ + void margin ( int m ) { _margin=m; } + + /*! Set margin character, default is '\t' */ + void margin_char ( char c ) { _margin_char=c; } + + /*! returns the current margin character. */ + char margin_char () const { return _margin_char; } + + /*! Changes the current margin being used by adding parameter inc to the + current value. Normally, inc is passed as 1 or -1. Implemented inline. */ + void incmargin ( int inc ) { _margin+=inc; } + + /*! Checks whether the associated output device pointer is valid. + A console type output will always be valid, returning true. Others output devices + are considered valid if their access pointers are not zero */ + bool valid (); + + /*! Will close the associated output device if this is the case. + If SrOutput is associated with a file, and if the file pointer is not zero, then + fclose() will be called to close this file and SrOutput will have a new null file + pointer. For other devices, nothing is done. The filename is set to null. */ + void close (); + + /*! Sets the format to print numbers in "printf format", e.g "%d", etc. */ + void fmt_int ( const char* s ); + void fmt_float ( const char* s ); + void fmt_double ( const char* s ); + + const char* fmt_int () { return _intfmt; } + const char* fmt_float () { return _floatfmt; } + const char* fmt_double () { return _doublefmt; } + + /*! Outputs the margin as defined by methods margin() and margin_char(). */ + void outm (); + + /*! Will fflush() when output is a file or console. */ + void flush (); + + /*! Sets the SrString formats intfmt, floatfmt, and doublefmt to their default values, + that are: "%d", "g", and "g". Format "g" is interpreted as a "%g", but with + an added ".0" in case the real number is equal to its integer part. + Format "i" will output real numbers as integer when equal to their integer part */ + void default_formats (); + + /*! Outputs a character to the connected device. No validity checkings are done. */ + void put ( char c ); + + /*! Outputs a string to the connected device. No validity checkings are done. */ + void put ( const char *st ); + + /*! Outputs a formated string to the connected device. The format string is the same + as for the printf() function. An internal 256 bytes buffer is used to translate + the format string. And then put() method is called. */ + void putf ( const char *fmt, ... ); + + /*! Outputs a formatted error message and calls exit(1). */ + void fatal_error ( const char *fmt, ... ); + + /*! Outputs a formatted warning message, but does not call exit(). */ + void warning ( const char *fmt, ... ); + + /*! Outputs a string using the put() method. */ + friend SrOutput& operator<< ( SrOutput& o, const char *st ); + + /*! Outputs a SrString using the put() method. */ + friend SrOutput& operator<< ( SrOutput& o, const SrString& st ); + + /*! Outputs a character using the put() method. */ + friend SrOutput& operator<< ( SrOutput& o, char c ); + + /*! Outputs a srbyte using intfmt with the putf() method. */ + friend SrOutput& operator<< ( SrOutput& o, srbyte c ); + + /*! Outputs an int using intfmt with the putf() method. */ + friend SrOutput& operator<< ( SrOutput& o, int i ); + + /*! Outputs a bool as "true" or "false" with the put() method. */ + friend SrOutput& operator<< ( SrOutput& o, bool b ); + + /*! Outputs a short int using intfmt with the putf() method. */ + friend SrOutput& operator<< ( SrOutput& o, short s ); + + /*! Outputs a sruint using intfmt with the putf() method. */ + friend SrOutput& operator<< ( SrOutput& o, sruint i ); + + /*! Outputs a float using floatfmt with the putf() method. */ + friend SrOutput& operator<< ( SrOutput& o, float f ); + + /*! Outputs a double using doublefmt with the putf() method. */ + friend SrOutput& operator<< ( SrOutput& o, double d ); + }; + +/*! Used to output error messages and by the trace functions It can be + redirected with init() if needed. */ +extern SrOutput sr_out; + +//============================== end of file =============================== + + +# endif // SR_OUTPUT_H + diff --git a/source/dcdt/se/sr_output_window.h b/source/dcdt/se/sr_output_window.h new file mode 100644 index 0000000000..750e6b9eae --- /dev/null +++ b/source/dcdt/se/sr_output_window.h @@ -0,0 +1,90 @@ + +# ifndef SR_OUTPUT_WINDOW_H +# define SR_OUTPUT_WINDOW_H + +/** \file sr_output_window.h + * Manages a fltk output window. */ + +class SrOutput; +class Fl_Window; +class Fl_Browser; + +/*! \class SrOutputWindow sr_output_window.h + \brief Fltk output window + + This class manages a fltk output window, that can capture and + display any data sent to a SrOutput. */ +class SrOutputWindow + { private : + Fl_Window* _window; + Fl_Browser* _browser; + char* _buffer; + int _bufcur; + int _bufcap; + bool _activated; + bool _already_shown; + void _init (); + int _buffer_len; + + public : + + /*! Default constructor */ + SrOutputWindow (); + + /*! Constructor which connects the window to the given output o */ + SrOutputWindow ( SrOutput& o ); + + /*! Destructor */ + ~SrOutputWindow (); + + /*! Change the number of lines kept in the window, default is 512 */ + void buffer_len ( int len ) { _buffer_len=len; } + + /*! Activate or deactivate the output window. If the window is + deactivated, all messages to be displayed are not displayed, + and are simply ignored. */ + void activated ( bool b ) { _activated=b; } + + /*! Returns the activation state of the window. */ + bool activated () { return _activated; } + + /* Writes the content of the output window to a file. */ + void save ( const char* fname ); + + /*! Display text in the output window */ + void output ( const char *s ); + + /*! The window will automatically open with the first output, but + it can be also showed using this function */ + void show (); + + /*! Returns true if the window is shown, and false otherwise */ + bool shown (); + + /* Will hide the window */ + void hide (); + + /* Clears all the window content */ + void clear (); + + /* Makes sure the first line is visible */ + void show_first_line (); + + /* Change the title of the window */ + void title ( const char* t ); + + /*! Will reinitialize o to output to an internal function which sends the + text output to SrOutputWindow. To disconnect, o should be initialized again */ + void connect ( SrOutput& o ); + + /*! Will forever enter the fltk main loop, which will manage the window. */ + static void run (); + + /*! Updates all fltk widgets. */ + static void check (); + }; + +//================================ End of File ================================================= + +# endif // SR_OUTPUT_WINDOW_H + diff --git a/source/dcdt/se/sr_path_array.cpp b/source/dcdt/se/sr_path_array.cpp new file mode 100644 index 0000000000..28e1bc224b --- /dev/null +++ b/source/dcdt/se/sr_path_array.cpp @@ -0,0 +1,28 @@ +#include "precompiled.h" +# include "sr_path_array.h" + +//# define SR_USE_TRACE1 +# include "sr_trace.h" + +//====================== SrPathArray ========================== + +void SrPathArray::basedir_from_filename ( const char* file ) + { + _basedir = file; + _basedir.remove_file_name(); + _basedir.make_valid_path(); + } + +bool SrPathArray::adjust_path ( SrString& file ) + { + SrInput in; + if ( open(in,file) ) + { file = in.filename(); + return true; + } + else + { return false; + } + } + +//=========================== EOF =============================== diff --git a/source/dcdt/se/sr_path_array.h b/source/dcdt/se/sr_path_array.h new file mode 100644 index 0000000000..4c70ba9bde --- /dev/null +++ b/source/dcdt/se/sr_path_array.h @@ -0,0 +1,57 @@ + +# ifndef SR_PATH_ARRAY_H +# define SR_PATH_ARRAY_H + +/** \file sr_path_array.h + * array of file paths */ + +# include "sr_string_array.h" + +//============================== SrPathArray =============================== + +/*! \class SrPathArray sr_path_array.h + \brief array of file paths + This class helps maintaining a list of file paths and + opening input files according to the declared paths and + absolute/relative path/filename cases. */ +class SrPathArray : public SrStringArray + { private: + SrString _basedir; // the basedir of relative filenames (has to have a slash in the end) + + public: + /*! Access to the base dir */ + SrString& basedir () { return _basedir; } + + /*! Set the basedir and ensures its validity, eg, adding a slash in the end if needed. */ + void basedir ( const char* bd ) + { _basedir.make_valid_path(bd); } + + /*! Set the basedir, extracting it from a filename and ensuring its validity */ + void basedir_from_filename ( const char* file ); + + /*! Add a path to the array. No validity checks done. */ + void push ( const char* path ) { push_path(path); } + + /*! Tries to open an input file: + If the given filename has an absolute path in it, the method simply + tries to open it and returns right after. Otherwise: + - First search in the paths stored in the array. + If basedir exists (not null), relative paths in the array + are made relative (concatenated) to basedir. + - Second, try to open filename using only the basedir path (if given). + - Finally tries to simply open filename. + Path basedir, if given, has to be a valid path name (with a slash in the end). + Returns true if the file could be open. In such case, the successfull full file + name can be found in inp.filename(). False is returned in case of failure. + Parameter mode is the fopen() mode: "rt", etc. */ + bool open ( SrInput& inp, const char* filename ) { return open_file(inp,filename,"rt",_basedir); } + + /*! Checks if file exists by testing all declared paths using the rules in open(). + Returns false if the file was not found, otherwise file will contain the + succesfull file name and true is returned */ + bool adjust_path ( SrString& file ); + }; + +#endif // SR_PATH_ARRAY_H + +//============================== end of file =============================== diff --git a/source/dcdt/se/sr_plane.cpp b/source/dcdt/se/sr_plane.cpp new file mode 100644 index 0000000000..42cec1794b --- /dev/null +++ b/source/dcdt/se/sr_plane.cpp @@ -0,0 +1,106 @@ +#include "precompiled.h" +# include "sr_plane.h" + +//============================== SrPlane ====================================== + +// Important: Static initializations cannot use other static initialized +// variables ( as SrVec::i, ... ), as we cannot know the order that they +// will be initialized by the compiler. + +const SrPlane SrPlane::XY ( SrVec(0,0,0), SrVec(1.0f,0,0), SrVec(0,1.0f,0) ); +const SrPlane SrPlane::XZ ( SrVec(0,0,0), SrVec(1.0f,0,0), SrVec(0,0,1.0f) ); +const SrPlane SrPlane::YZ ( SrVec(0,0,0), SrVec(0,1.0f,0), SrVec(0,0,1.0f) ); + +SrPlane::SrPlane () : coords ( SrVec::k ), coordsw(0) + { + } + +SrPlane::SrPlane ( const SrVec& center, const SrVec& normal ) + { + set ( center, normal ); + } + +SrPlane::SrPlane ( const SrVec& p1, const SrVec& p2, const SrVec& p3 ) + { + set ( p1, p2, p3 ); + } + +bool SrPlane::set ( const SrVec& center, const SrVec& normal ) + { + coords = normal; + coordsw = -dot(normal,center); + float n = normal.norm(); + if (n==0.0) return false; + coords/=n; coordsw/=n; + return true; + } + +bool SrPlane::set ( const SrVec& p1, const SrVec& p2, const SrVec& p3 ) + { + SrVec normal = cross ( p2-p1, p3-p1 ); + return set ( p1, normal ); + } + +bool SrPlane::parallel ( const SrVec& p1, const SrVec& p2, float ds ) const + { + float fact = dot ( coords, p1-p2 ); + return SR_NEXTZ(fact,ds)? true:false; + } + +/*! Returns p, that is the intersection between plane and infinity line of , + (0,0,0) is returned if they are parallel. */ +SrVec SrPlane::intersect ( const SrVec& p1, const SrVec& p2, float *t ) const + { + float fact = dot ( coords, p1-p2 ); + if ( fact==0.0 ) return SrVec::null; + float k = (coordsw+dot(coords,p1)) / fact; + if (t) *t=k; + return lerp ( p1, p2, k ); + } + +/* my old functions : + +float Dist ( const OVec3f &p, const OVec4f &plane ) + { + float c = plane.x*p.x + plane.y*p.y + plane.z*p.z + plane.w; + return ABS(c); + } + +bool Dist ( float &dist, const OVec3f &p, + const OVec3f &p1, const OVec3f &p2, const OVec3f &p3 ) + { + OVec4f plane; + if ( !PlaneCoords(p1,p2,p3,plane) ) return false; + dist = ::Dist(p,plane); + return true; + } + +float Dist ( const OVec3f &p, const OVec3f &p0, const OVec3f &n ) + { + OVec3f w ( p-p0 ); + w = Cross ( n, w ); + return w.Norm(); + } + +float IsParallel ( const OVec4f &plane, + const OVec3f &p1, const OVec3f &p2 ) + { + OVec3f p(plane.x,plane.y,plane.z); + float fact = Dot( p, p1-p2 ); + return ABS(fact); + } + +bool InterSeg ( const OVec4f &plane, + const OVec3f &p1, const OVec3f &p2, OVec3f &p ) + { + OVec3f pl(plane.x,plane.y,plane.z); + float t, fact = Dot( pl, p1-p2 ); + if ( fact==0.0 ) return false; + t = (plane.w+Dot(pl,p1)) / fact; + if ( t<0.0 || t>1.0 ) return false; + p = Evaluate( p1, p2, t ); + return true; + } + +*/ + diff --git a/source/dcdt/se/sr_plane.h b/source/dcdt/se/sr_plane.h new file mode 100644 index 0000000000..9bcb23338a --- /dev/null +++ b/source/dcdt/se/sr_plane.h @@ -0,0 +1,45 @@ + +# ifndef SR_PLANE_H +# define SR_PLANE_H + +# include "sr_vec.h" + +class SrPlane + { public : + SrVec coords; + float coordsw; + static const SrPlane XY, //. (0,0,0) is returned if they are parallel. Use parallel() + to test this before. If t is non null, t will contain the relative + interpolation factor such that p=p1(1-t)+p2(t). */ + SrVec intersect ( const SrVec& p1, const SrVec& p2, float *t=0 ) const; + }; + +# endif // SR_PLANE_H diff --git a/source/dcdt/se/sr_points.cpp b/source/dcdt/se/sr_points.cpp new file mode 100644 index 0000000000..b47cd98330 --- /dev/null +++ b/source/dcdt/se/sr_points.cpp @@ -0,0 +1,104 @@ +#include "precompiled.h" +# include "sr_box.h" +# include "sr_vec2.h" +# include "sr_points.h" + +//# define SR_USE_TRACE1 // Constructor and Destructor +# include "sr_trace.h" + +//======================================= SrPoints ==================================== + +const char* SrPoints::class_name = "Points"; + + +SrPoints::SrPoints () + { + SR_TRACE1 ( "Constructor" ); + A = 0; + } + +SrPoints::~SrPoints () + { + SR_TRACE1 ( "Destructor" ); + delete A; + } + +void SrPoints::init () + { + P.size(0); + } + +void SrPoints::init_with_attributes () + { + P.size(0); + if ( A ) + A->size(0); + else A = new SrArray; + } + +void SrPoints::compress () + { + P.compress(); + if ( A ) A->compress(); + } + +void SrPoints::push ( const SrPnt& p ) + { + P.push() = p; + if ( A ) A->push().s=1; + } + +void SrPoints::push ( const SrPnt2& p ) + { + P.push().set ( p.x, p.y, 0 ); + if ( A ) A->push().s=1; + } + +void SrPoints::push ( float x, float y, float z ) + { + P.push().set(x,y,z); + if ( A ) A->push().s=1; + } + +void SrPoints::push ( const SrPnt& p, SrColor c, float size ) + { + if ( !A ) return; + P.push() = p; + A->push().s=size; + A->top().c = c; + } + +void SrPoints::push ( const SrPnt2& p, SrColor c, float size ) + { + if ( !A ) return; + P.push().set ( p.x, p.y, 0 ); + A->push().s=size; + A->top().c = c; + } + +void SrPoints::push ( float x, float y, SrColor c, float size ) + { + if ( !A ) return; + P.push().set(x,y,0); + A->push().s=size; + A->top().c = c; + } + +void SrPoints::push ( float x, float y, float z, SrColor c, float size ) + { + if ( !A ) return; + P.push().set(x,y,z); + A->push().s=size; + A->top().c = c; + } + +void SrPoints::get_bounding_box ( SrBox& b ) const + { + int i; + b.set_empty (); + for ( i=0; i P; //* A; // +# include "sr_box.h" +# include "sr_geo2.h" +# include "sr_polygon.h" + +//# define SR_USE_TRACE1 +# include "sr_trace.h" + +//=================================== SrPolygon ================================================= + +const char* SrPolygon::class_name = "Polygon"; + +SrPolygon::SrPolygon ( int s, int c ) : SrArray ( s, c ) + { + _open = 0; + } + +SrPolygon::SrPolygon ( const SrPolygon& p ) : SrArray ( p ) + { + _open = p._open; + } + +SrPolygon::SrPolygon ( SrVec2* pt, int s, int c ) : SrArray ( pt, s, c ) + { + _open = 0; + } + +void SrPolygon::set_from_float_array ( const float* pt, int numv ) + { + if ( numv<0 ) return; + size ( numv ); + for ( int i=0; i0 if ccw + { + float sum=0; + int i, j; + + if ( size()<=2 ) return 0; // degenerated polygon with 1 or 2 edges + + for ( i=0; ip2.y ) { SrVec2 tmp; SR_SWAP(p1,p2); } + if ( p1.y>=p.y ) return false; // not intercepting + if ( p2.y0 ) + { p.set ( radius*sinf(ang), radius*cosf(ang) ); + p += center; + push ( p ); + ang += incang; + nvertices--; + } + _open = 0; + } + +float SrPolygon::perimeter () const + { + if ( size()<2 ) return 0; + int i; + float len=0; + for ( i=1; imaxlen ) + { nsub = (int)(len/maxlen); + len = len/(nsub+1); + v2 = v2 - v1; + v2.len(len); + while ( nsub>0 ) + { v1+=v2; obsamp.push()=v1; nsub--; } + } + } + take_data ( obsamp ); + } + +void SrPolygon::remove_duplicated_vertices ( float epsilon ) + { + int i1, i2; + SrVec2 v1, v2; + epsilon *= epsilon; + i1 = 0; + while ( i1=size()-2 ) break; + i1 = (i+1)%size(); + i2 = (i+2)%size(); + if ( sr_point_line_dist ( get(i).x, get(i).y, + get(i1).x, get(i1).y, + get(i2).x, get(i2).y )<=epsilon ) + remove(i1); + else + i++; + } + } + +//# define SE_CCW(ax,ay,bx,by,cx,cy) ((ax*by) + (bx*cy) + (cx*ay) - (bx*ay) - (cx*by) - (ax*cy)) +//# define VCCW(p1,p2,p3) SE_CCW(p1.x,p1.y,p2.x,p2.y,p3.x,p3.y) + +static void grow_corner + ( const SrVec2& v1, const SrVec2& v2, const SrVec2& v3, + float r, float maxang, bool grow, SrPolygon& obs ) + { + enum MoreOrLessThanPi { LESS_THAN_PI, MORE_THAN_PI }; + MoreOrLessThanPi type; + SrVec2 vec1, vec2; + + if ( grow ) + type = ccw(v1,v2,v3)>=0? MORE_THAN_PI:LESS_THAN_PI; + else + type = ccw(v1,v2,v3)>=0? LESS_THAN_PI:MORE_THAN_PI; + + vec1 = v1-v2; vec1.normalize(); + vec2 = v3-v2; vec2.normalize(); + + // get corner separator points: + if ( type==LESS_THAN_PI ) // get the bissector point + { float ang = angle_fornormvecs(vec1,vec2) / 2.0f; // angle divided by 2 + float sinang = sinf(ang); + if ( !grow ) ang=-ang; + SrVec2 x (vec1); + x.rot ( sinf(ang), cosf(ang) ); + x *= r/sinang; + x += v2; + +/* if ( grow ) + { type = ccw(v1,x,v3)>0? MORE_THAN_PI:LESS_THAN_PI; + if ( type==LESS_THAN_PI ) obs.push().set ( x.x, x.y ); + } + else + { obs.push().set ( x.x, x.y ); + } +*/ + obs.push().set ( x.x, x.y ); + } + else // MORE_THAN_PI + { SrVec2 sp1, sp2; + sp1.set ( -vec1.y, vec1.x ); // 90 degrees rotation + sp2.set ( vec2.y, -vec2.x ); // 90 degrees rotation + float f = r; + if ( !grow ) { vec1*=-1; vec2*=-1; f=-r; } + sp1.len(f); + sp2.len(f); + sp1 += v2; + sp2 += v2; + + // smooth: + obs.push() = sp1; + vec1 = sp1-v2; vec1.normalize(); + vec2 = sp2-v2; vec2.normalize(); + float ang = angle_fornormvecs(vec1,vec2); + if ( ang>maxang ) + { int nsub = (int)(ang/maxang); + float angstep = ang/(nsub+1); + if ( !grow ) angstep*=-1; + float s=sinf(angstep); + float c=cosf(angstep); + vec1.len ( r ); + while ( nsub-- ) + { vec1.rot(s,c); + vec2 = v2 + vec1; + obs.push() = vec2; + } + } + obs.push() = sp2; + } + } + +void SrPolygon::grow ( float radius, float maxangrad ) + { + if ( radius==0 || size()<2 ) return; + + const float MIN = SR_TORAD(1); + if ( maxangrad0? true:false; + if ( !grow ) radius = -radius; + + int i; + if ( _open ) + { for ( i=size()-02; i>0; i-- ) push()=get(i); + _open=0; + grow = true; + } + else + { if ( size()<3 ) return;} + + SrVec2 v1, v2, v3; + SrPolygon obgrow; + obgrow.size(size()*2); obgrow.size(0); + + for ( i=0; i& tris ) const + { + enum Case { TriangleOk, TriangleIntersects, DisconsiderPoint }; + Case c; + int a1, a2, a3, b, size; + float prec = 0.000001f; + //sr_out<3 ) + { size = pol.size(); + for ( a2=pol.size()-1; a2>=0; a2-- ) + { a1 = pol.validate(a2-1); + a3 = pol.validate(a2+1); + //printf ("%d:\n",a1); + if ( ccw ( pol[a1], pol[a2], pol[a3] )<=0 ) continue; // not ccw + //printf ("CCW\n"); + + c = TriangleOk; + for ( b=0; b&)p; + } + +SrInput& operator>> ( SrInput& inp, SrPolygon& p ) + { + inp.get_token(); + if ( inp.last_token_type()==SrInput::Name && inp.last_token()=="open" ) + p.open ( true ); + else + inp.unget_token(); + + return inp >> (SrArray&)p; + } + +//================================ End of File ================================================= + diff --git a/source/dcdt/se/sr_polygon.h b/source/dcdt/se/sr_polygon.h new file mode 100644 index 0000000000..06f0368a99 --- /dev/null +++ b/source/dcdt/se/sr_polygon.h @@ -0,0 +1,156 @@ + +# ifndef SR_POLYGON_H +# define SR_POLYGON_H + +/** \file sr_polygon.h + * A polygon described by an array of 2d points + */ + +# include "sr_vec2.h" +# include "sr_array.h" + +class SrBox; + +/*! \class SrPolygon sr_polygon.h + \brief Array of 2d points + + SrPolygon derives SrArray and provides methods for operations with + polygons. */ +class SrPolygon : public SrArray + { private : + char _open; + + public : + static const char* class_name; + + /*! Constructor with a given size and capacity, whith default values of 0 */ + SrPolygon ( int s=0, int c=0 ); + + /*! Copy constructor */ + SrPolygon ( const SrPolygon& p ); + + /*! Constructor from a given buffer */ + SrPolygon ( SrPnt2* pt, int s, int c ); + + /*! An open polygon is equivalent to a polygonal line */ + void open ( bool b ) { _open=b; } + + /*! Returns true if the polygon is open, and false otherwise */ + bool open () const { return _open? true:false; } + + /*! Sets the polygon from a float list. numv is the number of vertices, each + vertex being represented by two floats. */ + void set_from_float_array ( const float* pt, int numv ); + + /*! Returns true if the polygon is simple by testing if it is degenerated + or if non adjacent edges intersect */ + bool is_simple () const; + + /*! Returns true if the polygon is convex */ + bool is_convex () const; + + /*! Returns true if the polygon has orientation counter-clockwise */ + bool is_ccw () const { return area()>0? true:false; } + + /*! Returns the oriented area, it will be >0 if the polygon is ccw */ + float area () const; + + /*! Point in polygon test */ + bool contains ( const SrPnt2& p ) const; + + /*! Check if all points of pol are inside the polygon */ + bool contains ( const SrPolygon& pol ) const; + + /*! Check if p is in the boundary of the polygon according to the precision ds. + the function segment_contains_point() is used for each polygon edge. If p is not + in the boundary, -1 is returned, othersise the returned index says which edge of + the polygon contains the point. */ + int has_in_boundary ( const SrPnt2& p, float ds ) const; + + /*! Makes the polygon be a circle approximation with given center, radius, and + number of vertices. */ + void circle_approximation ( const SrPnt2& center, float radius, int nvertices ); + + /*! Calculates the total length along edges */ + float perimeter () const; + + /*! Parameter t must be between 0 and perimeter */ + SrPnt2 interpolate_along_edges ( float t ) const; + + /*! Resample the polygon by subdividing edges to ensure that each edge has at + most maxlen as length */ + void resample ( float maxlen ); + + /*! Remove adjacent vertices which are duplicated, according to epsilon */ + void remove_duplicated_vertices ( float epsilon ); + + /*! Remove adjacent vertices which are collinear, according to epsilon */ + void remove_collinear_vertices ( float epsilon ); + + /*! Grows the polygon by radius. CCW ordering is required. If radius<0, + the obstacle will be contracted. Resulting arc circles are discretized + by steps of maxangdeg */ + void grow ( float radius, float maxangrad ); + + /*! Get centroid of polygon */ + SrPnt2 centroid () const; + + /*! Reverse the order of the elements */ + void reverse (); + + /*! Adds dv to each vertex of the polygon */ + void translate ( const SrVec2& dv ); + + /*! Rotates the polygon around center about angle radians */ + void rotate ( const SrPnt2& center, float radians ); + + /*! Returns the lowest vertex of the polygon. If a non-null int pointer + is passed, it will contain the index of the found south pole */ + SrPnt2 south_pole ( int* index=0 ) const; + + /*! Returns the convex hull in pol */ + void convex_hull ( SrPolygon& pol ) const; + + /*! Returns the index of the vertex which is closer to p, and within + an epsilon distance of p, or -1 if there is not such a vertex */ + int pick_vertex ( const SrPnt2& p, float epsilon ); + + /*! Returns the index i of the edge (i,i+1) that is the closer edge + to p, and within a distance of epsilon to p. The square of that + distance is returned in dist2. -1 is returned if no edges are found */ + int pick_edge ( const SrPnt2& p, float epsilon, float& dist2 ) const; + + /*! Divides the polygon in triangles */ + void ear_triangulation ( SrArray& tris ) const; + + /*! Returns the bounding gox of the polygon */ + void get_bounding_box ( SrBox& b ) const; + + /*! Get the polygon configuration. x,y is the centroid, and a is the angle + between the centroid and the vertex 0, in the range [0,360) */ + void get_configuration ( float& x, float& y, float& a ) const; + + /*! Puts the polygon in the configuration (x,y,a): x,y is the centroid, and + a is the angle between the centroid and the vertex 0, in the range [0,360) */ + void set_configuration ( float x, float y, float a ); + + /*! Tests if the polygon intersects with the given segment. */ + bool intersects ( const SrPnt2& p1, const SrPnt2& p2 ) const; + + /*! Test if the two polygons intersect */ + bool intersects ( const SrPolygon& p ) const; + + /*! Comparison function not implemented, just returns 1 */ + friend int sr_compare ( const SrPolygon* p1, const SrPolygon* p2 ); + + /*! Outputs the open flag and calls the SrArray::method */ + friend SrOutput& operator<< ( SrOutput& out, const SrPolygon& p ); + + /*! Check for the open flag and calls the SrArray::method */ + friend SrInput& operator>> ( SrInput& inp, SrPolygon& p ); + }; + +//================================ End of File ================================================= + +# endif // SR_POLYGON_H + diff --git a/source/dcdt/se/sr_polygons.cpp b/source/dcdt/se/sr_polygons.cpp new file mode 100644 index 0000000000..a0224616ea --- /dev/null +++ b/source/dcdt/se/sr_polygons.cpp @@ -0,0 +1,208 @@ +#include "precompiled.h" +# include "sr_box.h" +# include "sr_polygons.h" + +//# define SR_USE_TRACE1 // Constructor and Destructor +# include "sr_trace.h" + +//============================= SrPolygons ================================== + +const char* SrPolygons::class_name = "Polygons"; + +SrPolygons::SrPolygons () + { + SR_TRACE1 ( "Default Constructor" ); + } + +SrPolygons::SrPolygons ( const SrPolygons& polys ) + { + SR_TRACE1 ( "Copy Constructor" ); + int i; + _data.size ( polys.size() ); + for ( i=0; i<_data.size(); i++ ) + _data[i] = new SrPolygon ( polys.const_get(i) ); + } + +SrPolygons::~SrPolygons () + { + SR_TRACE1 ( "Destructor" ); + size (0); + } + +void SrPolygons::size ( int ns ) + { + int i, s = _data.size(); + if ( ns>s ) + { _data.size(ns); + for ( i=s; i=0 ) + { if ( vid<0 || dist2> ( SrInput& in, SrPolygons& p ) + { + p.size(0); + in.get_token(); + while (true) + { in.get_token(); + if ( in.last_token()[0]==']' ) break; + in.unget_token(); + p.push(); + in >> p.top(); + } + return in; + } + +//================================ EOF ================================================= diff --git a/source/dcdt/se/sr_polygons.h b/source/dcdt/se/sr_polygons.h new file mode 100644 index 0000000000..ae135323ff --- /dev/null +++ b/source/dcdt/se/sr_polygons.h @@ -0,0 +1,134 @@ + +# ifndef SR_POLYGONS_H +# define SR_POLYGONS_H + +/** \file sr_polygons.h + * maintains an array of polygons + */ + +# include "sr_array.h" +# include "sr_polygon.h" +# include "sr_shared_class.h" + +/*! \class SrPolygons sr_polygons.h + \brief maintains an array of polygons + + SrPolygons keeps internally an array of polygons and provides + methods for manipulating them. */ +class SrPolygons : public SrSharedClass + { private : + SrArray _data; + + public : + static const char* class_name; + + /*! Default constructor */ + SrPolygons (); + + /*! Copy constructor */ + SrPolygons ( const SrPolygons& p ); + + /*! Virtual Destructor */ + virtual ~SrPolygons (); + + /*! Returns true if the array has no polygons, and false otherwise. */ + bool empty () const { return _data.empty(); } + + /*! Returns the capacity of the array. */ + int capacity () const { return _data.capacity(); } + + /*! Returns the current size of the array. */ + int size () const { return _data.size(); } + + /*! Changes the size of the array, filling new empty polygons in the new positions. */ + void size ( int ns ); + + /*! Makes the array empty; equivalent to size(0) */ + void init () { size(0); } + + /*! Changes the capacity of the array. */ + void capacity ( int nc ); + + /*! Makes capacity to be equal to size. */ + void compress () { _data.compress(); } + + /*! Swaps polygons with positions i and j, which must be valid positions. */ + void swap ( int i, int j ); + + /*! Returns a reference to polygon index i, which must be a valid index */ + SrPolygon& get ( int i ) { return *_data[i]; } + + /*! Returns a const reference to polygon index i, which must be a valid index */ + const SrPolygon& const_get ( int i ) const { return *_data[i]; } + + /*! Returns a const reference to the vertex j of polygon i. Indices must be valid. */ + const SrVec2& const_get ( int i, int j ) const { return _data[i]->const_get(j); } + + /*! Returns a reference to the vertex j of polygon i. Indices must be valid. */ + SrVec2& get ( int i, int j ) { return _data[i]->get(j); } + + /*! Equivalent to get(i) */ + SrPolygon& operator[] ( int i ) { return get(i); } + + /*! Equivalent to get(i,j) */ + SrVec2& operator() ( int i, int j ) { return get(i,j); } + + /*! Copy polygon p into position i */ + void set ( int i, const SrPolygon& p ) { get(i)=p; } + + /*! Returns the last polygon */ + SrPolygon& top () { return *_data.top(); } + + /*! Pop and frees last polygon if the array is not empty */ + void pop () { delete _data.pop(); } + + /*! Allocates and appends one empty polygon */ + SrPolygon& push () { return *(_data.push()=new SrPolygon); } + + /*! Inserts one polygon using copy operator */ + void insert ( int i, const SrPolygon& x ); + + /*! Allocates and inserts n empty polygons at position i */ + void insert ( int i, int n ); + + /*! Removes n polygons at position i */ + void remove ( int i, int n=1 ); + + /*! Extract (without deletion) and returns the pointer at position i */ + SrPolygon* extract ( int i ); + + /*! Returns true if there is a vertex closer to p than epsilon. In this case + the indices of the closest vertex to p are returned in pid and vid. + If no vertices exist, false is returned. */ + bool pick_vertex ( const SrVec2& p, float epsilon, int& pid, int& vid ) const; + + /*! Returns the index of the first polygon containing p, or -1 if not found */ + int pick_polygon ( const SrVec2& p ) const; + + /*! Returns true if there is an edge closer to p than epsilon. In this case + the indices of the first vertex of the closest edge are returned in pid and vid. + If no edge exist, false is returned. */ + bool pick_edge ( const SrVec2& p, float epsilon, int& pid, int& vid ) const; + + /*! Returns the first polygon intersecting the given segment, or -1 otherwise. */ + int intersects ( const SrVec2& p1, const SrVec2& p2 ) const; + + /*! Returns the first polygon intersecting p, or -1 otherwise. */ + int intersects ( const SrPolygon& p ) const; + + /*! Returns the bounding box of the set of polygons */ + void get_bounding_box ( SrBox &b ) const; + + /*! Copy operator */ + void operator = ( const SrPolygons& p ); + + /*! Outputs all elements of the array. */ + friend SrOutput& operator<< ( SrOutput& o, const SrPolygons& p ); + + /*! Inputs elements. */ + friend SrInput& operator>> ( SrInput& in, SrPolygons& p ); + }; + +//================================ End of File ================================================= + +# endif // SR_SCENE_POLYGONS_H diff --git a/source/dcdt/se/sr_quat.cpp b/source/dcdt/se/sr_quat.cpp new file mode 100644 index 0000000000..b853df1db6 --- /dev/null +++ b/source/dcdt/se/sr_quat.cpp @@ -0,0 +1,335 @@ +#include "precompiled.h" +# include "sr_quat.h" +# include "sr_random.h" + +//============================== Static Data ==================================== + +const SrQuat SrQuat::null ( 1.0f, 0, 0, 0 ); + +//============================ public members ==================================== + +// from: Kuffner, "Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning" +void SrQuat::random () + { + float s = SrRandom::randf (); + float s1 = sqrtf ( 1-s ); + float s2 = sqrtf ( s ); + float t1 = sr2pi * SrRandom::randf (); + float t2 = sr2pi * SrRandom::randf (); + w = cosf(t2) * s2; + x = sinf(t1) * s1; + y = cosf(t1) * s1; + z = sinf(t2) * s2; + } + +void SrQuat::set ( const SrVec& v1, const SrVec& v2 ) + { + SrVec axis = cross ( v1, v2 ); + set ( axis, ::angle(v1,v2) ); + } + +void SrQuat::set ( const SrVec& axis, float radians ) + { + float f; + + // normalize axis: + x=axis.x; y=axis.y; z=axis.z; + f = x*x + y*y + z*z; + if ( f>0 ) + { f = sqrtf ( f ); + x/=f; y/=f; z/=f; + } + + // set the quaternion: + radians/=2; + f = sinf ( radians ); + x*=f; y*=f; z*=f; + w = cosf ( radians ); + } + +void SrQuat::set ( const SrVec& axisangle ) + { + float ang; + + // normalize axis ang extract angle: + x=axisangle.x; y=axisangle.y; z=axisangle.z; + ang = x*x + y*y + z*z; + if ( ang>0 ) + { ang = sqrtf ( ang ); + x/=ang; y/=ang; z/=ang; + } + + // set the quaternion: + ang/=2; + w = cosf ( ang ); + ang = sinf ( ang ); + x*=ang; y*=ang; z*=ang; + } + +void SrQuat::set ( const SrMat& m ) + { + # define E(i) m.get(i) + # define M(i,j) m.get(i,j) + # define Q(i) *((&x)+i) + float s; + float tr = E(0) + E(5) + E(10); + + if ( tr>0 ) + { s = sqrtf ( 1.0f + tr ); + w = s / 2.0f; + s = 0.5f / s; + x = (E(6) - E(9)) * s; + y = (E(8) - E(2)) * s; + z = (E(1) - E(4)) * s; + } + else + { int i, j, k; + + i = M(1,1)>M(0,0)? 1:0; + if ( M(2,2)> M(i,i) ) i=2; + j = (i+1)%3; + k = (j+1)%3; + + s = sqrtf ( (M(i,i) - (M(j,j)+M(k,k))) + 1.0f ); + Q(i) = s * 0.5f; + + if ( s!=0 ) // s should never be equal to 0 if matrix is orthogonal + s = 0.5f / s; + + w = (M(j,k) - M(k,j)) * s; + Q(j) = (M(i,j) + M(j,i)) * s; + Q(k) = (M(i,k) + M(k,i)) * s; + } + # undef E + # undef M + # undef Q + } + +void SrQuat::get ( SrVec& axis, float& radians ) const + { + // if SrQuat==(1,0,0,0), the axis will be null, so we + // set the axis to (1,0,0) (SrVec::i); the angle will be 0. + // this is also done in SrQuat::axis() + axis.set ( x, y, z ); + float n = axis.norm(); + if ( n==0 ) axis=SrVec::i; else axis/=n; + radians = 2.0f * acosf ( w ); + } + +SrVec SrQuat::axis () const + { + SrVec axis ( x, y, z ); + float n = axis.norm(); + if ( n==0 ) axis=SrVec::i; else axis/=n; + return axis; + } + +float SrQuat::angle () const + { + return 2.0f * acosf ( w ); + } + +void SrQuat::normalize () + { + float f = sqrtf( norm2() ); + if ( f==0 ) return; + w /= f; + x/=f; y/=f; z/=f; + if ( w<0 ) { w=-w; x=-x; y=-y; z=-z; } + } + +SrQuat SrQuat::inverse() const + { + return conjugate();///norm(); + } + +void SrQuat::invert () +{ + //float n = norm(); + x=-x; y=-y; z=-z; // conjugate + //w/=n; x/=n; y/=n; z/=n; + } + +inline void get_rot_mat ( float w, float x, float y, float z, + float& x1, float& y1, float& z1, + float& x2, float& y2, float& z2, + float& x3, float& y3, float& z3 ) + { + x2 = x+x; + float x2x = x2*x; + float x2y = x2*y; + float x2z = x2*z; + float x2w = x2*w; + y2 = y+y; + float y2y = y2*y; + float y2z = y2*z; + float y2w = y2*w; + z2 = z+z; + float z2z = z2*z; + float z2w = z2*w; + + x1 = 1.0f - y2y - z2z; y1 = x2y + z2w; z1 = x2z - y2w; + x2 = x2y - z2w; y2 = 1.0f - x2x - z2z; z2 = y2z + x2w; + x3 = x2z + y2w; y3 = y2z - x2w; z3 = 1.0f - x2x - y2y; + +/* the transposed one is : + setl1 ( 1.0f - y2y - z2z, x2y - z2w, x2z + y2w, 0.0f ); + setl2 ( x2y + z2w, 1.0f - x2x - z2z, y2z - x2w, 0.0f ); + setl3 ( x2z - y2w, y2z + x2w, 1.0f - x2x - y2y, 0.0f ); + setl4 ( 0, 0, 0, 1.0f ); + */ + } + +void SrQuat::get_rot_mat ( float& a, float& b, float& c, + float& d, float& e, float& f, + float& g, float& h, float& i ) const + { + ::get_rot_mat ( w, x, y, z, a, b, c, + d, e, f, + g, h, i ); + } + +SrMat& SrQuat::get_mat ( SrMat& m ) const + { + ::get_rot_mat ( w, x, y, z, m[0], m[1], m[2], + m[4], m[5], m[6], + m[8], m[9], m[10] ); + + m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f; + m[15] = 1.0f; + + return m; + } + +//=================================== Friend Functions =================================== + +SrVec operator * ( const SrVec &v, const SrQuat &q ) + { + SrQuat qv ( 0, v.x, v.y, v.z ); + qv = q * qv * q.conjugate(); + return SrVec ( qv.x, qv.y, qv.z ); + } + +SrMat operator * ( const SrMat &m, const SrQuat &q ) + { + SrMat mat; + return m * q.get_mat(mat); + } + +SrMat operator * ( const SrQuat &q, const SrMat &m ) + { + SrMat mat; + return q.get_mat(mat) * m; + } + +SrQuat operator * ( const SrQuat &q1, const SrQuat &q2 ) + { + SrQuat q; + + q.w = (q1.w*q2.w) - (q1.x*q2.x + q1.y*q2.y + q1.z*q2.z); // w1*w2-dot(v1,v2) + q.x = q1.y*q2.z - q1.z*q2.y; // cross (q1.v,q2.v) + q.y = q1.z*q2.x - q1.x*q2.z; + q.z = q1.x*q2.y - q1.y*q2.x; + q.x += (q1.x*q2.w) + (q2.x*q1.w); + q.y += (q1.y*q2.w) + (q2.y*q1.w); + q.z += (q1.z*q2.w) + (q2.z*q1.w); + + return q; + } + +bool operator == ( const SrQuat &q1, const SrQuat &q2 ) + { + return q1.w==q2.w && q1.x==q2.x && q1.y==q2.y && q1.z==q2.z ? true:false; + } + +bool operator != ( const SrQuat &q1, const SrQuat &q2 ) + { + return q1.w!=q2.w && q1.x!=q2.x && q1.y!=q2.y && q1.z!=q2.z ? true:false; + } + +void swap ( SrQuat &q1, SrQuat &q2 ) + { + float tmp; + SR_SWAP(q1.w,q2.w); + SR_SWAP(q1.x,q2.x); + SR_SWAP(q1.y,q2.y); + SR_SWAP(q1.z,q2.z); + } + +SrQuat slerp ( SrQuat &q1, const SrQuat &q2, float t ) + { + float dot = q1.dot(q2); + if ( dot < 0 ) + { // the quaternions are pointing in opposite directions, so + // use the equivalent alternative representation for q1 + q1.set ( -q1.w, -q1.x, -q1.y, -q1.z ); + dot = -dot; + } + + // interpolation factors + float r, s; + + // decide according to an epsilon (30fps motions are of E-6 order) + // this IS needed for baked motion to avoid dealing with the E-6 values in floats + if ( 1.0f-dot < 0.05f ) + { // the quaternions are nearly parallel, so use linear interpolation + r = 1-t; + s = t; + } + else + { // calculate spherical linear interpolation factors + float a = acosf(dot); + float g = 1.0f / sinf(a); + r = sinf ( (1-t)*a ) * g; + s = sinf ( t*a ) * g; + } + + // set the interpolated quaternion + SrQuat q ( r*q1.w + s*q2.w, + r*q1.x + s*q2.x, + r*q1.y + s*q2.y, + r*q1.z + s*q2.z ); + q.normalize(); + return q; + } + +SrOutput& operator<< ( SrOutput& out, const SrQuat& q ) + { + return out << "axis " << q.axis() << " ang " << SR_TODEG(q.angle()); + } + +SrInput& operator>> ( SrInput& in, SrQuat& q ) + { + SrVec axis; + float ang; + + in.get_token(); // "axis" + in >> axis; + + in.get_token(); // "ang" + in >> ang; + + q.set ( axis, SR_TORAD(ang) ); + + return in; + } + +//================================== End of File =========================================== + +/* +double dist(const QUAT &left, const QUAT &right) +{ + return acos(dot_product(left,right)); +} + +// approximate distance: (Kuffner) +dot = dot_product(left,right); // in [-1,1] +return ( 1-||dot|| ) ; + +(avec dot_product le produit membre a membre des 4 composantes du quaternion) + +L'interpretation geometrique est toute simple: c'est la longueur de l'arc geodesique + entre les 2 points sur la sphere unite en 4D, et donc cela vaut tout simplement... +l'angle (puisque le rayon de la sphere est 1) +*/ + diff --git a/source/dcdt/se/sr_quat.h b/source/dcdt/se/sr_quat.h new file mode 100644 index 0000000000..ac5dad47f2 --- /dev/null +++ b/source/dcdt/se/sr_quat.h @@ -0,0 +1,148 @@ + +/** \file sr_quat.h + * Quaternion for rotations */ + +# ifndef SR_QUAT_H +# define SR_QUAT_H + +# include +# include "sr_vec.h" +# include "sr_mat.h" + +/*! \class SrQuat sr_quat.h + \brief Quaternion for rotations. + + A quaternion is represented as a four dimensional vector. Here we keep + one scalar float element w, and three floats for the imaginary vector. + The quaternion is then: w + x i + y j + z k */ +class SrQuat + { public : + float w; // scalar part + float x, y, z; // imaginary vector part + + private : + float norm2 () const { return w*w+x*x+y*y+z*z; } + float norm () const { return sqrtf(norm2()); } + SrQuat conjugate() const { return SrQuat(w,-x,-y,-z); } + void operator *= ( float r ) { w*=r; x*=r; y*=r; z*=r; } + SrQuat operator * ( float r ) const { return SrQuat(w*r,x*r,y*r,z*r); } + SrQuat operator / ( float r ) const { return SrQuat(w/r,x/r,y/r,z/r); } + SrQuat operator + ( const SrQuat &q ) const { return SrQuat(w+q.w,x+q.x,y+q.y,z+q.z); } + float dot ( const SrQuat &q ) const { return w*q.w + x*q.x + y*q.y + z*q.z; } + + public : + + /*! A null rotation that represents a rotation around the axis (1,0,0) + with angle 0, that generates the internal representation (1,0,0,0). */ + static const SrQuat null; + + public : + + /*! Initializes SrQuat as a null rotation. Implemented inline. */ + SrQuat () : w(1.0f), x(0), y(0), z(0) {} + + /*! Constructor from 4 floats (qw,qx,qy,qz). Implemented inline. */ + SrQuat ( float qw, float qx, float qy, float qz ) : w(qw), x(qx), y(qy), z(qz) {} + + /*! Constructor from 4 floats (w,x,y,z) from a float array. Implemented inline. */ + SrQuat ( const float* f ) : w(f[0]), x(f[1]), y(f[2]), z(f[3]) {} + + /*! Copy constructor. Implemented inline. */ + SrQuat ( const SrQuat& q ) : w(q.w), x(q.x), y(q.y), z(q.z) {} + + /*! Initializes SrQuat with the rotation around the given axis and angle in + radians. The method set() is called inline in this constructor. */ + SrQuat ( const SrVec& axis, float radians ) { set(axis,radians); } + + /*! Initializes SrQuat with the given axis-angle rotation, where the vector + is the rotation axis, and its norm is the angle of rotation. + The method set() is called inline in this constructor. */ + SrQuat ( const SrVec& axisangle ) { set(axisangle); } + + /*! Initializes SrQuat with the rotation from v1 to v2. + The method set() is called inline in this constructor. */ + SrQuat ( const SrVec& v1, const SrVec& v2 ) { set(v1,v2); } + + /*! Initializes SrQuat from a rotation matrix */ + SrQuat ( const SrMat& m ) { set(m); } + + /*! Set a random quaternion, using a uniform distribution method */ + void random (); + + /*! Set the four values */ + void set ( float qw, float qx, float qy, float qz ) { w=qw; x=qx; y=qy; z=qz; } + + /*! Set the four values from a float pointer */ + void set ( const float* f ) { w=f[0]; x=f[1]; y=f[2]; z=f[3]; } + + /*! Defines SrQuat as the rotation from v1 to v2. */ + void set ( const SrVec& v1, const SrVec& v2 ); + + /*! Defines SrQuat as the rotation around axis of the given angle in radians. + Axis is internaly normallized. */ + void set ( const SrVec& axis, float radians ); + + /*! Initializes SrQuat with the given axis-angle rotation, where the given + vector is the rotation axis, and its norm is the angle of rotation. */ + void set ( const SrVec& axisangle ); + + /*! Defines SrQuat by extracting the rotation from the given rotation matrix. */ + void set ( const SrMat& m ); + + /*! Gets the current axis and angle of rotation (in radians) that SrQuat defines. */ + void get ( SrVec& axis, float& radians ) const; + + /*! Returns the rotation axis. */ + SrVec axis () const; + + /*! Returns the angle in radians. */ + float angle () const; + + /*! Normalizes the quaternion and ensures w>=0 */ + void normalize (); + + /*! Returns the inverse quaternium. */ + SrQuat inverse() const; + + /*! Invert SrQuat. */ + void invert (); + + /*! Equivalent to get_mat(), but retrieves only the 3x3 portion + of the transformation matrix. */ + void get_rot_mat ( float& a, float& b, float& c, + float& d, float& e, float& f, + float& g, float& h, float& i ) const; + + /*! Gets the equivalent transformation matrix. */ + SrMat& get_mat ( SrMat& m ) const; + + /*! Given a vector v and a quaternion q, the result of applying the rotation + in q to v is returned (in mathematical notation this is q v q^-1) . */ + friend SrVec operator * ( const SrVec &v, const SrQuat &q ); + + /*! rotation q1 followed by rotation q2 is equal to q2*q1 . */ + friend SrQuat operator * ( const SrQuat &q1, const SrQuat &q2 ); + + /*! Comparison operator makes an exact comparison of the quaternion components. */ + friend bool operator == ( const SrQuat &q1, const SrQuat &q2 ); + + /*! Comparison operator makes an exact comparison, of the quaternion components. */ + friend bool operator != ( const SrQuat &q1, const SrQuat &q2 ); + + /*! Swaps the contents of q1 with q2. */ + friend void swap ( SrQuat &q1, SrQuat &q2 ); + + /*! Returns the interpolation between q1 and q2 with parameter t. + Parameter q1 is not const because it is normalized. */ + friend SrQuat slerp ( SrQuat &q1, const SrQuat &q2, float t ); + + /*! Outputs data in "axis x y z ang a" format. */ + friend SrOutput& operator<< ( SrOutput& out, const SrQuat& q ); + + /*! Input data from "axis x y z ang a" format. */ + friend SrInput& operator>> ( SrInput& in, SrQuat& q ); + }; + +//============================== end of file =============================== + +# endif // SR_QUAT_H diff --git a/source/dcdt/se/sr_queue.h b/source/dcdt/se/sr_queue.h new file mode 100644 index 0000000000..672a08f007 --- /dev/null +++ b/source/dcdt/se/sr_queue.h @@ -0,0 +1,97 @@ + +# ifndef SR_QUEUE_H +# define SR_QUEUE_H + +/** \file sr_queue.h + * Template for a queue based on SrArray. */ + +# include + +/*! \class SrQueue sr_queue.h + \brief Queue based on two SrArray + + SrQueue implements a FIFO queue using two internal SrArray objects, + one for elements being inserted, and the other for elements to output. + Both SrArray work as stacks. Note that SrQueue is oriented for speed, + and does not honors constructors or destructors of its elements X, see + SrArray documentation for additional comments on this. To overcome + this limitation, use SrQueue with pointers. */ +template +class SrQueue + { private : + SrArray _in; + SrArray _out; + public : + + /*! Constructor receives the capacity for both input and output arrays. */ + SrQueue ( int c=0 ) : _in(0,c), _out(0,c) {} + + /*! Copy constructor. */ + SrQueue ( const SrQueue& q ) : _in(q._in), _out(q._out) {} + + /*! Returns true if the queue is empty, false otherwise. */ + bool empty () const { return _in.empty() && _out.empty()? true:false; } + + /*! Returns the number of elements in the queue. */ + int size () const { return _in.size()+_out.size(); } + + /*! Compress both input and output arrays. */ + void compress () { _in.compress(); _out.compress(); } + + /*! Returns a reference to the first element; the queue must not be empty. */ + X& first () { return _out.empty()? _in[0]:_out.top(); } + + /*! Returns a reference to the last element; the queue must not be empty. */ + X& last () { return _in.empty()? _out[0]:_in.top(); } + + /*! Inserts an element at the end of the queue. */ + X& insert () { return _in.push(); } + + /*! Removes and returns a reference to the first element of the queue; + the queue must not be empty. */ + X& remove () + { if ( _out.empty() ) + { _out.size(_in.size()-1); + for ( int i=0; i<_out.size(); i++ ) _out[i]=_in.pop(); + return _in.pop(); + } + else return _out.pop(); + } + + /*! Empties the input stack, correctly moving its elements to the output stack. */ + void flush () + { if ( _in.empty() ) return; + _out.insert ( 0, _in.size() ); + for ( int i=0; _in.size()>0; i++ ) _out[i]=_in.pop(); + } + + /*! Makes this queue be the same as q, which becomes an empty queue. */ + void take_data ( SrQueue& q ) + { _in.take_data ( q._in ); + _out.take_data ( q._out ); + } + + /*! Outputs all elements of the queue in format [e0 e1 ... en]. */ + friend SrOutput& operator<< ( SrOutput& o, const SrQueue& q ) + { + int i, si, so; + si = q._in.size(); + so = q._out.size(); + + o << '['; + for ( i=0; i0 ) o << srspc; + } + for ( i=so-1; i>=0; i-- ) + { o << q._out[i]; + if ( i>0 ) o << srspc; + } + return o << ']'; + } + }; + +//============================== end of file =============================== + +#endif // SR_QUEUE_H + diff --git a/source/dcdt/se/sr_random.cpp b/source/dcdt/se/sr_random.cpp new file mode 100644 index 0000000000..649a67a946 --- /dev/null +++ b/source/dcdt/se/sr_random.cpp @@ -0,0 +1,174 @@ +#include "precompiled.h" +# include "sr.h" +# include "sr_random.h" + +//========================= static methods ============================================= + +/* From: http://www.math.keio.ac.jp/~matumoto/mt19937int.c */ +/* genrand() generates one pseudorandom unsigned integer (32bit) */ +/* which is uniformly distributed among 0 to 2^32-1 for each */ +/* call. sgenrand(seed) set initial values to the working area */ +/* of 624 words. Before genrand(), sgenrand(seed) must be */ +/* called once. (seed is any 32-bit integer except for 0). */ +/* Coded by Takuji Nishimura, considering the suggestions by */ +/* Topher Cooper and Marc Rieffel in July-Aug. 1997. */ + +/* This library is free software; you can redistribute it and/or */ +/* modify it under the terms of the GNU Library General Public */ +/* License as published by the Free Software Foundation; either */ +/* version 2 of the License, or (at your option) any later */ +/* version. */ +/* This library is distributed in the hope that it will be useful, */ +/* but WITHOUT ANY WARRANTY; without even the implied warranty of */ +/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. */ +/* See the GNU Library General Public License for more details. */ +/* You should have received a copy of the GNU Library General */ +/* Public License along with this library; if not, write to the */ +/* Free Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA */ +/* 02111-1307 USA */ + +/* Copyright (C) 1997 Makoto Matsumoto and Takuji Nishimura. */ +/* Any feedback is very welcome. For any question, comments, */ +/* see http://www.math.keio.ac.jp/matumoto/emt.html or email */ +/* matumoto@math.keio.ac.jp */ + +/* Period parameters */ +#define N 624 +#define M 397 +#define MATRIX_A 0x9908b0df /* constant vector a */ +#define UPPER_MASK 0x80000000 /* most significant w-r bits */ +#define LOWER_MASK 0x7fffffff /* least significant r bits */ + +/* Tempering parameters */ +#define TEMPERING_MASK_B 0x9d2c5680 +#define TEMPERING_MASK_C 0xefc60000 +#define TEMPERING_SHIFT_U(y) (y >> 11) +#define TEMPERING_SHIFT_S(y) (y << 7) +#define TEMPERING_SHIFT_T(y) (y << 15) +#define TEMPERING_SHIFT_L(y) (y >> 18) + +static unsigned long mt[N]; /* the array for the state vector */ +static int mti=N+1; /* mti==N+1 means mt[N] is not initialized */ +static bool first_use=true; + +/* initializing the array with a NONZERO seed */ +static void sgenrand ( unsigned long seed ) +{ + /* setting initial seeds to mt[N] using */ + /* the generator Line 25 of Table 1 in */ + /* [KNUTH 1981, The Art of Computer Programming */ + /* Vol. 2 (2nd Ed.), pp102] */ + mt[0]= seed & 0xffffffff; + for (mti=1; mti= N) { /* generate N words at one time */ + int kk; + + if (mti == N+1) /* if sgenrand() has not been called, */ + sgenrand(4357); /* a default initial seed is used */ + + for (kk=0;kk> 1) ^ mag01[y & 0x1]; + } + for (;kk> 1) ^ mag01[y & 0x1]; + } + y = (mt[N-1]&UPPER_MASK)|(mt[0]&LOWER_MASK); + mt[N-1] = mt[M-1] ^ (y >> 1) ^ mag01[y & 0x1]; + + mti = 0; + } + + y = mt[mti++]; + y ^= TEMPERING_SHIFT_U(y); + y ^= TEMPERING_SHIFT_S(y) & TEMPERING_MASK_B; + y ^= TEMPERING_SHIFT_T(y) & TEMPERING_MASK_C; + y ^= TEMPERING_SHIFT_L(y); + + return y; +} + +static double drand () // between 0 and 1 + { + static double max = double(0xffffffff); + double r = double(genrand()); + return r / max; + } + +//========================= SrRandom ============================================= + +SrRandom::SrRandom () + { + if ( first_use ) { sgenrand(1); first_use=false; } + limits ( 0.0f, 1.0f ); + } + +void SrRandom::limits ( double inf, double sup ) + { + _inf = inf; + _sup = sup; + _dif = sup-inf; + _type = 'd'; + } + +void SrRandom::limits ( float inf, float sup, int br ) + { + _inf = (double)inf; + _sup = (double)sup; + _dif = _sup-_inf; + _type = 'f'; + } + +void SrRandom::limits ( int inf, int sup ) + { + _inf = (double)inf; + _sup = (double)sup; + _dif = (double)(sup-inf); + _type = 'i'; + } + +int SrRandom::geti () + { + double r = drand()*_dif; + return int ( _inf + SR_ROUND(r) ); + } + +float SrRandom::getf () + { + return float ( _inf + drand()*_dif ); + } + +double SrRandom::getd () + { + unsigned long a=genrand()>>5, b=genrand()>>6; + // this gives a uniform randomnumber in [0,1) with 53-bit precision: + double d = (a*67108864.0+b)*(1.0/9007199254740992.0); + return _inf + d * _dif; + } + +//=== Static Methods ======================================================================= + +void SrRandom::seed ( unsigned long seed ) + { + if ( first_use ) first_use=false; + sgenrand ( seed ); + } + +float SrRandom::randf () + { + if ( first_use ) { sgenrand(1); first_use=false; } + return (float) drand(); + } + +//=== End of File ======================================================================= + diff --git a/source/dcdt/se/sr_random.h b/source/dcdt/se/sr_random.h new file mode 100644 index 0000000000..b8ccd15b00 --- /dev/null +++ b/source/dcdt/se/sr_random.h @@ -0,0 +1,77 @@ + +# ifndef SR_RANDOM_H +# define SR_RANDOM_H + +/** \file sr_random.h + * Random number generation */ + +//================================= SrRandom ==================================== + +/*! Random methods use a custom random generator with resolution of 2^32-1 */ +class SrRandom + { private: + double _inf, _sup, _dif; + char _type; // 'd', 'f', or 'i' + + public : + + /*! Default constructor sets the random generator to have + limits [0,1] and type float */ + SrRandom (); + + /*! Constructor with given limits in type double, with 53-bit resolution */ + SrRandom ( double inf, double sup ) { limits(inf,sup); } + + /*! Constructor with given limits in type float, with 32-bit resolution */ + SrRandom ( float inf, float sup ) { limits(inf,sup); } + + /*! Constructor with given integer limits. */ + SrRandom ( int inf, int sup ) { limits(inf,sup); } + + /*! Sets limits in type double, with 53-bit resolution */ + void limits ( double inf, double sup ); + + /*! Sets limits in type float, with 32-bit resolution. */ + void limits ( float inf, float sup, int br=15 ); + + /*! Sets integer limits.. */ + void limits ( int inf, int sup ); + + /*! Returns the type on which the limits were set: 'd', 'f', or 'i' */ + char type () { return _type; } + + /*! Returns the superior limit. */ + double inf () { return _inf; } + + /*! Returns the inferior limit. */ + double sup () { return _sup; } + + /*! Returns a random integer number in [inf,sup]. */ + int geti (); + + /*! Returns a random float number in [inf,sup], with 32-bit resolution. */ + float getf (); + + /*! Returns a random double number in [inf,sup], with 53-bit resolution. */ + double getd (); + + /*! Set limits and returns one corresponding random value. */ + int get ( int inf, int sup ) { limits(inf,sup); return geti(); } + + /*! Set limits and returns one corresponding random value, with 32-bit resolution. */ + float get ( float inf, float sup ) { limits(inf,sup); return getf(); } + + /*! Set limits and returns one corresponding random value, with 53-bit resolution. */ + double get ( double inf, double sup ) { limits(inf,sup); return getd(); } + + /*! Sets the starting point for generating random numbers. The seed is 1 by default. */ + static void seed ( unsigned long seed ); + + /*! Returns a random float in [0,1], with 32-bit resolution */ + static float randf (); + }; + +//============================== end of file ====================================== + +# endif // SR_RANDOM_H + diff --git a/source/dcdt/se/sr_sa.cpp b/source/dcdt/se/sr_sa.cpp new file mode 100644 index 0000000000..e1cf5b9d44 --- /dev/null +++ b/source/dcdt/se/sr_sa.cpp @@ -0,0 +1,151 @@ +#include "precompiled.h" +//# include + +# include "sr_sa.h" +# include "sr_sn_group.h" +# include "sr_sn_editor.h" +# include "sr_sn_matrix.h" + +//# define SR_USE_TRACE1 // constructor / destructor +# include "sr_trace.h" + +//=============================== SrSa ==================================== + +SrSa::SrSa () + { + SR_TRACE1 ( "Constructor" ); + + _matrix_stack.capacity ( 8 ); + _matrix_stack.push ( SrMat::id ); + } + +SrSa::~SrSa () + { + SR_TRACE1 ( "Destructor" ); + } + +bool SrSa::apply ( SrSn* n, bool init ) + { + if ( init ) init_matrix (); + + apply_begin (); + + bool result; + int t = n->type(); + + if ( t==SrSn::TypeMatrix ) + { + result = matrix_apply ( (SrSnMatrix*) n ); + } + else if ( t==SrSn::TypeGroup ) + { + result = group_apply ( (SrSnGroup*) n ); + } + else if ( t==SrSn::TypeShape ) + { + result = shape_apply ( (SrSnShapeBase*) n ); + } + else if ( t==SrSn::TypeEditor ) + { + result = editor_apply ( (SrSnEditor*) n ); + } + else + { sr_out.fatal_error ( "Undefined type &d in SrSa::apply()!", t ); + result = false; + } + + apply_end (); + return result; + } + +//--------------------------------- virtuals ----------------------------------- + +void SrSa::get_top_matrix ( SrMat& mat ) + { + mat = _matrix_stack.top(); + } + +int SrSa::matrix_stack_size () + { + return _matrix_stack.size(); + } + +void SrSa::init_matrix () + { + _matrix_stack.size ( 1 ); + _matrix_stack[0] = SrMat::id; + } + +void SrSa::mult_matrix ( const SrMat& mat ) + { + SrMat stackm = _matrix_stack.top(); + _matrix_stack.top().mult ( mat, stackm ); // top = mat * top + } + +void SrSa::push_matrix () + { + _matrix_stack.push(); + _matrix_stack.top() = _matrix_stack[ _matrix_stack.size()-2 ]; + } + +void SrSa::pop_matrix () + { + _matrix_stack.pop(); + } + +bool SrSa::matrix_apply ( SrSnMatrix* m ) + { + if ( !m->visible() ) return true; + mult_matrix ( m->get() ); + return true; + } + +bool SrSa::group_apply ( SrSnGroup* g ) + { + bool b=true; + int i, s; + + if ( !g->visible() ) return true; + + if ( g->separator() ) push_matrix(); + + s = g->size(); + + for ( i=0; iget(i), false ); + if ( !b ) break; + } + + if ( g->separator() ) pop_matrix(); + return b; + } + +bool SrSa::shape_apply ( SrSnShapeBase* s ) + { + return true; // implementation specific to the derived class + } + +bool SrSa::editor_apply ( SrSnEditor* e ) + { + SrSnGroup* h = e->helpers(); + SrSn* c = e->child(); + if ( !c ) return true; + + push_matrix (); + mult_matrix ( e->mat() ); + + bool b = apply ( c, false ); + + if ( b && h && e->visible() ) + { int i, s = h->size(); + for ( i=0; iget(i), false ); + if ( !b ) break; + } + } + + pop_matrix(); + return b; + } + +//======================================= EOF ==================================== diff --git a/source/dcdt/se/sr_sa.h b/source/dcdt/se/sr_sa.h new file mode 100644 index 0000000000..6355b23227 --- /dev/null +++ b/source/dcdt/se/sr_sa.h @@ -0,0 +1,88 @@ + +# ifndef SR_SA_H +# define SR_SA_H + +/** \file sr_sa.h + * scene action base class + */ + +# include "sr_sn.h" + +class SrSnGroup; +class SrSnEditor; +class SrSnMatrix; +class SrSnShapeBase; + +/*! \class SrSa sr_sa.h + \brief scene action base class + + Defines a base class for applying actions to the scene graph */ +class SrSa + { protected : + SrArray _matrix_stack; + + protected : + + /*! Constructor . An id matrix is pushed into the matrix stack. */ + SrSa (); + + /*! Virtual destructor. */ + virtual ~SrSa (); + + public : + + /*! Makes the node to start applying the action. If the action is not + applied to the entire scene, false is returned. + If init is true (default), init_matrix(Id) is called. */ + bool apply ( SrSn* n, bool init=true ); + + protected : // virtual methods + + /*! Put in mat the current top matrix */ + virtual void get_top_matrix ( SrMat& mat ); + + /*! returns the size of the matrix stack */ + virtual int matrix_stack_size (); + + /*! Sets the matrix stack size to 1 and put the identity mat in the top position. */ + virtual void init_matrix (); + + /*! Called right after the apply method is called */ + virtual void apply_begin () {} + + /*! Called when the apply method is finished */ + virtual void apply_end () {} + + /*! Multiply mat to the topmost matrix in the matrix stack. */ + virtual void mult_matrix ( const SrMat& mat ); + + /*! Take the current topmost matrix in the matrix stack, and push a copy + into a new matrix position in the stack. */ + virtual void push_matrix (); + + /*! Pop one matrix from the matrix stack. */ + virtual void pop_matrix (); + + /*! Simply calls multi_matrix(), if m->visible() state is true. + False can be returned to stop the traverse, otherwise true must be returned. */ + virtual bool matrix_apply ( SrSnMatrix* m ); + + /*! Apply the action to each group child, pushing/popping the current + matrix if the group is acting as a separator. + False can be returned to stop the traverse, otherwise true must be returned. */ + virtual bool group_apply ( SrSnGroup* g ); + + /*! Apply the action to a shape, SrSa implementation simply returns true. + False can be returned to stop the traverse, otherwise true must be returned. */ + virtual bool shape_apply ( SrSnShapeBase* s ); + + /*! Apply the action to the child and helpers, pushing/popping the current + matrix. If the manipulator is not visible, only the child receives the action. + False can be returned to stop the traverse, otherwise true must be returned. */ + virtual bool editor_apply ( SrSnEditor* e ); + }; + +//================================ End of File ================================================= + +# endif // SR_SA_H + diff --git a/source/dcdt/se/sr_sa_bbox.cpp b/source/dcdt/se/sr_sa_bbox.cpp new file mode 100644 index 0000000000..e6236e5f30 --- /dev/null +++ b/source/dcdt/se/sr_sa_bbox.cpp @@ -0,0 +1,32 @@ +#include "precompiled.h" +//# include + +# include "sr_sa_bbox.h" +# include "sr_sn_shape.h" + +//# define SR_USE_TRACE1 // constructor / destructor +# include "sr_trace.h" + +//================================== SrSaBBox ==================================== + +bool SrSaBBox::shape_apply ( SrSnShapeBase* s ) + { + if ( !s->visible() ) return true; + SrBox b; + SrMat m; + s->get_bounding_box ( b ); + get_top_matrix ( m ); + _box.extend ( b * m ); + return true; + } +/* +bool SrSaBBox::manipulator_apply ( SrSceneShapeBase* s ) + { + SrBox b; + s->get_bounding_box ( b ); + _box.extend ( b * top_matrix() ); + return true; + } +*/ + +//======================================= EOF ==================================== diff --git a/source/dcdt/se/sr_sa_bbox.h b/source/dcdt/se/sr_sa_bbox.h new file mode 100644 index 0000000000..4451ca4e32 --- /dev/null +++ b/source/dcdt/se/sr_sa_bbox.h @@ -0,0 +1,31 @@ + +# ifndef SR_SA_BBOX_H +# define SR_SA_BBOX_H + +/** \file sr_sa_bbox.h + * retrives the bbox + */ + +# include "sr_sa.h" + +/*! \class SrSaBBox sr_sa_bbox.h + \brief bbox action + + Retrieves the bounding box of a scene */ +class SrSaBBox : public SrSa + { private : + SrBox _box; + + public : + void init () { _box.set_empty(); } + void apply ( SrSn* n ) { init(); SrSa::apply(n); } + const SrBox& get () const { return _box; } + + private : // virtual methods + virtual bool shape_apply ( SrSnShapeBase* s ); + }; + +//================================ End of File ================================================= + +# endif // SR_SA_BBOX_H + diff --git a/source/dcdt/se/sr_sa_eps_export.cpp b/source/dcdt/se/sr_sa_eps_export.cpp new file mode 100644 index 0000000000..9f69313360 --- /dev/null +++ b/source/dcdt/se/sr_sa_eps_export.cpp @@ -0,0 +1,379 @@ +#include "precompiled.h" +# include "sr_sa_eps_export.h" +# include "sr_sa_bbox.h" +# include "sr_sn_shape.h" +# include "sr_lines.h" +# include "sr_points.h" +# include "sr_polygons.h" + + +//# define SR_USE_TRACE1 // constructor / destructor +//# define SR_USE_TRACE2 // render +# include "sr_trace.h" + +//============================= export functions ==================================== + +// detect if lines are in 3d or 2d!!? +// make use of current matrix (and camera) transformation + +static void export_lines ( SrSnShapeBase* shape, SrOutput& o, const SrSaEpsExport* epsexp ) + { + SrLines& l = ((SrSnLines*)shape)->shape(); + SrArray& V = l.V; + SrArray& C = l.C; + SrArray& I = l.I; + float c[4]; + + if ( V.size()<2 ) return; + + shape->material().diffuse.get ( c ); + o << c[0] << srspc << c[1] << srspc << c[2] << " setrgbcolor\n"; + + float res = shape->resolution(); // 0.5 1.0 1.5 + res *= 0.02f; // makes resolution 1 be .02 cms + res *= 28.346f; // put in unit pts + res /= epsexp->scale_factor(); + o << res << " setlinewidth\n"; + + int v=0; // current V index + int i; // current I index + int imax = I.size()-1; // max value for i + int i1=-1, i2; // pair I[i],I[i+1] + + if ( I.size()>1 ) { i=0; i1=I[i]; i2=I[i+1]; } + + while ( vget(); + + if ( p.P.size()==0 ) return; + + glDisable ( GL_LIGHTING ); + glColor ( shape->material().diffuse ); + + if ( shape->render_mode()==srRenderModeSmooth ) + { // render shperes, with resolution as radius? + } + + glPointSize ( shape->resolution() ); // default is 1.0 + + int i; + glBegin ( GL_POINTS ); + for ( i=0; i tris; + p.ear_triangulation ( tris ); + glBegin ( GL_TRIANGLES ); + for ( i=0; ishape(); + if ( p.size()==0 ) return; + + float resolution = shape->resolution(); + +// glDisable ( GL_LIGHTING ); +// glColor ( shape->material().diffuse ); +// glLineWidth ( resolution ); // default is 1.0 + + export_polygon ( p, shape->render_mode(), resolution, o, epsexp ); + } + +static void export_polygons ( SrSnShapeBase* shape, SrOutput& o, const SrSaEpsExport* epsexp ) + { + SrPolygons& p = ((SrSnPolygons*)shape)->shape(); + + if ( p.size()==0 ) return; + + float resolution = shape->resolution(); +/* glDisable ( GL_LIGHTING ); + glColor ( shape->material().diffuse ); + glLineWidth ( resolution ); // default is 1.0 +*/ + int i; + for ( i=0; irender_mode(), resolution, o, epsexp ); + } + +//=============================== SrSaEpsExport ==================================== + +SrArray SrSaEpsExport::_efuncs; + +SrSaEpsExport::SrSaEpsExport ( SrOutput& o ) + : _output ( o ) + { + SR_TRACE1 ( "Constructor" ); + + _page_width = 21.59f; // == 612 pts + _page_height = 27.94f; // == 792 pts + _page_margin = 4.0f; + _bbox_margin = 0.1f; + + if ( _efuncs.size()==0 ) // no functions registered + { //register_export_function ( "model", export_model ); + register_export_function ( "lines", export_lines ); + register_export_function ( "points", export_points ); + //register_export_function ( "box", export_box ); + //register_export_function ( "sphere", export_sphere ); + register_export_function ( "polygon", export_polygon ); + register_export_function ( "polygons", export_polygons ); + } + } + +SrSaEpsExport::~SrSaEpsExport () + { + SR_TRACE1 ( "Destructor" ); + } + +void register_export_function ( const char* class_name, SrSaEpsExport::export_function efunc ) // friend function + { + SrArray &ef = SrSaEpsExport::_efuncs; + + int i; + for ( i=0; iinst_class_name (); + SrArray& ef = SrSaEpsExport::_efuncs; + for ( i=0; ivisible() ) return true; + + // 3. Export + ef[i].efunc ( s, _output, this ); + return true; + } + +//======================================= PS GUIDE ==================================== + +/* +http://adela.karlin.mff.cuni.cz/netkarl/prirucky/Flat/paths.html + +72 72 scale points to inches, 1 pt = 1/72 inches +2.8346 2.8346 scale points to mms +1.0 setlinewidth 0 is valid +2 setlinecap 0, 1, 2: butt ends, round ends, square ends +2 setlinejoin 0, 1, 2: miter corners, round corners, bevel corners +0.5 setgray 0 to 1 + +gsave +grestore + + +EXAMPLE: +%! + +0.025 setlinewidth +72 72 scale +0.8 setgray +newpath +1 1 moveto +7 1 lineto +1 5 lineto +closepath +fill + +0 setgray +newpath +1 1 moveto +7 1 lineto +1 5 lineto +closepath +stroke + +showpage + + +*/ + +//======================================= EOF ==================================== + diff --git a/source/dcdt/se/sr_sa_eps_export.h b/source/dcdt/se/sr_sa_eps_export.h new file mode 100644 index 0000000000..c783c3a4be --- /dev/null +++ b/source/dcdt/se/sr_sa_eps_export.h @@ -0,0 +1,71 @@ + +# ifndef SR_SA_EPS_EXPORT_H +# define SR_SA_EPS_EXPORT_H + +/** \file sr_sa_eps_export.h + * Encapsulated Postscript Export + */ + +# include "sr_sa.h" + +/*! Export the scene graph to a .eps file. + Note that not all nodes are supported, mainly only 2d shapes are supported*/ +class SrSaEpsExport : public SrSa + { public : + typedef void (*export_function)(SrSnShapeBase*,SrOutput&,const SrSaEpsExport*); + + private : + struct RegData { const char* class_name; + export_function efunc; + }; + static SrArray _efuncs; + SrOutput& _output; + float _page_width; + float _page_height; + float _page_margin; + float _bbox_margin; + float _scale_factor; + SrVec2 _translation; + + public : + /*! Constructor */ + SrSaEpsExport ( SrOutput& o ); + + /*! Virtual destructor. */ + virtual ~SrSaEpsExport (); + + float scale_factor () const { return _scale_factor; } + + /*! Set print page dimensions in cms. Default is w=21.59 and h=27.94 */ + void set_page ( float w, float h ) { _page_width=w; _page_height=h; } + + /*! Set the page margin in cms to use, default is 4cms */ + void set_page_margin ( float m ) { _page_margin=m; } + + /*! The eps bounding box might need to be increased to ensure that the + style of drawn shapes will not generate drawings outside the bouding + box limits. The default value is 0.1 cms. */ + void set_bbox_margin ( float m ) { _bbox_margin=m; } + + /*! Registration is kept in a static array, shared by all instances + of SrSaEpsExport. Currently only SrLine export is available. + Registration must be explicitly called for user-defined shapes. + In case a name already registered is registered again, the new + one replaces the old one. */ + friend void register_export_function ( const char* class_name, export_function efunc ); + + /*! Makes the node to start applying the action. If the action is not + applied to the entire scene, false is returned. */ + bool apply ( SrSn* n ); + + private : + virtual void mult_matrix ( const SrMat& mat ); + virtual void push_matrix (); + virtual void pop_matrix (); + virtual bool shape_apply ( SrSnShapeBase* s ); + }; + +//================================ End of File ================================================= + +# endif // SR_SA_EPS_EXPORT_H + diff --git a/source/dcdt/se/sr_sa_event.cpp b/source/dcdt/se/sr_sa_event.cpp new file mode 100644 index 0000000000..ae147dc074 --- /dev/null +++ b/source/dcdt/se/sr_sa_event.cpp @@ -0,0 +1,41 @@ +#include "precompiled.h" +//# include + +# include "sr_sn_editor.h" +# include "sr_sa_event.h" + +//# define SR_USE_TRACE1 // constructor / destructor +# include "sr_trace.h" + +//================================== SrSaEvent ==================================== + +bool SrSaEvent::editor_apply ( SrSnEditor* m ) + { + bool b=true; + if ( !m->visible() ) return b; + + push_matrix (); + mult_matrix ( m->mat() ); + + SrMat mat = _matrix_stack.top(); + mat.invert(); +//sr_out<handle_event ( e ); + if ( _result ) b = false; // event used: stop the scene traversing + + pop_matrix (); + return b; + } + +//======================================= EOF ==================================== diff --git a/source/dcdt/se/sr_sa_event.h b/source/dcdt/se/sr_sa_event.h new file mode 100644 index 0000000000..aab4cf9a37 --- /dev/null +++ b/source/dcdt/se/sr_sa_event.h @@ -0,0 +1,33 @@ + +# ifndef SR_SA_EVENT_H +# define SR_SA_EVENT_H + +/** \file sr_sa.h + * propagates events + */ + +# include "sr_sa.h" +# include "sr_event.h" + +/*! \class SrSaEvent sr_sa_event.h + \brief propagates events in a scene + + sends an event to the scene graph */ +class SrSaEvent : public SrSa + { private : + SrEvent _ev; + int _result; + + public : + SrSaEvent ( const SrEvent& e ) { _ev=e; _result=0; } + SrEvent& get () { return _ev; } + int result () const { return _result; } + + private : // virtual methods + virtual bool editor_apply ( SrSnEditor* m ); + }; + +//================================ End of File ================================================= + +# endif // SR_SA_EVENT_H + diff --git a/source/dcdt/se/sr_sa_gl_render.h b/source/dcdt/se/sr_sa_gl_render.h new file mode 100644 index 0000000000..73ab5ef858 --- /dev/null +++ b/source/dcdt/se/sr_sa_gl_render.h @@ -0,0 +1,67 @@ + +# ifndef SR_SA_GL_RENDER_H +# define SR_SA_GL_RENDER_H + +/** \file sr_gl_render_action.h + * OpenGL render action + */ + +# include +# include + +struct SrOverrideAction; +struct SrOGLData; + +/*! \class SrSaGlRender sr_gl_render_action.h + \brief OpenGL render action + + OpenGL render action */ +class SrSaGlRender : public SrSa + { public : + typedef void (*render_function)(SrSnShapeBase*); + + private : + struct RegData { const char* class_name; + render_function rfunc; + }; + static SrArray _rfuncs; + friend struct SrOverrideAction; + friend struct SrOGLData; + + public : + /*! Constructor */ + SrSaGlRender (); + + /*! Virtual destructor. */ + virtual ~SrSaGlRender (); + + /*! Registration is kept in a static array, shared by all instances + of SrSaGlRender. + All buit-in SR scene shapes are automatically registered when + the first SrSaGlRender instance is done. However, + registration must be explicitly called for user-defined shapes. + In case a name already registered is registered again, the new + one replaces the old one. */ + friend void register_render_function ( const char* class_name, render_function rfunc ); + + /*! Traverse all scene graph overriding the render mode of each shape node. */ + void override_render_mode ( SrSn* n, srRenderMode m ); + + /*! Traverse all scene graph restoring the original render mode of each + shape node, saved at the last override_render_mode() call. */ + void restore_render_mode ( SrSn* n ); + + private : + virtual void get_top_matrix ( SrMat& mat ); + virtual int matrix_stack_size (); + virtual void init_matrix (); + virtual void mult_matrix ( const SrMat& mat ); + virtual void push_matrix (); + virtual void pop_matrix (); + virtual bool shape_apply ( SrSnShapeBase* s ); + }; + +//================================ End of File ================================================= + +# endif // SR_SA_GL_RENDER_H + diff --git a/source/dcdt/se/sr_sa_model_export.cpp b/source/dcdt/se/sr_sa_model_export.cpp new file mode 100644 index 0000000000..c7b5eb6b41 --- /dev/null +++ b/source/dcdt/se/sr_sa_model_export.cpp @@ -0,0 +1,65 @@ +#include "precompiled.h" +# include "sr_sa_model_export.h" +# include "sr_model.h" +# include "sr_sn_shape.h" + +//# define SR_USE_TRACE1 // Const / Dest +//# define SR_USE_TRACE2 // +# include "sr_trace.h" + +//=============================== SrSaModelExport ==================================== + +SrSaModelExport::SrSaModelExport ( const SrString& dir ) + { + SR_TRACE1 ( "Constructor" ); + directory ( dir ); + _num = 0; + _prefix = "model"; + } + +SrSaModelExport::~SrSaModelExport () + { + SR_TRACE1 ( "Destructor" ); + } + +void SrSaModelExport::directory ( const SrString& dir ) + { + _dir = dir; + char c = _dir.last_char(); + if ( c==0 || c=='/' || c=='\\' ) return; + _dir << '/'; + } + +bool SrSaModelExport::apply ( SrSn* n ) + { + _num = 0; + bool result = SrSa::apply ( n ); + return result; + } + +//==================================== virtuals ==================================== + +bool SrSaModelExport::shape_apply ( SrSnShapeBase* s ) + { + if ( sr_compare(s->inst_class_name(),"model")!=0 ) return true; + if ( !s->visible() ) return true; + + SrMat mat; + get_top_matrix ( mat ); + + SrModel model; + model = ((SrSnModel*)s)->shape(); + model.apply_transformation ( mat ); + + SrString fname; + fname.setf ( "%s%s%04d.srm", (const char*)_dir,(const char*)_prefix, _num++ ); + SrOutput out ( fopen(fname,"wt") ); + if ( !out.valid() ) return false; + + model.save ( out ); + + return true; + } + +//======================================= EOF ==================================== + diff --git a/source/dcdt/se/sr_sa_model_export.h b/source/dcdt/se/sr_sa_model_export.h new file mode 100644 index 0000000000..0ad18ffed1 --- /dev/null +++ b/source/dcdt/se/sr_sa_model_export.h @@ -0,0 +1,42 @@ + +# ifndef SR_SA_MODEL_EXPORT_H +# define SR_SA_MODEL_EXPORT_H + +/** \file sr_sa_model_exportT.h + * Export all models in global coordinates + */ + +# include "sr_sa.h" + +/*! Export all models in the scene graph in global coordinates + to several .srm files. */ +class SrSaModelExport : public SrSa + { private : + SrString _dir; + SrString _prefix; + int _num; + public : + /*! Constructor */ + SrSaModelExport ( const SrString& dir ); + + /*! Virtual destructor. */ + virtual ~SrSaModelExport (); + + /*! Change the directory to save files */ + void directory ( const SrString& dir ); + + /*! Change the prefix name of the files */ + void prefix ( const SrString& pref ) { _prefix=pref; } + + /*! Start applying the action. If the action is not + applied to the entire scene, false is returned. */ + bool apply ( SrSn* n ); + + private : + virtual bool shape_apply ( SrSnShapeBase* s ); + }; + +//================================ End of File ================================================= + +# endif // SR_SA_MODEL_EXPORT_H + diff --git a/source/dcdt/se/sr_sa_render_mode.cpp b/source/dcdt/se/sr_sa_render_mode.cpp new file mode 100644 index 0000000000..0aa775ddea --- /dev/null +++ b/source/dcdt/se/sr_sa_render_mode.cpp @@ -0,0 +1,23 @@ +#include "precompiled.h" +//# include + +# include "sr_sa_render_mode.h" + +# include "sr_sn_shape.h" + +//# define SR_USE_TRACE1 // constructor / destructor +# include "sr_trace.h" + +//=============================== SrSaRenderMode ==================================== + +bool SrSaRenderMode::shape_apply ( SrSnShapeBase* s ) + { + if ( _override ) + s->override_render_mode ( _render_mode ); + else + s->restore_render_mode (); + + return true; + } + +//======================================= EOF ==================================== diff --git a/source/dcdt/se/sr_sa_render_mode.h b/source/dcdt/se/sr_sa_render_mode.h new file mode 100644 index 0000000000..9b2f9fc4b5 --- /dev/null +++ b/source/dcdt/se/sr_sa_render_mode.h @@ -0,0 +1,46 @@ + +# ifndef SR_SA_RENDER_MODE_H +# define SR_SA_RENDER_MODE_H + +/** \file sr_sa_render_mode.h + * changes the render mode + */ + +# include "sr_sa.h" +# include "sr_sn_shape.h" + +/*! \class SrSaRenderMode sr_sa_render_mode.h + \brief changes the render mode + + changes the render mode of all nodes visited by the action */ +class SrSaRenderMode : public SrSa + { private : + bool _override; + srRenderMode _render_mode; + + public : + + /*! Constructor that initializes the action to override the render mode to m */ + SrSaRenderMode ( srRenderMode m ) { set_mode(m); } + + /*! Constructor that initializes the action to restore the original render mode */ + SrSaRenderMode () { _render_mode=srRenderModeSmooth; _override=false; } + + /*! Set the mode m to be overriden */ + void set_mode ( srRenderMode m ) { _render_mode=m; _override=true; } + + /*! Set the action to restore the original render mode */ + void restore_mode () { _override=false; } + + private : // virtual methods + virtual void mult_matrix ( const SrMat& mat ) {} + virtual void push_matrix () {} + virtual void pop_matrix () {} + virtual bool shape_apply ( SrSnShapeBase* s ); + virtual bool matrix_apply ( SrSnMatrix* m ) { return true; } + }; + +//================================ End of File ================================================= + +# endif // SR_SA_RENDER_MODE_H + diff --git a/source/dcdt/se/sr_set.cpp b/source/dcdt/se/sr_set.cpp new file mode 100644 index 0000000000..3b8e2d859c --- /dev/null +++ b/source/dcdt/se/sr_set.cpp @@ -0,0 +1,210 @@ +#include "precompiled.h" +# include +# include "sr_set.h" + +//============================= SrSetBasic ================================ + +int SrSetBasic::insert ( void* pt ) + { + int i; + + if ( _freepos.size()>0 ) + { i=_freepos.pop(); _data[i]=pt; } + else + { i=_data.size(); _data.push()=pt; } + + return i; + } + +void* SrSetBasic::extract ( int i ) + { + if ( i<0 || i>=_data.size() ) return 0; + if ( !_data[i] ) return 0; + + void* pt = _data[i]; + _data[i]=0; + + if ( i==_data.size()-1 ) + { _data.pop(); } + else + { _freepos.push()=i; } + + return pt; + } + +void SrSetBasic::compress () + { + if ( _data.size()>0 ) + { int i, n; + while ( _data.top()==0 ) + { _data.pop(); + n = _data.size(); + for ( i=0; i<_freepos.size(); i++ ) + if ( _freepos[i]==n ) { _freepos[i]=_freepos.pop(); break; } + } + } + _data.compress(); + _freepos.compress(); + } + +bool SrSetBasic::remove_gaps ( SrArray& newindices ) + { + int i, j, gap=0; + + newindices.size ( _data.size() ); + + // advance untill first null + for ( i=0; i<_data.size(); i++ ) + { if ( _data[i] ) newindices[i]=i; + else break; + } + if ( i==_data.size() ) return false; // nothing done + + j = i; + while ( i<_data.size() ) + { if ( !_data[i] ) + { newindices[i] = -1; } + else + { _data[j]=_data[i]; + _data[i]=0; + newindices[i] = j; + j++; + } + i++; + } + + _data.size ( j ); + _freepos.size ( 0 ); + return true; + } + +//============================== SrSetBase ================================ + +SrSetBase::SrSetBase ( SrClassManagerBase* m ) + { + _man = m; + _man->ref(); + } + +SrSetBase::SrSetBase ( const SrSetBase& s ) + { + _man = s._man; + _man->ref(); + copy ( s ); + } + +SrSetBase::~SrSetBase () + { + init (); + _man->unref(); + } + +void SrSetBase::init () + { + void* pt; + while ( _data.size() ) + { pt = _data.pop(); + if ( pt ) _man->free ( pt ); + } + _freepos.size(0); + } + +void SrSetBase::copy ( const SrSetBase& s ) + { + init (); + _freepos = s._freepos; + _data = s._data; + + int i; + for ( i=0; i<_data.size(); i++ ) + if ( _data[i] ) _data[i]=_man->alloc(s._data[i]); + } + +void SrSetBase::remove ( int i ) + { + void* pt = extract ( i ); + if ( pt ) _man->free ( pt ); + } + +SrOutput& operator<< ( SrOutput& out, const SrSetBase& s ) + { + int i, lasti, n=0; + + lasti = s.maxid(); + while ( lasti>=0 && s._data[lasti]==0 ) lasti--; + + out<<'['; + + for ( i=0; ioutput(out,s._data[i]); + if ( n++> ( SrInput& inp, SrSetBase& s ) + { + s.init (); + int id, mi; + SrArray ids; + + inp.get_token(); // '[' + while (true) + { inp.get_token(); + if ( inp.last_token()[0]==']' ) + { break; } + else inp.unget_token(); + + s._data.push() = s._man->alloc(); + s._man->input ( inp, s._data.top() ); + + inp.get_token(); + if ( inp.last_token()[0]==':' ) + { id = atoi(inp.getn()); + mi = s.maxid(); + while ( mi=_minid && !_set.get(_curid) ) _curid--; + } + +//============================== EOF ================================ diff --git a/source/dcdt/se/sr_set.h b/source/dcdt/se/sr_set.h new file mode 100644 index 0000000000..295874726b --- /dev/null +++ b/source/dcdt/se/sr_set.h @@ -0,0 +1,186 @@ +# ifndef SR_SET_H +# define SR_SET_H + +/** \file sr_set.h + * indexed set of pointers */ + +# include "sr_array.h" +# include "sr_class_manager.h" + +/*! The simplest way to use a 'Set' by means of void pointers. + This is a low level management class and there is no + destructor to delete stored data. The user is completely + responsible of the data allocation and deletion. + Here is an example of how to delete all data in the set: + for ( int id=0; id<=set.maxid(); id++ ) + delete ((UserClassTypeCast*)set.get(id)); // ok with null pointers + set.init_arrays(); */ +class SrSetBasic + { protected : + SrArray _data; // data list + SrArray _freepos; // keeps freed positions in the array + + public : + /*! Will set the sizes of the internal arrays to 0. This is + to be called only in case it is guaranteed that all referenced + objects are already deleted. Use with care! */ + void init_arrays () { _data.size(0); _freepos.size(0); } + + /*! Inserts pt in one empty position and return its index. If a class + manager is used (by means of SrSetBase), pt must be an allocated + pointer of the same element being managed by the class manager. */ + int insert ( void* pt ); + + /*! Removes and returns the element stored at position i. */ + void* extract ( int i ); + + /*! Compress internal arrays, removing possible nonused positions. */ + void compress (); + + /*! Removes all internal gaps changing current existing indices. + Old indices can be updated with their new values, which are + stored in the given Array newindices in the following way: + newindices[oldindex] == newindex */ + bool remove_gaps ( SrArray& newindices ); + + /*! Returns the number of elements in the Set */ + int SrSetBasic::elements () const { return _data.size()-_freepos.size(); } + + /*! Returns true if the number of elements is 0, or false otherwise */ + bool empty() const { return elements()==0? true:false; } + + /*! maxid() returns the maximum existing id or -1 if the set is empty */ + int maxid () const { return _data.size()-1; } + + /*! Returns the element pointer stored at position i. + It will return null for previously deleted positions; and + it is the user responsability to ensure that i is not out of range. */ + void* get ( int i ) const { return _data[i]; } + + /*! Const version of get() */ + const void* const_get ( int i ) const { return _data[i]; } + }; + +/*! \class SrSetBase sr_set.h + \brief Base class for SrSet + + SrSet saves in an array, pointers for user data, which can be later + retrieved with indices. Indices are unique and are not changed + during insertions and removals. Indices are always >=0. + As no rearrangements are performed when removals are done, + internal free positions (gaps) are created. These gaps can be + removed, but then indices to access data will change. */ +class SrSetBase : public SrSetBasic + { private: + SrClassManagerBase* _man; + public: + + /*! Creates a new Set sharing the given class manager */ + SrSetBase ( SrClassManagerBase* m ); + + /*! Creates a new Set containing a copy of all elements in s, and + sharing the same class manager */ + SrSetBase ( const SrSetBase& s ); + + /*! Destructor deletes all data internally referenced */ + ~SrSetBase (); + + /*! Deletes all data internally referenced, and make an empty Set */ + void init (); + + /*! Init Set and make it become a copy of s, with the same indices + and possible internal gaps. */ + void copy ( const SrSetBase& s ); + + /*! Allocates one element using the associated class manager, insert + it in one empty position and return its index. */ + int insert () { return SrSetBasic::insert(_man->alloc()); } + + /*! Deletes and removes the element stored at position i. */ + void remove ( int i ); + + friend SrOutput& operator<< ( SrOutput& out, const SrSetBase& s ); + + friend SrInput& operator>> ( SrInput& inp, SrSetBase& s ); + }; + +template +class SrSet : public SrSetBase + { public: + SrSet () : SrSetBase ( new SrClassManager ) {} + SrSet ( SrClassManagerBase* m ) : SrSetBase ( m ) {} + SrSet ( const SrSet& s ) : SrSetBase ( s ) {} + ~SrSet () {} + int insert ( X* x ) { return SrSetBasic::insert( (void*)x ); } + int insert () { return SrSetBase::insert(); } + X* extract ( int i ) { return (X*)SrSetBase::extract(i); } + X* get ( int i ) const { return (X*)SrSetBase::get(i); } + const X* const_get ( int i ) const { return (const X*)SrSetBase::const_get(i); } + X* operator[] ( int i ) const { return (X*)SrSetBase::get(i); } + void operator = ( const SrSet& s ) + { SrArrayBase::copy ( s ); } + }; + +/*! Base class for iterating over sets. */ +class SrSetIteratorBase + { private : + int _maxid; + int _minid; + int _curid; + const SrSetBase& _set; + + public : + /*! Constructor */ + SrSetIteratorBase ( const SrSetBase& s ); + + /*! Returns the current index being pointed by the iterator */ + int curid () const { return _curid; } + + /*! Returns the max index being pointed by the iterator */ + int maxid () const { return _maxid; } + + /*! Returns the min index being pointed by the iterator */ + int minid () const { return _minid; } + + /*! Must be called each time the associate set is changed */ + void reset (); + + /*! Points the iterator to the first element. */ + void first () { _curid=_minid; } + + /*! Points the iterator to the last element. */ + void last () { _curid=_maxid; } + + /*! Advances the current position of the iterator of one position */ + void next (); + + /*! Walk back the current position of the iterator of one position */ + void prior (); + + /*! Returns true if get() points to a valid position */ + bool inrange () { return _curid>=_minid && _curid<=_maxid? true:false; } + + /*! Returns the current element */ + void* get () { return _set.get(_curid); } + + /*! Returns true if the current position is pointing to the last element. */ + bool inlast () const { return _curid==_maxid? true:false; } + + /*! Returns true if the current position is pointing to the first element, + or if the list is empty. */ + bool infirst () const { return _curid==_minid? true:false; } + }; + +/*! Derives SrSetIteratorBase providing correct type casts for the user type */ +template +class SrSetIterator : public SrSetIteratorBase + { public : + SrSetIterator ( const SrSet& s ) : SrSetIteratorBase(s) {} + X* get () { return (X*)SrSetIteratorBase::get(); } + X* operator-> () { return (X*)SrSetIteratorBase::get(); } + }; + +//============================== end of file =============================== + +#endif // SR_SET_H + diff --git a/source/dcdt/se/sr_shared_class.h b/source/dcdt/se/sr_shared_class.h new file mode 100644 index 0000000000..10e2ffd06b --- /dev/null +++ b/source/dcdt/se/sr_shared_class.h @@ -0,0 +1,37 @@ + +# ifndef SR_SHARED_CLASS_H +# define SR_SHARED_CLASS_H + +/*! \file sr_shared_class.h + reference counter for smart-pointer like behavior. + Note: attention is required to avoid circular references */ +class SrSharedClass + { private : + int _ref; + + protected : + + /*! Constructor initializes the reference counter as 0 */ + SrSharedClass () { _ref=0; }; + + /*! Destructor in derived classes should always be declared as + protected in order to oblige users to call always unref(), + instead of delete */ + virtual ~SrSharedClass() {}; + + public : + + /*! Returns the current reference counter value. */ + int getref () const { return _ref; } + + /*! Increments the reference counter. */ + void ref () { _ref++; } + + /*! Decrements the reference counter (if >0), and if the + counter becomes 0, the class is automatically self deleted. */ + void unref() { if(_ref>0) _ref--; if(_ref==0) delete this; } + }; + +//============================== end of file =============================== + +# endif // SR_SHARED_CLASS_H diff --git a/source/dcdt/se/sr_sn.cpp b/source/dcdt/se/sr_sn.cpp new file mode 100644 index 0000000000..7c8bef3d78 --- /dev/null +++ b/source/dcdt/se/sr_sn.cpp @@ -0,0 +1,37 @@ +#include "precompiled.h" +# include "sr_sn.h" + +//# define SR_USE_TRACE1 // SrSn Const/Dest +# include "sr_trace.h" + +//======================================= SrSn ==================================== + +SrSn::SrSn ( SrSn::Type t, const char* class_name ) + { + SR_TRACE1 ( "Constructor" ); + _ref = 0; + _visible = 1; + _type = t; + _label = 0; + _inst_class_name = class_name; + } + +SrSn::~SrSn () + { + SR_TRACE1 ( "Destructor" ); + delete _label; + } + +void SrSn::label ( const char* s ) + { + sr_string_set ( _label, s ); + } + +const char* SrSn::label () const + { + return _label? _label:""; + } + + +//======================================= EOF ==================================== + diff --git a/source/dcdt/se/sr_sn.h b/source/dcdt/se/sr_sn.h new file mode 100644 index 0000000000..96cb076368 --- /dev/null +++ b/source/dcdt/se/sr_sn.h @@ -0,0 +1,80 @@ + +# ifndef SR_SN_H +# define SR_SN_H + +/** \file sr_sn.h + * base scene node class + */ + +# include "sr_box.h" +# include "sr_mat.h" +# include "sr_event.h" +# include "sr_array.h" +# include "sr_material.h" +# include "sr_shared_class.h" + +//======================================= SrSn ==================================== + +/*! \class SrSn sr_sn.h + \brief scene node base class + + Defines a base class for all scene nodes, which might be of + four types: group, editor, matrix, or shape. + 1. It cannot be directly instantiated by the user, and so there is not + a public constructor available. + 2. The scene graph was designed to be simple and small. There are only + few basic nodes defined in the sr library. + 3. A scene node does not have any information about its parent in the + scene graph, and node sharing is supported. A reference counter is + used for automatically deleting unused nodes, see ref() and unref() + methods in the parent class SrSharedClass. */ +class SrSn : public SrSharedClass + { public : + enum Type { TypeMatrix, TypeGroup, TypeEditor, TypeShape }; + + private : + int _ref; + char _visible; + char _type; + char* _label; + const char* _inst_class_name; + + protected : + /* Constructor requires the definition of the node type, and a const string + containing the name of the instantiated node. The convention for the class_name + is to take exactly the name of the class, without the leading "SrSn" + word. The type parameter indicates how the node should behave when the + scene graph is traversed by an action. */ + SrSn ( Type t, const char* class_name ); + + /*! This is a virtual destructor, so each derived class is responsible to + delete/unref its internal data. */ + virtual ~SrSn (); + + public : + /*! Returns a const string with the name of the instantiated node class. + The convention is exactly the name of the instantiated class, + without the leading "SrSn" word. */ + const char* inst_class_name () const { return _inst_class_name; } + + /*! Returns the type of this node. The type is set at instantiation + and cannot be changed later. */ + Type type () const { return (Type) _type; } + + /*! Gets the visible state of this node. */ + bool visible () const { return _visible? true:false; } + + /*! Changes the active state of this node. */ + void visible ( bool b ) { _visible = (char)b; } + + /*! Defines any label to be kept within the node. */ + void label ( const char* s ); + + /*! Returns the associated label, or an empty string. */ + const char* label () const; + }; + +//================================ End of File ================================================= + +# endif // SR_SN_H + diff --git a/source/dcdt/se/sr_sn_editor.cpp b/source/dcdt/se/sr_sn_editor.cpp new file mode 100644 index 0000000000..674e084a0c --- /dev/null +++ b/source/dcdt/se/sr_sn_editor.cpp @@ -0,0 +1,45 @@ +#include "precompiled.h" +# include "sr_sn_editor.h" + +//# define SR_USE_TRACE1 // SrSn Const/Dest +//# define SR_USE_TRACE2 // SrSceneGroup Const/Dest +//# define SR_USE_TRACE3 // SrSceneGroup children management +//# define SR_USE_TRACE4 // SrSceneMatrix Const/Dest +//# define SR_USE_TRACE5 // SrSceneShapeBase Const/Dest +# include "sr_trace.h" + + +//======================================= SrSnEditor ==================================== + +SrSnEditor::SrSnEditor ( const char* name ) + :SrSn ( SrSn::TypeEditor, name ) + { + SR_TRACE2 ( "Constructor" ); + _child = 0; + _helpers = new SrSnGroup; + _helpers->ref (); + } + +SrSnEditor::~SrSnEditor () + { + SR_TRACE2 ( "Destructor" ); + _helpers->unref (); + child ( 0 ); + } + +// protected : + +void SrSnEditor::child ( SrSn *sn ) + { + if ( _child ) _child->unref(); + if ( sn ) sn->ref(); // Increment reference counter + _child = sn; + } + +int SrSnEditor::handle_event ( const SrEvent &e ) + { + return 0; + } + +//======================================= EOF ==================================== + diff --git a/source/dcdt/se/sr_sn_editor.h b/source/dcdt/se/sr_sn_editor.h new file mode 100644 index 0000000000..d0206ac6d2 --- /dev/null +++ b/source/dcdt/se/sr_sn_editor.h @@ -0,0 +1,62 @@ + +# ifndef SR_SN_EDITOR_H +# define SR_SN_EDITOR_H + +/** \file sr_sn_editor.h + * base editor node + */ + +# include "sr_sn.h" +# include "sr_sn_group.h" + +//==================================== SrSnEditor ==================================== + +/*! \class SrSnEditor sr_sn_editor.h + \brief manipulates/edit scene nodes + + SrSnEditor is to be used as a base class to be derived. + It keeps one child which is the node to manipulate, a + transformation matrix, and a list of internal nodes to display + any required manipulation/edition helpers. */ +class SrSnEditor : public SrSn + { private : + SrMat _mat; + SrSn* _child; + SrSnGroup* _helpers; + + protected : + /*! Changes the current node to be manipulated, normally this is to be + managed by the derived class specific implementation */ + void child ( SrSn *sn ); + + /*! Destructor dereferences the child and the helpers group. + Only accessible through unref(). */ + virtual ~SrSnEditor (); + + public : + /*! Constructor requires the name of the derived class. */ + SrSnEditor ( const char* name ); + + /*! Get a reference to the manipulator matrix. */ + SrMat& mat () { return _mat; } + + /*! Set a new manipulation matrix. */ + void mat ( const SrMat& m ) { _mat=m; } + + /*! Get pointer to the helpers group. Only derived classes should operate + on the helpers class, however other classes (like the draw action) need + at least reading access */ + SrSnGroup* helpers () const { return _helpers; } + + /*! Get a pointer to the current child. Can be null. */ + SrSn* child () const { return _child; } + + /*! If 1 is returned, the event is no more propagated and the scene will be + redrawn. The SrSnEditor implementation simply returns 0 */ + virtual int handle_event ( const SrEvent &e ); + }; + +//================================ End of File ================================================= + +# endif // SR_SN_EDITOR_H + diff --git a/source/dcdt/se/sr_sn_group.cpp b/source/dcdt/se/sr_sn_group.cpp new file mode 100644 index 0000000000..b6cc9bc2db --- /dev/null +++ b/source/dcdt/se/sr_sn_group.cpp @@ -0,0 +1,108 @@ +#include "precompiled.h" +# include "sr_sn_group.h" + +//# define SR_USE_TRACE1 // Const/Dest +# include "sr_trace.h" + +//======================================= SrSnGroup ==================================== + +const char* SrSnGroup::class_name = "Group"; + +SrSnGroup::SrSnGroup () + :SrSn ( SrSn::TypeGroup, SrSnGroup::class_name ) + { + SR_TRACE1 ( "Constructor" ); + _separator = false; + } + +SrSnGroup::~SrSnGroup () + { + SR_TRACE1 ( "Destructor" ); + remove_all (); + } + +void SrSnGroup::capacity ( int c ) + { + if ( c<=_children.size() ) return; + _children.capacity ( c ); + } + +SrSn* SrSnGroup::get ( int pos ) const + { + if ( _children.size()==0 ) return 0; + if ( pos<0 || pos>=_children.size() ) return _children.get(_children.size()-1); + return _children.get(pos); + } + +int SrSnGroup::search ( SrSn *n ) const + { + for ( int i=0; i<_children.size(); i++ ) + { if ( _children[i]==n ) return i; } + + return -1; + } + +void SrSnGroup::add ( SrSn *sn, int pos ) + { + sn->ref(); // Increment reference counter + + if ( pos<0 || pos>=_children.size() ) // Append + { _children.push() = sn; + } + else // Insert + { _children.insert(pos) = sn; + } + } + +SrSn *SrSnGroup::remove ( int pos ) + { + SR_TRACE3 ( "remove "<=_children.size() ) // get last child + { sn = _children.pop(); + } + else // remove item in the middle + { sn = _children[pos]; + _children.remove(pos); + } + + int oldref = sn->getref(); + sn->unref(); + return oldref>1? sn:0; + } + +SrSn *SrSnGroup::remove ( SrSn *n ) + { + int pos = search ( n ); + if ( pos<0 ) return 0; + return remove ( pos ); + } + +SrSn *SrSnGroup::replace ( int pos, SrSn *sn ) + { + SR_TRACE3 ( "replace "<=_children.size() ) return 0; // invalid pos + + sn->ref(); + SrSn *old = _children[pos]; + _children[pos] = sn; + + int oldref = old->getref(); + old->unref(); + return oldref>1? old:0; + } + +void SrSnGroup::remove_all () + { + SR_TRACE3 ( "remove_all" ); + while ( !_children.empty() ) _children.pop()->unref(); + } + +//======================================= EOF ==================================== + diff --git a/source/dcdt/se/sr_sn_group.h b/source/dcdt/se/sr_sn_group.h new file mode 100644 index 0000000000..369e185cf1 --- /dev/null +++ b/source/dcdt/se/sr_sn_group.h @@ -0,0 +1,97 @@ + +# ifndef SR_SN_GROUP_H +# define SR_SN_GROUP_H + +/** \file sr_sn_group.h + * groups scene nodes + */ + +# include "sr_sn.h" + +//======================================= SrSnGroup ==================================== + +/*! \class SrSnGroup sr_scene.h + \brief groups scene graph nodes + + SrSnGroup keeps a list of children. The group can be set or not + to behave as a separator of the render state during action traversals. + By default, it is not set as a separator. */ +class SrSnGroup : public SrSn + { private : + char _separator; + SrArray _children; + + public : + static const char* class_name; + + protected : + /*! The destructor will remove all children in the subtree. + Only accessible through unref(). */ + virtual ~SrSnGroup (); + + public : + /*! Default constructor. Separator behavior is set to false. */ + SrSnGroup (); + + /*! Sets the group separator behavior. If it is set to true, the + render state is pushed to restored after the traversal of the + group children. Mainly used to localize the effect of transformation + matrices, applying only to the group children. */ + void separator ( bool b ) { _separator = (char)b; } + + /*! Returns the group separator behavior state. */ + bool separator () const { return _separator? true:false; } + + /*! Changes the capacity of the children array. If the requested capacity + is smaller than the current size, nothing is done. */ + void capacity ( int c ); + + /*! Compresses the children array. */ + void compress () { _children.compress(); } + + /*! Returns the number of children. */ + int size () const { return _children.size(); } + + /*! Returns the number of children (same as size()). */ + int num_children () const { return _children.size(); } + + /*! Get the child at position pos. If pos is invalid (as -1) the last + child is returned, and if there are no children, 0 is returned. */ + SrSn* get ( int pos ) const; + + /*! Searches for the child position. -1 is returned if the child is + not found. */ + int search ( SrSn *n ) const; + + /*! If pos<0 or pos>=num_children(), sn is appended. Otherwise, sn is + inserted in the given position in the children array, and + reallocation is done if required. */ + void add ( SrSn *sn, int pos=-1 ); + + /*! Removes one child. + If the node removed is no more referenced by the scene graph, it is + deallocated together with all its sub-graph and 0 is returned. 0 is + also returned if the group has no children. If pos==-1 (the default) + or pos is larger than the maximum child index, the last child is + removed. Otherwise, the removed node is returned. */ + SrSn *remove ( int pos=-1 ); + + /*! Searches for the position of the given child pointer and removes it + with remove_child ( position ). */ + SrSn *remove ( SrSn *n ); + + /*! Removes child pos and insert sn in place. Same reference rules of + remove applies. Will return the old node or 0 if the node is deleted + because its ref counter reached zero. If pos is out of range, 0 is + returned and nothing is done. */ + SrSn *replace ( int pos, SrSn *sn ); + + /*! Removes all children, calling the unref() method of each children. + The result is the same as calling remove_child() for each child. */ + void remove_all (); + }; + +//================================ End of File ================================================= + +# endif // SR_SN_GROUP_H + diff --git a/source/dcdt/se/sr_sn_manipulator.cpp b/source/dcdt/se/sr_sn_manipulator.cpp new file mode 100644 index 0000000000..f8e98070c6 --- /dev/null +++ b/source/dcdt/se/sr_sn_manipulator.cpp @@ -0,0 +1,233 @@ +#include "precompiled.h" +# include + +# include "sr_sn_manipulator.h" + +# include "sr_quat.h" +# include "sr_box.h" +# include "sr_lines.h" +# include "sr_sa_bbox.h" + +//# define SR_USE_TRACE1 // events +# include "sr_trace.h" + +//============================ SrSnManipulator ==================================== + +const char* SrSnManipulator::class_name = "Manipulator"; + +SrSnManipulator::SrSnManipulator () + :SrSnEditor ( SrSnManipulator::class_name ) + { + _box = new SrSnBox; + _box->render_mode ( srRenderModeLines ); + _box->can_override_render_mode ( false ); + _box->visible ( false ); + + _dragsel = new SrSnLines; + _dragsel->color ( SrColor::cyan ); + + helpers()->add ( _box ); + helpers()->add ( _dragsel ); + + _precision_in_pixels = 6; + + _rx = _ry = _rz = 0; + + _user_cb = 0; + _user_cb_data = 0; + + _translationray = false; + + init (); + } + +SrSnManipulator::~SrSnManipulator () + { + } + +void SrSnManipulator::child ( SrSn *sn ) + { + SrSnEditor::child ( sn ); + update (); + } + +void SrSnManipulator::initial_mat ( const SrMat& m ) + { + SrSnEditor::mat ( m ); + _translation = SrVec::null; + _rotation = SrQuat::null; + _initmat = m; + _mode=ModeWaiting; + _firstp = SrPnt::null; + _transform ( SrPnt::null, SrVec::null ); + } + +void SrSnManipulator::init () + { + _dragsel->shape().init(); + _mode = ModeWaiting; + _corner = -1; + + if ( SrSnEditor::child() ) + { _box->visible ( true ); + } + else + { _box->visible ( false ); + } + } + +void SrSnManipulator::update () + { + SrSaBBox bbox_action; + bbox_action.apply ( SrSnEditor::child() ); + _box->shape() = bbox_action.get(); + init (); + } + +void SrSnManipulator::translation ( const SrVec& t ) + { + _translation = t; + + _mode=ModeWaiting; + _firstp = SrPnt::null; + _transform ( SrPnt::null, SrVec::null ); + } + +void SrSnManipulator::_transform ( const SrPnt& p, const SrVec& r ) + { + SrVec dt; + + if ( _mode==ModeRotating ) + { } + else + { dt = p-_firstp; } + + SrMat R; + SrQuat q; + if ( r.x ) q.set ( SrVec::i, r.x ); + if ( r.y ) q.set ( SrVec::j, r.y ); + if ( r.z ) q.set ( SrVec::k, r.z ); + _rotation = _rotation * q; + + _rotation.get_mat ( R ); + _translation += dt * R; + + R[12] = _translation.x; + R[13] = _translation.y; + R[14] = _translation.z; + + SrSnEditor::mat ( R * _initmat ); + } + +void SrSnManipulator::_set_drag_selection ( const SrPnt& p ) + { + float factor = 8; + + if ( _mode==ModeRotating ) factor/=2.0f; + + SrLines& l = _dragsel->shape(); + l.init(); + + if ( _mode==ModeRotating ) + { l.push_line ( _center, p ); + } + else + { SrVec v1 = _bside[1]-_bside[0]; + SrVec v2 = _bside[3]-_bside[0]; + v1 /= 2; + v2 /= 2; + if ( _translationray ) + { float l1 = v1.len()/4; + float l2 = v2.len()/4; + if ( l1>l2 ) l1=l2; + v1.len(l1); v2.len(l1); + l.push_circle_approximation ( p, v1, cross(v1,v2), 12 ); + } + else + { l.push_line ( p-v1, p+v1 ); + l.push_line ( p-v2, p+v2 ); + } + } + } + +int SrSnManipulator::handle_event ( const SrEvent &e ) + { + if ( _mode!=ModeWaiting && e.type==SrEvent::Release ) + { SR_TRACE1 ( "RELEASE" ); + init (); + if ( _user_cb ) _user_cb ( this, e, _user_cb_data ); + return 1; + } + + if ( _mode==ModeWaiting && e.type==SrEvent::Push ) + { SR_TRACE1 ( "PUSH" ); + float t1, t2; + int k = e.ray.intersects_box ( _box->const_shape(), t1, t2, _bside ); + if ( k>0 ) + { _firstp = SR_LERP(e.ray.p1,e.ray.p2,t1); + _plane.set ( _bside[0], _bside[1], _bside[2] ); + _mode = ModeTranslating; + _set_drag_selection ( _firstp ); + if ( _user_cb ) _user_cb ( this, e, _user_cb_data ); + return 1; + } + } + + if ( _mode==ModeTranslating && e.type==SrEvent::Drag ) + { SR_TRACE1 ( "TRANSLATING" ); + SrPnt p; + if ( _translationray ) + p = e.ray.closestpt ( _firstp ); + else + p = _plane.intersect ( e.ray.p1, e.ray.p2 ); + _transform ( p, SrVec::null ); + _set_drag_selection ( p ); + if ( _user_cb ) _user_cb ( this, e, _user_cb_data ); + return 1; + } + + if ( _mode==ModeTranslating && e.type==SrEvent::Keyboard ) + { SR_TRACE1 ( "ROTATING" ); + if ( e.key=='p' ) sr_out<0 ) + { transform ( ip[0] ); + set_drag_selection ( _bside[_corner] ); + } + return 1; + } +*/ + _mode = ModeWaiting; + + return 0; // event not used + } + +//================================ End of File ================================================= + diff --git a/source/dcdt/se/sr_sn_manipulator.h b/source/dcdt/se/sr_sn_manipulator.h new file mode 100644 index 0000000000..d3daf2988b --- /dev/null +++ b/source/dcdt/se/sr_sn_manipulator.h @@ -0,0 +1,109 @@ + +# ifndef SR_SCENE_MANIPULATOR_H +# define SR_SCENE_MANIPULATOR_H + +/** \file sr_scene.h + * basic scene elements + */ + +# include "sr_quat.h" +# include "sr_plane.h" +# include "sr_sn_editor.h" +# include "sr_sn_shape.h" +# include "sr_polygons.h" + +class SrBox; +class SrLines; +class SrSphere; + +//==================================== SrSnManipulator ==================================== + +/*! \class SrSnManipulator sr_sn_manipulator.h + \brief edit polygons + + SrSnManipulator puts a bounding box around the child object and lets + the user manipulate the box, updating the transformation matrix + accordingly. */ +class SrSnManipulator : public SrSnEditor + { public : + enum Mode { ModeWaiting, ModeTranslating, ModeRotating }; + static const char* class_name; + + private : + Mode _mode; + SrSnBox* _box; + SrSnLines* _dragsel; + SrPnt _firstp; // 1st pt selected (center of the blue cross) + int _corner; + SrPnt _bside[4]; + SrPlane _plane; + SrMat _initmat; + SrQuat _rotation; // local rot after _initmat + SrVec _translation; // local transl after _initmat + float _rx, _ry, _rz; + SrPnt _center; + float _radius; + float _precision_in_pixels; + void (*_user_cb) ( SrSnManipulator*, const SrEvent&, void* ); + void* _user_cb_data; + bool _translationray; + + protected : + /*! Destructor only accessible through unref() */ + virtual ~SrSnManipulator (); + + public : + /*! Constructor */ + SrSnManipulator (); + + /*! Defines the child node */ + void child ( SrSn* sn ); + + /*! Retrieves the child node */ + SrSn* child () const { return SrSnEditor::child(); } + + /*! Get a reference to the manipulator matrix. */ + SrMat& mat () { return SrSnEditor::mat(); } + + /*! Set a initial matrix to the manipulator. This matrix will be saved + and all manipulations performed will be combined to it. + Note: the final transformation is obtained with mat() */ + void initial_mat ( const SrMat& m ); + + /*! Put into waiting mode, ie, no selections appearing */ + void init (); + + /*! To be called to update the manipulation box */ + void update (); + + /*! Set a user callback that is called each time an event is processed + by the manipulator */ + void callback ( void(*cb)(SrSnManipulator*,const SrEvent&,void*), void* udata ) + { _user_cb=cb; _user_cb_data=udata; } + + /*! Returns the associated callback data */ + void* user_cb_data () const { return _user_cb_data; } + + /*! Set the translation DOFs */ + void translation ( const SrVec& t ); + + /*! Returns the translation DOFs */ + SrVec translation () const { return _translation; } + + /*! Handles mouse drag events, and few keys: + qawsed = rotation around xyz axis (shift and alt change the step) + esc: put back the original location + x: switch to/from the "translation ray" mode + p: print the current global matrix of the manipulator */ + virtual int handle_event ( const SrEvent &e ); + + private : + void _transform ( const SrPnt& p, const SrVec& r ); + void _set_drag_selection ( const SrPnt& p ); + }; + +//================================ End of File ================================================= + +# endif // SR_SN_MANIPULATOR_H + + diff --git a/source/dcdt/se/sr_sn_matrix.cpp b/source/dcdt/se/sr_sn_matrix.cpp new file mode 100644 index 0000000000..73f574150e --- /dev/null +++ b/source/dcdt/se/sr_sn_matrix.cpp @@ -0,0 +1,31 @@ +#include "precompiled.h" +# include "sr_sn_matrix.h" + +//# define SR_USE_TRACE1 // Const/Dest +# include "sr_trace.h" + +//======================================= SrSnMatrix ==================================== + +const char* SrSnMatrix::class_name = "Matrix"; + +SrSnMatrix::SrSnMatrix () + :SrSn ( SrSn::TypeMatrix, SrSnMatrix::class_name ) + { + SR_TRACE1 ( "Constructor" ); + } + +SrSnMatrix::SrSnMatrix ( const SrMat& m ) + :SrSn ( SrSn::TypeMatrix, SrSnMatrix::class_name ) + { + SR_TRACE1 ( "Constructor from SrMat" ); + _mat = m; + } + +SrSnMatrix::~SrSnMatrix () + { + SR_TRACE1 ( "Destructor" ); + } + + +//======================================= EOF ==================================== + diff --git a/source/dcdt/se/sr_sn_matrix.h b/source/dcdt/se/sr_sn_matrix.h new file mode 100644 index 0000000000..68634296c4 --- /dev/null +++ b/source/dcdt/se/sr_sn_matrix.h @@ -0,0 +1,48 @@ + +# ifndef SR_SN_MATRIX_H +# define SR_SN_MATRIX_H + +/** \file sr_sn_mat.h + * matrix transformation + */ + +# include "sr_sn.h" + +//======================================= SrSnMatrix ==================================== + +/*! \class SrSnMatrix sr_scene.h + \brief Applies a matrix transformation + + SrSnMatrix accumulates the specified transformation matrix to the + current transformation during traversals of the scene graph when + an action is applied. */ +class SrSnMatrix : public SrSn + { private : + SrMat _mat; + + public : + static const char* class_name; + + protected : + /*! Destructor only accessible through unref() */ + virtual ~SrSnMatrix (); + + public : + /*! Default constructor */ + SrSnMatrix (); + + /*! Constructor receiving a matrix */ + SrSnMatrix ( const SrMat& m ); + + /*! Set the matrix. */ + void set ( const SrMat& m ) { _mat=m; } + + /*! Get the matrix. */ + SrMat& get () { return _mat; } + }; + + +//================================ End of File ================================================= + +# endif // SR_SN_MATRIX_H + diff --git a/source/dcdt/se/sr_sn_polygon_editor.cpp b/source/dcdt/se/sr_sn_polygon_editor.cpp new file mode 100644 index 0000000000..1addd592bc --- /dev/null +++ b/source/dcdt/se/sr_sn_polygon_editor.cpp @@ -0,0 +1,469 @@ +#include "precompiled.h" +# include "sr_sn_polygon_editor.h" + +//============================ SrSnPolygonEditor ==================================== + +const char* SrSnPolygonEditor::class_name = "PolygonEditor"; + +SrSnPolygonEditor::SrSnPolygonEditor () + :SrSnEditor ( SrSnPolygonEditor::class_name ) + { + _polygons = new SrSnSharedPolygons; + child ( _polygons ); + + _creation = new SrSnLines; + _selection = new SrSnLines; + + helpers()->add ( _creation ); + helpers()->add ( _selection ); + + _creation_color = SrColor::red; + _edition_color = SrColor::green; + _selection_color = SrColor::black; + _no_edition_color = SrColor::blue; + + _polygons->color ( _edition_color ); + _creation->color ( _creation_color ); + _selection->color ( _selection_color ); + + _mode = ModeAdd; + _draw_vertices = false; + _precision_in_pixels = 5.0f; + _precision = 0.05f; + _selpol = _selvtx = -1; + _max_polys = -1; + + _solid = true; + _polygons->render_mode ( srRenderModeSmooth ); + } + +SrSnPolygonEditor::~SrSnPolygonEditor () + { + } + +void SrSnPolygonEditor::init () + { + _polygons->shape().init(); + _creation->shape().init(); + _selection->shape().init(); + _selpol = _selvtx = -1; + if ( _mode!=ModeNoEdition && _mode!=ModeOnlyMove ) + _mode = ModeAdd; + } + +void SrSnPolygonEditor::set_polygons_to_share ( SrPolygons* p ) + { + init (); + _polygons->shape ( p ); + } + +void SrSnPolygonEditor::solid ( bool b ) + { + _solid=b; + + if ( _mode==ModeEdit ) + _polygons->render_mode ( _solid? srRenderModeFlat:srRenderModePoints ); + else + _polygons->render_mode ( _solid? srRenderModeSmooth:srRenderModeLines ); + } + +void SrSnPolygonEditor::set_creation_color ( SrColor c ) + { + _creation_color = c; + _creation->color ( _creation_color ); + } + +void SrSnPolygonEditor::set_edition_color ( SrColor c ) + { + _edition_color = c; + _polygons->color ( _edition_color ); + } + +void SrSnPolygonEditor::set_selection_color ( SrColor c ) + { + _selection->changed (true); + _selection_color = c; + } + +void SrSnPolygonEditor::set_no_edition_color ( SrColor c ) + { + _no_edition_color = c; + } + +void SrSnPolygonEditor::set_maximum_number_of_polygons ( int i ) + { + _max_polys = i; + } + +void SrSnPolygonEditor::mode ( SrSnPolygonEditor::Mode m ) + { + SrPolygons& p = _polygons->shape(); + if ( m==ModeEdit && p.size()==0 ) m=ModeAdd; + if ( m==ModeAdd && p.size()==_max_polys ) m=ModeEdit; + + if ( m==ModeEdit ) + _polygons->render_mode ( _solid? srRenderModeFlat:srRenderModePoints ); + else + _polygons->render_mode ( _solid? srRenderModeSmooth:srRenderModeLines ); + + if ( m==ModeNoEdition ) + _polygons->color ( _no_edition_color ); + else + _polygons->color ( _edition_color ); + + // clear edition nodes: + if ( !_creation->const_shape().empty() ) _creation->shape().init(); + if ( !_selection->const_shape().empty() ) _selection->shape().init(); + _selpol = _selvtx = -1; + + // change mode: + _mode = m; + } + +//==================================== private ==================================== + +bool SrSnPolygonEditor::pick_polygon_vertex ( const SrVec2& p ) + { + _selpol = _selvtx = -1; + + bool ret = _polygons->const_shape().pick_vertex ( p, _precision, _selpol, _selvtx ); + if ( ret==false ) return ret; + + //sr_out << "Vertex picked!\n"; + create_vertex_selection ( _selpol, _selvtx ); + + return true; + } + +bool SrSnPolygonEditor::pick_polygon ( const SrVec2& p ) + { + _selvtx = -1; + + _selpol = _polygons->const_shape().pick_polygon ( p ); + if ( _selpol<0 ) return false; + + //sr_out << "Polygon picked!\n"; + create_polygon_selection ( _selpol ); + + return true; + } + +bool SrSnPolygonEditor::subdivide_polygon_edge ( const SrVec2& p ) + { + if ( !_polygons->const_shape().pick_edge ( p, _precision, _selpol, _selvtx ) ) return false; + + //sr_out << "Edge picked!\n"; + SrPolygon& pol = _polygons->shape()[_selpol]; + _selvtx = (_selvtx+1)%pol.size(); + pol.insert ( _selvtx ) = p; + create_vertex_selection ( _selpol, _selvtx ); + + return true; + } + +void SrSnPolygonEditor::create_vertex_selection ( const SrVec2& p ) + { + float r = _precision; + + r*=0.8f; + SrVec2 v(r,r); + SrLines& l = _selection->shape(); + l.init(); + l.push_color(_creation_color); + l.push_line ( p-v, p+v ); v.x*=-1; + l.push_line ( p-v, p+v ); + } + +void SrSnPolygonEditor::create_vertex_selection ( int i, int j ) + { + const SrVec2& p = _polygons->const_shape().const_get(i,j); + float s = _precision; + SrVec2 v ( p.x+s, p.y+s ); + + _selection->color(_selection_color); + SrLines& l = _selection->shape(); + l.init(); + l.begin_polyline(); + l.push_vertex ( v ); + v.y-=2*s; l.push_vertex ( v ); + v.x-=2*s; l.push_vertex ( v ); + v.y+=2*s; l.push_vertex ( v ); + v.x+=2*s; l.push_vertex ( v ); + l.end_polyline(); + } + +void SrSnPolygonEditor::add_polygon_selection ( int i ) + { + SrBox b; + const SrPolygon& p = _polygons->shape().get(i); + + // this is to test the convex hull: +/* SrPolygon hull; + SrPolygon p = _polygons->shape().const_shape(i); + p.convex_hull ( hull ); + p = hull; +*/ + SrLines& l = _selection->shape(); + l.begin_polyline(); + for ( i=0; ishape().init(); + _selection->color(_selection_color); + add_polygon_selection ( i ); + } + +void SrSnPolygonEditor::add_centroid_selection ( int i ) + { + SrVec2 c, p; + c = _polygons->const_shape().const_get(i).centroid(); + + float r = _precision; + r*=1.2f; + p.set(r,r); + _selection->shape().push_line ( c-p, c+p ); p.x*=-1; + _selection->shape().push_line ( c-p, c+p ); + } + +void SrSnPolygonEditor::translate_polygon ( int i, const SrVec2& lp, const SrVec2& p ) + { + SrPolygon& pol = _polygons->shape()[i]; + pol.translate ( p-lp ); + } + +void SrSnPolygonEditor::rotate_polygon ( int i, const SrVec2& lp, const SrVec2& p ) + { + SrPolygon& pol = _polygons->shape()[i]; + SrVec2 cent = pol.centroid(); + + float ang = angle_ori ( lp-cent, p-cent ); + pol.rotate ( cent, ang ); + } + +void SrSnPolygonEditor::remove_selected_polygon () + { + _selection->shape().init (); + if ( _selpol<0 ) return; + _polygons->shape().remove(_selpol); + _selpol=_selvtx=-1; + if ( _polygons->shape().size()==0 ) mode(ModeAdd); + } + +int SrSnPolygonEditor::handle_keyboard ( const SrEvent& e ) + { + //sr_out<<"Key: "<<(int)e.key<=0 ) + { SrPolygon& p = _polygons->shape()[_selpol]; + p.open ( !p.open() ); + _selvtx = -1; + create_polygon_selection ( _selpol ); + return 1; + } + + if ( e.key=='s' ) + { solid ( _solid? false:true ); + return 1; + } + + switch ( _mode ) + { case ModeAdd: + { if ( e.key==SrEvent::KeyDel || e.key==SrEvent::KeyBack ) + { SrLines& creat = _creation->shape(); + int size = _creation->shape().V.size(); + if ( size==0 ) return 0; + creat.V.pop(); size--; + creat.I[1] = size-1; + if ( size==1 ) + create_vertex_selection ( SrVec2( (float*)creat.V.top()) ); + else + _selection->shape().init(); + return 1; + } + else if ( e.key==SrEvent::KeyEsc ) + { int size = _creation->shape().V.size(); + if ( size<3 ) { mode(ModeEdit); return 1; } + SrPolygons& p = _polygons->shape(); + p.push().size ( size ); + for ( int i=0; ishape().V[i]; + mode ( ModeAdd ); + return 1; // tell that I understood the event + } + } break; + + case ModeEdit: + { if ( e.key==SrEvent::KeyEsc ) { mode(ModeAdd); return 1; } + if ( e.key==SrEvent::KeyDel && _selvtx>-1 ) + { SrPolygons& p = _polygons->shape(); + int size = p[_selpol].size(); + bool open = p[_selpol].open(); + if ( size<=2 || (size<=3 && !open) ) + { remove_selected_polygon(); } + else + { p[_selpol].remove(_selvtx); + if (--_selvtx<0) _selvtx=size-2; + create_vertex_selection ( _selpol, _selvtx ); + } + return 1; + } + } break; + + case ModeMove: + { if ( e.key==SrEvent::KeyEsc ) { mode(ModeEdit); return 1; } + if ( e.key==SrEvent::KeyDel && _selpol>-1 ) + { remove_selected_polygon (); + return 1; + } + if ( e.key==SrEvent::KeyEnd && _selpol>-1 && _polygons->const_shape().size()>1 ) + { SrPolygons& p = _polygons->shape(); + for ( int i=_selpol; i-1 && _polygons->const_shape().size()>1 ) + { SrPolygons& p = _polygons->shape(); + for ( int i=_selpol; i>0; i-- ) p.swap(i,i-1); + _selpol = 0; + return 1; + } + } break; + + }; + return 0; + } + +int SrSnPolygonEditor::handle_only_move_event ( const SrEvent& e, const SrVec2& p, const SrVec2& lp ) + { + if ( e.type==SrEvent::Push ) + { if ( pick_polygon_vertex(p) ) + { add_polygon_selection ( _selpol ); + add_centroid_selection ( _selpol ); + return 1; + } + else if ( pick_polygon(p) ) + { return 1; } + } + else if ( e.type==SrEvent::Drag && _selvtx>-1 ) + { rotate_polygon ( _selpol, lp, p ); + create_vertex_selection ( _selpol, _selvtx ); + add_polygon_selection ( _selpol ); + add_centroid_selection ( _selpol ); + return 1; + } + else if ( e.type==SrEvent::Drag && _selpol>-1 ) + { translate_polygon ( _selpol, lp, p ); + create_polygon_selection ( _selpol ); + return 1; + } + return 0; // event not used + } + +int SrSnPolygonEditor::handle_event ( const SrEvent &e ) + { + //sr_out<<"SrSnPolygonEditor event: "<const_shape(); + switch ( _mode ) + { case ModeAdd: + { SrLines& creat = _creation->shape(); + if ( e.type==SrEvent::Push ) + { int size = creat.V.size(); + + if ( polys.size()==_max_polys ) + { mode ( ModeEdit ); } + else if ( size==0 ) + { create_vertex_selection ( p ); + creat.push_vertex(p); // lines are not rendered with only 1 vertex + } + else if ( size==1 ) + { creat.I.size(2); + creat.I[0]=0; + creat.I[1]=1; + creat.push_vertex(p); + _selection->shape().init(); + } + else + { creat.I[1] = size; + creat.push_vertex(p); + } + return 1; // tell that I understood the event + } + } break; + + case ModeEdit: + { if ( e.type==SrEvent::Push ) + { if ( pick_polygon_vertex(p) ) + { } + else if ( subdivide_polygon_edge(p) ) + { } + else if ( pick_polygon(p) ) + { _mode=ModeMove; } + else + { if (_selection->const_shape().V.size()==0 ) break; + _selection->shape().init(); + } + return 1; + } + else if ( e.type==SrEvent::Drag && _selvtx>-1 ) + { _polygons->shape()(_selpol,_selvtx) = p; + create_vertex_selection ( _selpol, _selvtx ); + return 1; + } + } break; + + case ModeMove: // case when a polygon has been selected + { if ( e.type==SrEvent::Push ) + { int pol = _selpol; + if ( pick_polygon_vertex(p) ) + { if ( _selpol!=pol ) + { _mode=ModeEdit; } + else + { add_polygon_selection ( _selpol ); + add_centroid_selection ( _selpol ); + } + } + else if ( pick_polygon(p) ) + { } + else + { mode(ModeEdit); break; } + return 1; + } + else if ( e.type==SrEvent::Drag && _selvtx>-1 ) + { rotate_polygon ( _selpol, lp, p ); + create_vertex_selection ( _selpol, _selvtx ); + add_polygon_selection ( _selpol ); + add_centroid_selection ( _selpol ); + return 1; + } + else if ( e.type==SrEvent::Drag && _selpol>-1 ) + { translate_polygon ( _selpol, lp, p ); + create_polygon_selection ( _selpol ); + return 1; + } + } break; + + }; + + return 0; // event not used + } + +//================================ End of File ================================================= + diff --git a/source/dcdt/se/sr_sn_polygon_editor.h b/source/dcdt/se/sr_sn_polygon_editor.h new file mode 100644 index 0000000000..824dd1cb20 --- /dev/null +++ b/source/dcdt/se/sr_sn_polygon_editor.h @@ -0,0 +1,102 @@ + +# ifndef SR_SN_POLYGON_EDITOR_H +# define SR_SN_POLYGON_EDITOR_H + +/** \file sr_sn_polygon_editor.h + * edits polygons + */ + +# include "sr_sn_shape.h" +# include "sr_sn_editor.h" +# include "sr_lines.h" +# include "sr_polygons.h" + +//==================================== SrSnPolygonEditor ==================================== + +/*! \class SrSnPolygonEditor sr_scene_polygons_editor.h + \brief edit polygons + */ +class SrSnPolygonEditor : public SrSnEditor + { public : + enum Mode { ModeAdd, ModeEdit, ModeMove, ModeOnlyMove, ModeNoEdition }; + static const char* class_name; + + private : + Mode _mode; + bool _draw_vertices; + float _precision; + float _precision_in_pixels; + int _selpol, _selvtx; + int _max_polys; + SrColor _creation_color; + SrColor _edition_color; + SrColor _selection_color; + SrColor _no_edition_color; + SrSnSharedPolygons* _polygons; + SrSnLines* _creation; + SrSnLines* _selection; + SrSnLines* _picking; + bool _solid; + + protected : + /*! Destructor only accessible through unref() */ + virtual ~SrSnPolygonEditor (); + + public : + /*! Constructor */ + SrSnPolygonEditor (); + + void init (); + + void polygons_changed () { _polygons->changed(true); } + + void set_polygons_to_share ( SrPolygons* p ); + + void resolution ( float r ) { _polygons->resolution(r); } + + void show_polygons ( bool b ) { _polygons->visible(b); } + + /*! Determines if should draw the interiorior of the polygon or not */ + void solid ( bool b ); + + /*! precision is in number of pixels (aprox.). Default is 7. */ + void set_picking_precision ( float prec ) { _precision_in_pixels=prec; } + + void set_creation_color ( SrColor c ); + void set_edition_color ( SrColor c ); + void set_selection_color ( SrColor c ); + void set_no_edition_color ( SrColor c ); + + /*! Limits the maximum allowed number of polygons to be created */ + void set_maximum_number_of_polygons ( int i ); + + const SrPolygons& const_shape () const { return _polygons->const_shape(); } + SrPolygons& shape () { return _polygons->shape(); } + void mode ( SrSnPolygonEditor::Mode m ); + int get_selected_polygon () const { return _selpol; } + + private : + bool pick_polygon_vertex ( const SrVec2& p ); + bool pick_polygon ( const SrVec2& p ); + bool subdivide_polygon_edge ( const SrVec2& p ); + + void create_vertex_selection ( const SrVec2& p ); + void create_vertex_selection ( int i, int j ); + + void add_polygon_selection ( int i ); + void create_polygon_selection ( int i ); + void add_centroid_selection ( int i ); + void translate_polygon ( int i, const SrVec2& lp, const SrVec2& p ); + void rotate_polygon ( int i, const SrVec2& lp, const SrVec2& p ); + void remove_selected_polygon (); + + int handle_keyboard ( const SrEvent& e ); + int handle_only_move_event ( const SrEvent& e, const SrVec2& p, const SrVec2& lp ); + int handle_event ( const SrEvent &e ); + }; + +//================================ End of File ================================================= + +# endif // SR_SN_POLYGON_EDITOR_H + + diff --git a/source/dcdt/se/sr_sn_shape.cpp b/source/dcdt/se/sr_sn_shape.cpp new file mode 100644 index 0000000000..1df40a2cd2 --- /dev/null +++ b/source/dcdt/se/sr_sn_shape.cpp @@ -0,0 +1,93 @@ +#include "precompiled.h" +# include "sr_sn_shape.h" + +//# define SR_USE_TRACE1 // Const/Dest +# include "sr_trace.h" + +//=================================== SrSnShapeBase ==================================== + +SrSnShapeBase::SrSnShapeBase ( const char* name ) : SrSn ( SrSn::TypeShape, name ) + { + SR_TRACE5 ( "Constructor" ); + + _renderlib_data = 0; + + _changed = 1; + _render_mode = srRenderModeDefault; + _overriden_render_mode = -1; + _can_override_render_mode = 1; + _resolution = 1.0f; // the meaning depends on the derived node + + _material_is_overriden = 0; + } + +SrSnShapeBase::~SrSnShapeBase () + { + SR_TRACE5 ( "Destructor" ); + delete _renderlib_data; + } + +bool SrSnShapeBase::set_renderlib_data ( SrSnShapeBase::RenderLibData* rdata ) + { + if ( _renderlib_data ) return false; + _renderlib_data = rdata; + return true; + } + +void SrSnShapeBase::render_mode ( srRenderMode m ) + { + if ( _render_mode==m ) return; + _changed = 1; + _render_mode = m; + } + +void SrSnShapeBase::override_render_mode ( srRenderMode m ) + { + if ( !_can_override_render_mode ) return; + if ( _overriden_render_mode<0 ) _overriden_render_mode = _render_mode; + render_mode ( m ); + } + +void SrSnShapeBase::restore_render_mode () + { + if ( _overriden_render_mode>=0 ) + { render_mode ( (srRenderMode)_overriden_render_mode ); + _overriden_render_mode = -1; + } + } + +void SrSnShapeBase::override_material ( SrMaterial m ) + { + if ( !_material_is_overriden ) _overriden_material = _material; + material ( m ); + _material_is_overriden = 1; + } + +void SrSnShapeBase::restore_material () + { + _material_is_overriden = 0; + material ( _overriden_material ); + } + +void SrSnShapeBase::resolution ( float r ) + { + if (_resolution==r) return; + if ( r<0 ) r=0; + _changed=1; + _resolution=r; + } + +void SrSnShapeBase::material ( const SrMaterial& m ) + { + _changed=1; + _material=m; + } + +void SrSnShapeBase::color ( const SrColor& c ) + { + _changed=1; + _material.diffuse=c; + } + +//======================================= EOF ==================================== + diff --git a/source/dcdt/se/sr_sn_shape.h b/source/dcdt/se/sr_sn_shape.h new file mode 100644 index 0000000000..7b59f40009 --- /dev/null +++ b/source/dcdt/se/sr_sn_shape.h @@ -0,0 +1,211 @@ + +# ifndef SR_SN_SHAPE_H +# define SR_SN_SHAPE_H + +/** \file sr_sn_shape.h + * shape base and template class + */ + +# include "sr_sn.h" + +//================================ SrSnShapeBase ==================================== + +enum srRenderMode { srRenderModeDefault, + srRenderModeSmooth, + srRenderModeFlat, + srRenderModeLines, + srRenderModePoints }; + +/*! \class SrSnShapeBase sr_scene.h + \brief general scene shape element + + Base class for scene shape nodes. This class may be derived for + creating shape nodes. A template class SrSnShape is provided + to easily create shape nodes, and is used for all SR buit-in shape + node types by means of typedefs, as for instance, SrSceneModel is + a typedef of SrSnShape (note that, in the example, + sr_model.h must be included before sr_scene.h) */ +class SrSnShapeBase : public SrSn + { public : + class RenderLibData { public : virtual ~RenderLibData() {}; }; + private : + RenderLibData* _renderlib_data; + char _changed; + char _render_mode; + char _overriden_render_mode; + char _can_override_render_mode; + float _resolution; // default is 1 + SrMaterial _material; + SrMaterial _overriden_material; + char _material_is_overriden; + + protected : + + /*! Constructor requires the name of the derived class. */ + SrSnShapeBase ( const char* name ); + + /* Destructor only accessible through unref().*/ + virtual ~SrSnShapeBase (); + + public : + + /*! Associates a user-derived class from RenderLibData to this node. + This function can be only called one time. If it is being called by the + second time, false is returned and nothing is done. + The pointer rdata should be allocated using operator new and SrSnShapeBase + will delete it in its destructor */ + bool set_renderlib_data ( RenderLibData* rdata ); + + /*! Retrieves the data related to the used rendering library */ + RenderLibData* get_renderlib_data () { return _renderlib_data; } + + /*! By default the render mode is srRenderModeDefault, that is specific to the + shape type. For example a SrModel has as default smooth render mode, while + SrLines has line mode as default. */ + srRenderMode render_mode () const { return (srRenderMode)_render_mode; } + void render_mode ( srRenderMode m ); + + /*! Saves the current render mode and then replaces it by m. If + can_override_render_mode() is false, then nothing is done. */ + void override_render_mode ( srRenderMode m ); + + /*! Restore the render mode that was used before overriding it. */ + void restore_render_mode (); + + /*! If false, the render action will not be able to override the render mode + of this shape node. Default is true. */ + void can_override_render_mode ( bool b ) { _can_override_render_mode = (char)b; } + bool can_override_render_mode () { return _can_override_render_mode? true:false; } + + /*! Saves the current material and then replaces it by m. */ + void override_material ( SrMaterial m ); + + /*! Restore the material that was used before overriding it. */ + void restore_material (); + + /*! Returns true if the material is being overrriden, and false otherwise. */ + bool material_is_overriden () { return _material_is_overriden? true:false; } + + /*! The default resolution is 1, a greater value will render more accurate + parametric objects, 0 represents the coarser possible representation. + Values cannot be <0. */ + float resolution () const { return _resolution; } + void resolution ( float r ); + + /*! Sets the material to be used for the shape */ + void material ( const SrMaterial& m ); + const SrMaterial& material () const { return _material; } + + /*! Sets the diffuse color of the shape material. Note that when rendering + shapes without lighting, only the diffuse color is used. */ + void color ( const SrColor& c ); + const SrColor& color () const { return _material.diffuse; } + + /*! Returns true if the node parameters have changed, requiring + the re-generation of the associated render lists. */ + bool haschanged () const { return _changed? true:false; } + + /*! Sets the changed / unchanged state. If true, it will force the + node to regenerate its render lists. */ + void changed ( bool b ) { _changed = (char)b; } + + /*! Calculates the bounding box of the shape. */ + virtual void get_bounding_box ( SrBox &box ) const=0; + }; + +//================================= SrSnShapeTpl ==================================== + +/*! \class SrSnShape sr_scene.h + \brief general scene shape template + + SrSnShape is used for all SR buit-in shape + node types by means of typedefs. For example, SrSnModel is + a typedef of SrSnShape (note that, in the example, + sr_model.h must be included before instantiating SrSnModel) */ +template +class SrSnShape : public SrSnShapeBase + { protected : + X* _data; + + protected : + /*! Constructor from a given pointer to a shape. If the pointer is null, + a new shape will be allocated and used. */ + SrSnShape ( X* pt ) : SrSnShapeBase(X::class_name) + { _data = pt? pt: new X; } + + public : + SrSnShape() : SrSnShapeBase(X::class_name) { _data = new X; } + + /* Virtual Destructor .*/ + virtual ~SrSnShape () { delete _data; } + + /*! Get a const reference to the shape data, without setting the state + of the node as changed. */ + const X& const_shape () const { return *_data; } + + /*! Get a reference to the shape data, and sets the state of the node + as changed, implying that display lists should be regenerated. */ + X& shape () { changed(true); return *_data; } + + /*! Set data using the copy constructor of X. */ + void shape ( const X& data ) { changed(true); *_data=data; } + + /*! Deletes the internal data and starts using the new one pt. + Pointer pt must not be null. Use with care. */ + void replace_shape_pointer ( X* pt ) { changed(true); delete _data; _data=pt; } + + /*! Calculates the bounding box of the shape. */ + virtual void get_bounding_box ( SrBox &box ) const { _data->get_bounding_box(box); } + }; + +/*! \class SrSnSharedShape sr_scene.h + \brief template for shared scene shapes + + SrSnSharedShape is used only for shapes deriving SrSharedClass, + so that a pointer to the shape and ref()/unref() methods are used. + Typedefs are used similarly to SrSnShape */ +template +class SrSnSharedShape : public SrSnShape + { public : + /*! Constructor receives a pointer to a shape. If the pointer is null, + a new shape will be allocated and used. */ + SrSnSharedShape ( X* pt=0 ) : SrSnShape (pt) { _data->ref(); } + + /* Virtual Destructor .*/ + virtual ~SrSnSharedShape () + { _data->unref(); + _data=0; // to cope with base class destructor + } + + /*! Reference a new shape, and unreference the old one. */ + void shape ( X* pt ) { changed(true); _data->unref(); _data=pt; _data->ref(); } + + /*! Get a reference to the shape data, and sets the state of the node + as changed, implying that display lists should be regenerated. */ + X& shape () { changed(true); return *_data; } + }; + +class SrModel; +class SrLines; +class SrPoints; +class SrSphere; +class SrCylinder; +class SrPolygon; +class SrPolygons; + +typedef SrSnShape SrSnBox; +typedef SrSnShape SrSnSphere; +typedef SrSnShape SrSnCylinder; +typedef SrSnShape SrSnLines; +typedef SrSnShape SrSnPoints; +typedef SrSnShape SrSnModel; +typedef SrSnShape SrSnPolygon; +typedef SrSnShape SrSnPolygons; + +typedef SrSnSharedShape SrSnSharedModel; +typedef SrSnSharedShape SrSnSharedPolygons; + +//================================ End of File ================================================= + +# endif // SR_SN_SHAPE_H + diff --git a/source/dcdt/se/sr_sphere.cpp b/source/dcdt/se/sr_sphere.cpp new file mode 100644 index 0000000000..ba50a2dd31 --- /dev/null +++ b/source/dcdt/se/sr_sphere.cpp @@ -0,0 +1,35 @@ +#include "precompiled.h" +# include "sr_sphere.h" +# include "sr_box.h" + +//================================== SrSphere ==================================== + +const char* SrSphere::class_name = "Sphere"; + +SrSphere::SrSphere () : center(SrPnt::null) + { + radius = 1.0f; + } + +SrSphere::SrSphere ( const SrSphere& s ) : center(s.center) + { + radius = s.radius; + } + +void SrSphere::get_bounding_box ( SrBox& box ) const + { + SrVec r ( radius, radius, radius ); + box.set ( center-r, center+r ); + } + +SrOutput& operator<< ( SrOutput& o, const SrSphere& sph ) + { + return o << sph.center << ' ' << sph.radius; + } + +SrInput& operator>> ( SrInput& in, SrSphere& sph ) + { + return in >> sph.center >> sph.radius; + } + +//================================ EOF ================================================= diff --git a/source/dcdt/se/sr_sphere.h b/source/dcdt/se/sr_sphere.h new file mode 100644 index 0000000000..94cfb135fe --- /dev/null +++ b/source/dcdt/se/sr_sphere.h @@ -0,0 +1,46 @@ + +# ifndef SR_SPHERE_H +# define SR_SPHERE_H + +/** \file sr_sphere.h + * a sphere + */ + +# include "sr_vec.h" + +class SrBox; + +/*! \class SrSphere sr_sphere.h + \brief a sphere + + SrSphere represents a sphere based on its center and radius. + By default, the sphere has center (0,0,0) and radius 1*/ +class SrSphere + { public : + SrPnt center; + float radius; + static const char* class_name; //!< constain the static string "Sphere" + + public : + + /*! Constructs as a sphere centered at (0,0,0) with radius 1 */ + SrSphere (); + + /*! Copy constructor */ + SrSphere ( const SrSphere& s ); + + /* Returns the bounding box of all vertices used. The returned box can be empty. */ + void get_bounding_box ( SrBox &b ) const; + + /*! Outputs in format: "center radius". */ + friend SrOutput& operator<< ( SrOutput& o, const SrSphere& sph ); + + /*! Input from format: "center radius". */ + friend SrInput& operator>> ( SrInput& in, SrSphere& sph ); + }; + + +//================================ End of File ================================================= + +# endif // SR_SCENE_SPHERE_H + diff --git a/source/dcdt/se/sr_spline.cpp b/source/dcdt/se/sr_spline.cpp new file mode 100644 index 0000000000..f5b66e8ac1 --- /dev/null +++ b/source/dcdt/se/sr_spline.cpp @@ -0,0 +1,23 @@ +#include "precompiled.h" +//# include +# include "sr_spline.h" + +//============================== SrSpline =============================== + +SrSpline::SrSpline ( const SrSpline& c ) + { + } + +void SrSpline::init ( int d, int k ) + { + _pieces = k-1; + _dim = d; + if ( _pieces<0 ) _pieces=0; + if ( _dim<0 ) _dim=0; + _spline.size ( _pieces*(_dim*3) + _dim ); + _spline.setall ( 0.0f ); + } + + +//============================== end of file =============================== + diff --git a/source/dcdt/se/sr_spline.h b/source/dcdt/se/sr_spline.h new file mode 100644 index 0000000000..9032e3af9e --- /dev/null +++ b/source/dcdt/se/sr_spline.h @@ -0,0 +1,67 @@ + +/** \file sr_spline.h + * piecewise spline */ + +# ifndef SR_SPLINE_H +# define SR_SPLINE_H + +# include +# include "sr_array.h" + +/*! \class SrSpline sr_spline.h + \brief A N-dimensional piecewise cubic spline + + Implements a N-dimensional piecewise cubic spline + + Given control points x0 x1 x2 x3, "their cubic spline" + S(t) = at3 + bt2 + ct + x0 has coefficients + c=3(x1-x0), b=3(x2-x1)-c, a=x3-x0-c-b. + + This class is not yet implemented! + */ +class SrSpline + { private : + int _dim; // the dimension of each point + int _pieces; // the number of cubic pieces + SrArray _spline; // each (3*_dim) position starts one cubic spline + + private : + + public : + + /*! The constructor initializes a piecewise spline with + given dimension and number of knots. */ + SrSpline ( int d=0, int k=0 ) { init(d,k); } + + /*! Copy constructor. */ + SrSpline ( const SrSpline& c ); + + /*! Initializes spline in given dimension and number of knots. + The start and end points are considered to be knots, thus a meaningfull + number of knots will be >= 2. */ + void init ( int d, int k ); + + /*! Returns the number of knots (which include endpoints) */ + int knots () const { return _pieces+1; } + + /*! Returns a pointer to the n-dimensional coordinates of given knot + Parameter k must obey 0<=k> ( SrInput& in, SrSpline& c ); + }; + +//============================== end of file =============================== + +# endif // SR_SPLINE_H + + diff --git a/source/dcdt/se/sr_string.cpp b/source/dcdt/se/sr_string.cpp new file mode 100644 index 0000000000..5882099576 --- /dev/null +++ b/source/dcdt/se/sr_string.cpp @@ -0,0 +1,663 @@ +#include "precompiled.h" +# include +# include +# include +# include +# include + +# include "sr_string.h" +# include "sr_input.h" + +//# define SR_USE_TRACE1 // constructors / destructors +//# define SR_USE_TRACE2 // member functions +//# define SR_USE_TRACE3 // set() member function +# include "sr_trace.h" + +char *SrString::_empty = ""; + +//============================= SrString ========================================== + +SrString::SrString () : _capacity(0), _data(SrString::_empty) + { + SR_TRACE1 ( "Default Constructor" ); + } + +SrString::SrString ( char c, int len ) : _capacity(0), _data(SrString::_empty) + { + if ( len<=0 ) return; + _capacity = len+1; + _data = new char[_capacity]; + memset ( _data, c, len ); + _data[len] = 0; + SR_TRACE1 ( "Char Constructor: "<=xi && isspace(_data[xf]) ) xf--; + } + +void SrString::substring ( int inf, int sup ) + { + int n=0; + int max=strlen(_data)-1; + + if ( max<0 ) return; + if ( inf<0 ) inf=0; + if ( sup<0 || sup>max ) sup=max; + + while (inf<=sup) _data[n++] = _data[inf++]; + _data[n] = 0; + } + +char SrString::last_char () const + { + int len = strlen(_data); + if ( len==0 ) return 0; + return _data[len-1]; + } + +void SrString::last_char ( char c ) + { + int len = strlen(_data); + if ( len==0 ) return; + _data[len-1] = c; + } + +void SrString::get_substring ( SrString& s, int inf, int sup ) const + { + int max=strlen(_data)-1; + if ( max<0 ) return; + if ( sup<0 || sup>max ) sup=max; + if ( inf<0 ) inf=0; + if ( inf>sup ) return; + s.len ( sup-inf+1 ); + int i=0; + while ( inf<=sup ) s[i++]=_data[inf++]; + s[i]=0; + } + +int SrString::get_next_string ( SrString& s, int i ) const + { + int i1, i2; + int max=strlen(_data)-1; + + if ( max<0 ) return -1; + if ( i<0 ) i=0; + + while ( i<=max && isspace(_data[i]) ) i++; // cut leading spaces + i1 = i; + while ( i<=max && !isspace(_data[i]) ) i++; // advance untill finding other white characters + i2 = i; + + if ( i2>i1 ) + { get_substring ( s, i1, i2-1 ); + return i2; + } + else + { return -1; + } + } + +void SrString::lower () + { + char *pt = _data; + while ( *pt ) { *pt=(char)SR_LOWER(*pt); pt++; } + } + +void SrString::upper () + { + char *pt = _data; + while ( *pt ) { *pt=(char)SR_UPPER(*pt); pt++; } + } + +int SrString::search ( char c ) const + { + int len = strlen(_data); + for ( int i=0; i=0; i-- ) + { if ( _data[i]=='/' || _data[i]=='\\' ) + { _data[i+1]=0; + break; + } + } + + return i; + } + +int SrString::remove_path () + { + int i; + + for ( i=len()-1; i>=0; i-- ) + { if ( _data[i]=='/' || _data[i]=='\\' ) + { substring(i+1,-1); + break; + } + } + + return i; + } + +int SrString::get_file_name ( SrString& filename ) const + { + filename.set ( _data ); + return filename.remove_path (); + } + +int SrString::extract_file_name ( SrString& filename ) + { + int i = get_file_name ( filename ); + if ( i>=0 ) _data[i+1]=0; + return i; + } + +int SrString::remove_file_extension () + { + int i; + + for ( i=len()-1; i>=0; i-- ) + { if ( _data[i]=='.' ) _data[i]=0; + if ( _data[i]=='/' || _data[i]=='\\' ) return -1; // no extension + } + + return i; + } + +int SrString::get_file_extension ( SrString& ext ) const + { + int i; + + ext.set ( _data ); + + for ( i=len()-1; i>=0; i-- ) + { if ( _data[i]=='.' ) + { ext.substring(i+1,-1); + break; + } + if ( _data[i]=='/' || _data[i]=='\\' ) return -1; // no extension + } + + return i; + } + +int SrString::extract_file_extension ( SrString& ext ) + { + int i = get_file_extension ( ext ); + if ( i>=0 ) _data[i]=0; + return i; + } + +bool SrString::has_file_extension ( const char* ext ) const + { + int i; + + for ( i=len()-1; i>=0; i-- ) + { if ( _data[i]=='.' ) + { if ( !ext ) return true; + return sr_compare_cs ( _data+i+1, ext )==0? true:false; + } + if ( _data[i]=='/' || _data[i]=='\\' ) break; // early termination + } + + return false; + } + +bool SrString::make_valid_path () + { + replace_all ( "\\", "/" ); + + if ( _data[0]==0 ) return false; + + if ( last_char()!='/' ) append ( "/" ); + + return true; + } + +bool SrString::make_valid_path ( const char* s ) + { + if ( !s ) return false; + set ( s ); + return make_valid_path(); + } + +bool SrString::has_absolute_path ( const char* s ) // static function + { + if ( !s ) return false; + if ( !s[0] ) return false; + if ( s[0]=='\\' || s[0]=='/' ) return true; // /dir, \\machine\dir + if ( !s[1] ) return false; // eg a 1-letter filename + if ( s[1]==':' ) return true; // c:dir is considered absolute + return false; + } + +bool SrString::make_valid_string ( const char* s ) + { + bool need_quotes = false; + bool need_slash = false; + + int i; + int len = strlen ( s ); + + if ( len==0 ) + need_quotes = true; + else + { for ( i=0; i=len ) { append(st); return; } + + int dp = strlen ( st ); + if ( !dp ) return; + + int ns = len+dp+1; + + // at this point _data!=_empty, because of the i>=len test. + + if ( ns>_capacity ) + { _capacity = ns*2; + _data = sr_string_realloc ( _data, _capacity ); + } + + // memmove deals with overlapped regions + memmove ( _data+i+dp, _data+i, sizeof(char)*(len-i+1) ); // copies also the ending 0 + + // now we fill the created space : + memcpy ( _data+i, st, sizeof(char)*dp ); + } + +void SrString::remove ( int i, int dp ) + { + if ( _data==_empty || dp<=0 ) return; + int len = strlen(_data); + if ( i>=len || i<0 ) return; + if ( i+dp>=len ) { _data[i]=0; return; } + + // memmove deals with overlapped regions : + memmove ( _data+i, _data+i+dp, sizeof(char)*(len-i-dp+1) ); // copies also the ending 0 + } + +int SrString::replace ( const char* oldst, const char* newst, bool ci ) + { + int i = search ( oldst, ci ); + // printf("%d\n",i); + + if ( i<0 ) return i; // not found + + int oldlen = strlen(oldst); + int newlen = newst? strlen(newst):0; + + if ( oldlennewlen ) // remove space + { remove ( i, oldlen-newlen ); + } + + // void *memmove( void *dest, const void *src, size_t count ); + // The memmove function copies count bytes of characters from src to dest. + // If some regions of the source area and the destination overlap, + // memmove ensures that the original source bytes in the overlapping + // region are copied before being overwritten. + if ( newlen>1 ) memmove ( _data+i, newst, sizeof(char)*newlen ); + else if ( newlen==1 ) _data[i]=newst[0]; + + return i; + } + +int SrString::replace_all ( const char* oldst, const char* newst, bool ci ) + { + int count=0; + while ( replace(oldst,newst,ci)!=-1 ) count++; + return count; + } + +void SrString::take_data ( SrString &s ) + { + if ( _data!=_empty ) delete[] ( _data ); + + _data = s._data; + _capacity = s._capacity; + s._data = _empty; + s._capacity = 0; + + SR_TRACE2 ( "take_data!" ); + } + +char* SrString::leave_data ( char*& s ) + { + s = _data; + if ( s==_empty ) { s=new char[1]; s[0]=0; } + + _data = _empty; + _capacity = 0; + + SR_TRACE2 ( "leave_data!" ); + return s; + } + +SrString& SrString::operator << ( const char* st ) + { + append(st); + return *this; + } + +SrString& SrString::operator << ( int i ) + { + char buf[64]; + sprintf ( buf, "%d", i ); + append(buf); + return *this; + } + +SrString& SrString::operator << ( float f ) + { + char buf[64]; + sprintf ( buf, "%f", f ); + append(buf); + return *this; + } + +SrString& SrString::operator << ( double d ) + { + char buf[64]; + sprintf ( buf, "%f", d ); + append(buf); + return *this; + } + +SrString& SrString::operator << ( char c ) + { + char buf[2]; + buf[0]=c; buf[1]=0; + append(buf); + return *this; + } + +SrString& SrString::operator = ( const SrString &s ) + { + set(s._data); + return *this; + } + +SrString& SrString::operator = ( const char* st ) + { + set(st); + return *this; + } + +SrString& SrString::operator = ( int i ) + { + set(""); + return *this<=l+1), calling the method capacity(l+1) if needed. Then + a \0 character is placed at position l. */ + void len ( int l ); + + /*! Returns the current capacity of the internal buffer being used. */ + int capacity () const; + + /*! Sets the desired capacity for the internal buffer. If c<=0, the + internal string is deallocated if needed and then it is pointed + to _empty. Otherwise, the internal buffer size is adjusted with + realloc(c). */ + void capacity ( int c ); + + /*! Frees any not used space in the internal buffer. If SrString is empty, + nothing is done. If the internal buffer points to an empty string + different than SrString::_empty, it is freed and MgString becomes empty. + Otherwise, the internal buffer uses realloc() to achieve the optimal + memory case where capacity()==len()+1. */ + void compress (); + + /*! Take out leading and ending spaces of SrString. No reallocation is + done, so that SrString keeps its capacity(), regardless of its len(). */ + void trim (); + + /*! Take out the leading spaces of SrString. No reallocation is done, so + that SrString keeps its capacity(), regardless of its len(). */ + void ltrim (); + + /*! Take out ending spaces of SrString. No reallocation is used, so that + SrString keeps its capacity(), regardless of its len(). */ + void rtrim (); + + /*! Puts in xi and xf the indices of the first and last non-white + characters. The first argument will contain the position of the first + non-white char of the string, or -1 if the string is empty. The second + argument will contain the position of the last non-white char, or -1 + if the string is empty. If SrString contains only white-space + characters, we will have xi>xf, where xi==len() and xf==len()-1. The + C function isspace() is used to determine if a character is a white-space + char or not. */ + void bounds ( int &xi, int &xf ) const; + + /*! Makes SrString to becomes its substring enclosed in the coordinates + [inf,sup]. If sup<0, sup is considerd to be len()-1, ie, the maximum + valid coordinate. If sup is greater than this maximum, sup becomes + this maximum value. If inf<0, inf is considered to be 0. And if + inf>sup, SrString becomes empty; already taking into account all + checkings for the sup parameter. If SrString is empty, nothing is done. + In any case, no reallocation is used, so that SrString keeps its + same original capacity(). */ + void substring ( int inf, int sup ); + + /*! Returns the last char in the string, or 0 if string is empty. */ + char last_char () const; + + /*! Replaces the last char in the string by c if the string is not empty */ + void last_char ( char c ); + + /*! Put in s the substring defined by positions inf and sup. + If inf>sup or s is empty nothing is done and the method returns. + If sup is out of range it is set to the end of the string, and + if inf<0 inf is set to 0. */ + void get_substring ( SrString& s, int inf, int sup ) const; + + /*! Put in s the next sequence of characters (a string) found in SrString. + The search starts at the position i. In case a string is not found, + -1 is returned, otherwise the position just after the end of the + found string is returned. In this way it is possible to parse efficiently + all names separated by white spaces. */ + int get_next_string ( SrString& s, int i ) const; + + /*! Changes each character to lower case. */ + void lower (); + + /*! Changes each character to upper case. */ + void upper (); + + /*! Returns the index of the first occurrence of c, or -1 if not found. */ + int search ( char c ) const; + + /*! Returns the index of the first occurrence of str, or -1 if not found, + or if any of the strings are empty or if the length of st is greater + than that of SrString. + If ci is true (the default), the used compare function is case-insensitive. */ + int search ( const char* st, bool ci=true ) const; + + /*! Keeps only the path of a filename. The cut point is determined by + the last slash character (/ or \) found, which stays in the string. + Returns the cut point, which can be -1 if no slash is found. */ + int remove_file_name (); + + /*! Removes the path of a filename. The cut point (returned) is determined + by the last slash character (/ or \) found. -1 is returned if + no slash is found. */ + int remove_path (); + + /*! Search for a file name at the end of the string and put it in filename. + The cut point is determined by the last slash character present in the + string and its position is returned. */ + int get_file_name ( SrString& filename ) const; + + /*! Same as get filename, but SrString is modified to not contain the + file name. The slash character stays in the string (not in filename). + -1 is returned if not found, and in this case nothing is changed. */ + int extract_file_name ( SrString& filename ); + + /*! Search for an extension at the end of the string and remove it. + The cut point is the last point character found, and its position + is returned (or -1 in case of no extension found). */ + int remove_file_extension (); + + /*! Search for an extension at the end of the string and put it in ext. + The cut point is determined by the last point character present in + the string and its position is returned (or -1 in case of failure). + The returned extension does not contain the point character. */ + int get_file_extension ( SrString& ext ) const; + + /*! Search for an extension at the end of the string, put it in ext, and + erase it from the string. The cut point is the last point character + found, its position is returned (or -1 in case of failure). + The point character will be no more present in the string and neither + in ext. -1 is returned if not found, and in this case nothing is + changed. */ + int extract_file_extension ( SrString& ext ); + + /*! Checks if ".ext" exists at the end of the string (case sensitive). + If ext is a null pointer, only checks if a '.' exists. + Note: 'ext' is not supposed to contain the '.' character. */ + bool has_file_extension ( const char* ext ) const; + + /*! Ensures a slash exists at the end, and replaces all '\' with '/'. + Returns true if a non empty path (>=2 chars) is found and false otherwise. */ + bool make_valid_path (); + + /*! Copies s (s can be a type-casted SrString), ensures a slash exists at + the end, and replaces all '\' with '/'. Returns true if a non empty path + (>=2 chars) is found and false otherwise (false is returned if s is null). */ + bool make_valid_path ( const char* s ); + + /*! Returns true if the string is an absolute path and false otherwise. + It is considered an absolute path if it starts with a slash character, + or if it starts with a drive letter in the format "X:" */ + static bool has_absolute_path ( const char* s ); + + /*! Member version of the has_absolute_path() static method */ + bool has_absolute_path () const { return has_absolute_path(_data); } + + /*! Copy s to SrString, adding to any found double quotes a back slash. If + s is empty, or double quotes or any delimiters are found, the whole + string is enclosed with double quotes and true is returned. This ensures + a valid string name for I/O. If no modifications are required false is + returned and s is simply copied. + Example: [My "book" is ] will become ["My \"book\" is "] */ + bool make_valid_string ( const char* s ); + + /*! Converts to an integer, same as the C library function */ + int atoi () const; + + /*! Converts to a float, same as the C library function. */ + float atof () const; + + /*! Converts to a double, using the C library atof function. */ + double atod () const; + + /*! Type cast operator to access the internal string buffer. + The internal buffer is always a non-null pointer, but it can point + to an empty string, ie, SrString::_empty. Implemented inline. */ + operator const char* () const { return _data; } + + /*! Accesses the character/element number i of the string. + SrString is indexed as the standard c string, so that valid indices + are in the range [0,len()-1]. There are no checkings if the index is + inside the range. Implemented inline. */ + char& operator[] ( int i ) { return _data[i]; } + + /*! Gives a const access of the element number i of the string. SrString + is indexed as the standard c string, so that valid indices are in the + range [0,len()-1]. There are no checkings if the index is inside the + range. Implemented inline. */ + const char& get ( int i ) const { return _data[i]; } + + /*! Appends st to the string. If more space is required, the string capacity + is increased by 2 times the required capacity in order to speed up many + consecutive string concatenations. Use compress() for reducing the used + memory later if required. */ + void append ( const char* st ); + + /*! Inserts st in the middle of SrString, starting at position i. + If i<0, 0 is considered. If reallocation is required, the string capacity + is increased by 2 times the required capacity. If i>=len(), append() is + called. If st==0, or st=="", nothing is done. */ + void insert ( int i, const char* st ); + + /*! Removes dp characters of the string, starting at position i. + If SrString is empty, or the indices are out of range, nothing is done. + If the space to remove goes outside the string, the space is clipped. */ + void remove ( int i, int dp ); + + /*! Replaces the first occurence of oldst with newst, + and returns the position where newst was inserted. + If oldst was not found, -1 is returned and nothing is done. + oldst and newst can overlap. + If ci is true (the default), the used compare function is case-insensitive. */ + int replace ( const char* oldst, const char* newst, bool ci=true ); + + /*! Calls the method replace until -1 is returned. + Returns the number of replacements done. */ + int replace_all ( const char* oldst, const char* newst, bool ci=true ); + + /*! Makes SrString be exactly the same object as b, and then makes b an + empty string. This is done without reallocation, and is used to speed + up many algorithms. */ + void take_data ( SrString &s ); + + /*! Makes SrString be empty, and assigns the internal buffer to s. Returns + the string s, that will never be 0, and that will never point to the + internal SrString::_empty. The returned pointer will need to be deleted + (with delete) by the user somewhere after. */ + char* leave_data ( char*& s ); + + /*! Appends st to the SrString by calling append(). */ + SrString& operator << ( const char* st ); + SrString& operator << ( int i ); + SrString& operator << ( float f ); + SrString& operator << ( double d ); + SrString& operator << ( char c ); + + /*! Assignment operator by calling set(). */ + SrString& operator = ( const SrString &s ); + + /*! Copies st into SrString by calling set(). */ + SrString& operator = ( const char* st ); + SrString& operator = ( int i ); + SrString& operator = ( char c ); + SrString& operator = ( float f ); + SrString& operator = ( double d ); + + /*! Compares strings using sr_compare(const char*,const char*), that does + a case-insensitive comparison. Returns 0 if they are equal, <0 if s10 if s1>s2. */ + friend inline int sr_compare ( const SrString* s1, const SrString* s2 ) { return ::sr_compare(s1->_data,s2->_data); } + + // Notice: The compiler requires one class type argument for operators. + + /*! Case-insensitive comparison operator that returns a boolean value. */ + friend inline bool operator == ( const SrString& s1, const char* s2 ) { return sr_compare(s1._data,s2)==0? true:false; } + friend inline bool operator == ( const char* s1, const SrString& s2 ) { return sr_compare(s1,s2._data)==0? true:false; } + friend inline bool operator == ( const SrString& s1, const SrString& s2 ) { return sr_compare(s1._data,s2._data)==0? true:false; } + + /*! Case-insensitive comparison operator that returns a boolean value. */ + friend inline bool operator != ( const SrString& s1, const char* s2 ) { return sr_compare(s1._data,s2)!=0? true:false; } + friend inline bool operator != ( const char* s1, const SrString& s2 ) { return sr_compare(s1,s2._data)!=0? true:false; } + friend inline bool operator != ( const SrString& s1, const SrString& s2 ) { return sr_compare(s1._data,s2._data)!=0? true:false; } + + /*! Case-insensitive comparison operator that returns a boolean value. */ + friend inline bool operator < ( const SrString& s1, const char* s2 ) { return sr_compare(s1._data,s2)<0? true:false; } + friend inline bool operator < ( const char* s1, const SrString& s2 ) { return sr_compare(s1,s2._data)<0? true:false; } + friend inline bool operator < ( const SrString& s1, const SrString& s2 ) { return sr_compare(s1._data,s2._data)<0? true:false; } + + /*! Case-insensitive comparison operator that returns a boolean value. */ + friend inline bool operator <= ( const SrString& s1, const char* s2 ) { return sr_compare(s1._data,s2)<=0? true:false; } + friend inline bool operator <= ( const char* s1, const SrString& s2 ) { return sr_compare(s1,s2._data)<=0? true:false; } + friend inline bool operator <= ( const SrString& s1, const SrString& s2 ) { return sr_compare(s1._data,s2._data)<=0? true:false; } + + /*! Case-insensitive comparison operator that returns a boolean value. */ + friend inline bool operator > ( const SrString& s1, const char* s2 ) { return sr_compare(s1._data,s2)>0? true:false; } + friend inline bool operator > ( const char* s1, const SrString& s2 ) { return sr_compare(s1,s2._data)>0? true:false; } + friend inline bool operator > ( const SrString& s1, const SrString& s2 ) { return sr_compare(s1._data,s2._data)>0? true:false; } + + /*! Case-insensitive comparison operator that returns a boolean value. */ + friend inline bool operator >= ( const SrString& s1, const char* s2 ) { return sr_compare(s1._data,s2)>=0? true:false; } + friend inline bool operator >= ( const char* s1, const SrString& s2 ) { return sr_compare(s1,s2._data)>=0? true:false; } + friend inline bool operator >= ( const SrString& s1, const SrString& s2 ) { return sr_compare(s1._data,s2._data)>=0? true:false; } + }; + +//============================== end of file =============================== + +# endif // SR_STRING_H diff --git a/source/dcdt/se/sr_string_array.cpp b/source/dcdt/se/sr_string_array.cpp new file mode 100644 index 0000000000..e5ecab102d --- /dev/null +++ b/source/dcdt/se/sr_string_array.cpp @@ -0,0 +1,245 @@ +#include "precompiled.h" +# include "sr_string_array.h" + +//# define SR_USE_TRACE1 +# include "sr_trace.h" + +//====================== SrStringArray ========================== + +# define SETZERO(i,ini,size) for(i=ini;i::set(i,0) +# define DELETE(i,ini,size) for(i=ini;i::get(i) + +SrStringArray::SrStringArray ( int s, int c ) + :SrArray(s,c) + { + int i; + SETZERO(i,0,s); + } + +SrStringArray::SrStringArray ( const SrStringArray& a ) + :SrArray(0,0) + { + *this = a; // calls copy operator + } + +SrStringArray::~SrStringArray () + { + size(0); + } + +void SrStringArray::size ( int ns ) + { + int i, s = size(); + if ( ns>s ) + { SrArray::size(ns); + SETZERO(i,s,ns); + } + else if ( ns::size(ns); + } + } + +void SrStringArray::capacity ( int nc ) + { + int i, s = size(); + if ( nc::capacity(nc); + } + +void SrStringArray::compress () + { + capacity ( size() ); + } + +void SrStringArray::setall ( const char* s ) + { + int i; + for ( i=0; i=0 && i::get(i); + SrArray::set ( i, sr_string_new(s) ); + } + +const char* SrStringArray::get ( int i ) const + { + SR_ASSERT ( i>=0 && i::get ( i ); + return st? st:""; + } + +const char* SrStringArray::top () const + { + if ( size()==0 ) return 0; + return SrArray::get ( size()-1 ); + } + +void SrStringArray::pop () + { + if ( size()>0 ) delete SrArray::pop(); + } + +void SrStringArray::push ( const char* s ) + { + SrArray::push() = sr_string_new(s); + } + +void SrStringArray::insert ( int i, const char* s, int dp ) + { + SrArray::insert ( i, dp ); + int j; + for ( j=0; j::get(i+j) = sr_string_new(s); + } + +void SrStringArray::remove ( int i, int dp ) + { + int j; + for ( j=0; j::get(i+j); + SrArray::remove ( i, dp ); + } + +void SrStringArray::operator = ( const SrStringArray& a ) + { + size ( 0 ); // deletes all data + SrArray::size ( a.size() ); + SrArray::compress(); + int i; + for ( i=0; i::set ( i, sr_string_new(a[i]) ); + } + +static int fcmpst ( const void* pt1, const void* pt2 ) + { + typedef const char* cchar; + return sr_compare( *((cchar*)pt1), (cchar)pt2 ); + } + +static int fcmppt ( const void* pt1, const void* pt2 ) + { + typedef const char* cchar; + return sr_compare( *((cchar*)pt1), *((cchar*)pt2) ); + } + +int SrStringArray::insort ( const char* s, bool allowdup ) + { + int pos; + pos = SrArrayBase::insort ( sizeof(char*), s, fcmpst, allowdup ); + if ( pos>=0 ) SrArray::get(pos) = sr_string_new(s); + return pos; + } + +void SrStringArray::sort () + { + SrArrayBase::sort ( sizeof(char*), fcmppt ); + } + +int SrStringArray::lsearch ( const char* s ) const + { + return SrArrayBase::lsearch ( sizeof(char*), s, fcmpst ); + } + +int SrStringArray::bsearch ( const char* s, int *pos ) + { + return SrArrayBase::bsearch ( sizeof(char*), s, fcmpst, pos ); + } + +int SrStringArray::push_path ( const char* path ) + { + // validate path: + SrString spath; + if ( !spath.make_valid_path(path) ) return -1; + + // check if already there: + int i; + for ( i=0; i::take_data(a); + } + +SrOutput& operator<< ( SrOutput& o, const SrStringArray& a ) + { + o << '['; + for ( int i=0; i> ( SrInput& in, SrStringArray& a ) + { + a.size(0); + in.get_token(); + while (true) + { in.get_token(); + if ( in.last_token()[0]==']' ) break; + a.push ( in.last_token() ); + } + return in; + } + +//=========================== EOF =============================== diff --git a/source/dcdt/se/sr_string_array.h b/source/dcdt/se/sr_string_array.h new file mode 100644 index 0000000000..f2ab84e2f8 --- /dev/null +++ b/source/dcdt/se/sr_string_array.h @@ -0,0 +1,133 @@ + +# ifndef SR_STRING_ARRAY_H +# define SR_STRING_ARRAY_H + +/** \file sr_string_array.h + * resizeable array of strings */ + +# include "sr_array.h" + +/*! \class SrStringArray sr_string_array.h + \brief resizeable array of strings + + SrStringArray implements methods for managing a resizeable array + of strings. It derives SrArray, rewriting all methods to correctly + manage the allocation and deallocation of the strings. */ +class SrStringArray : private SrArray + { public : + /*! Default constructor */ + SrStringArray ( int s=0, int c=0 ); + + /*! Copy constructor */ + SrStringArray ( const SrStringArray& a ); + + /*! Destructor */ + ~SrStringArray (); + + /*! Returns true if the array has no elements, and false otherwise. */ + bool empty () const { return SrArray::empty(); } + + /*! Returns the capacity of the array. */ + int capacity () const { return SrArray::capacity(); } + + /*! Returns the current size of the array. */ + int size () const { return SrArray::size(); } + + /*! Changes the size of the array. */ + void size ( int ns ); + + /*! Changes the capacity of the array. */ + void capacity ( int nc ); + + /*! Makes capacity to be equal to size. */ + void compress (); + + /*! Returns a valid index as if the given index references a circular + array, ie, it returns index%size() for positive numbers. Negative + numbers are also correctly mapped. */ + int validate ( int index ) const { return SrArray::validate(index); } + + /*! Sets all elements to s */ + void setall ( const char* s ); + + /*! Sets element i to become a copy of s. Index i must be a valid entry. */ + void set ( int i, const char* s ); + + /*! Gets a const pointer to the string index i. If that string was not + defined it returns a pointer to a static empty string "", so that + always a valid string is returned. */ + const char* get ( int i ) const; + + /*! Operator version of get() */ + const char* operator[] ( int i ) const { return get(i); } + + /*! Returns a const pointer to the last element or 0 if the array is empty*/ + const char* top () const; + + /*! Pop and frees element size-1 if the array is not empty */ + void pop (); + + /*! Appends one element */ + void push ( const char* s ); + + /*! Inserts dp positions, starting at pos i, and seting string s in + each new position created. */ + void insert ( int i, const char* s, int dp=1 ); + + /*! Removes dp positions starting from pos i */ + void remove ( int i, int dp=1 ); + + /*! Copy operator */ + void operator = ( const SrStringArray& a ); + + /*! Inserts one string, considering the array is sorted. Returns the inserted position, + or -1 if duplication occurs and allowdup is false. */ + int insort ( const char* s, bool allowdup=true ); + + /*! Sort array */ + void sort (); + + /*! Linear search */ + int lsearch ( const char* s ) const; + + /*! Binary search for sorted arrays. Returns index of the element found, + or -1 if not found. If not found and pos is not 0, pos will have the + position to insert the element keeping the array sorted. */ + int bsearch ( const char* s, int* pos=0 ); + + /*! Validate path to be a valid path and push it to the string array. + If the path is not valid (eg null), or is already in the array, nothing + is done and -1 is returned. Case sensitive comparison is used. + In case of success the position of the added path is returned. */ + int push_path ( const char* path ); + + /*! Considers that the string array contains a list of paths and tries to locate + and open a file by searching in these paths. + First, if the given filename has an absolute path in it, the method simply + tries to open it and returns right after. Otherwise: + - First search in the paths stored in the string array. + If basedir is given (not null), relative paths in the string array + are made relative (concatenated) to basedir. + - Second, try to open filename using only the basedir path (if given). + - Finally tries to simply open filename. + Path basedir, if given, has to be a valid path name (with a slash in the end). + Returns true if the file could be open. In such case, the successfull full file + name can be found in inp.filename(). False is returned in case of failure. + Parameter mode is the fopen() mode: "rt", etc. */ + bool open_file ( SrInput& inp, const char* filename, const char* mode, const char* basedir ); + + /*! Frees the data of SrStringArray, and then makes SrStringArray be the + given array a. After this, a is set to be a valid empty array. The data + is moved without reallocation. */ + void take_data ( SrStringArray& a ); + + /*! Outputs all elements of the array in format ["e0" "e1" ... "en"]. */ + friend SrOutput& operator<< ( SrOutput& o, const SrStringArray& a ); + + /*! Inputs elements in format ["e0" "e1" ... "en"]. */ + friend SrInput& operator>> ( SrInput& in, SrStringArray& a ); + }; + +#endif // SR_STRING_ARRAY_H + +//============================== end of file =============================== diff --git a/source/dcdt/se/sr_swing_control.h b/source/dcdt/se/sr_swing_control.h new file mode 100644 index 0000000000..710ac6c51e --- /dev/null +++ b/source/dcdt/se/sr_swing_control.h @@ -0,0 +1,60 @@ + +# include +# include + +# include + +//============================ SrSwingControl ============================ + +/*! Controls a 2D panel allowing to specify a swing rotation. + The 2D panel uses the axis-angle parameterization of the + swing, and ellipse-like joint limits are supported */ +class SrSwingControl : public Fl_Widget + { private : + float _sx, _sy; // the axis angle representation of the swing + float _ex, _ey; // ellipse limits (default PI) + bool _swap; + int _crossr; + SrArray _points; + + public : + + /*! Construtor as required by fltk */ + SrSwingControl ( int x, int y, int w, int h, const char *l=0 ); + + /*! Swap mode (default is true) puts the X (red) axis vertical and + the Y (green) axis horizontal, better to visualize swing rotations */ + void swap_axis ( bool b ) { _swap=b; } + + /*! Set radius of the cross marking the current value (default is 3)*/ + void cross_radius ( int r ) { _crossr=r; } + + /*! Set new value, ensuring that ellipse limits are respected. + True is returned if a new value is accepted */ + bool value ( float x, float y ); + + /*! Returns the x value as float */ + float xval() const { return _sx; } + + /*! Returns the x value as float */ + float yval() const { return _sy; } + + /*! Define the X and Y radius of the ellipse limiting the swing. + The default value is (pi,pi). Values are ensured to be <=pi */ + void ellipse ( float ex, float ey ); + + /*! Returns the limiting ellipse x radius */ + float ellipsex () const { return _swap? _ey:_ex; } + + /*! Returns the limiting ellipse y radius */ + float ellipsey () const { return _swap? _ex:_ey; } + + /*! Send segments (as a point list) to be drawn in the swing pannel */ + void draw_segments ( const SrArray& pl ) { _points=pl; } + + protected : + virtual void draw (); + virtual int handle ( int e ); + }; + +//================================ End of File ================================================= diff --git a/source/dcdt/se/sr_time.cpp b/source/dcdt/se/sr_time.cpp new file mode 100644 index 0000000000..17b43d7e78 --- /dev/null +++ b/source/dcdt/se/sr_time.cpp @@ -0,0 +1,98 @@ +#include "precompiled.h" +# include "sr_time.h" +# include "sr_string.h" + +# include "sr_output.h" + +//======================= SrTime ===================================== + +SrTime::SrTime () + { + _h = _m = 0; + _s = 0; + } + +void SrTime::set ( int h, int m, float s ) + { + _h = h; + _m = m; + _s = s; + + //sr_out<=60.0f ) + { m = (int) (_s/60.0f); + _s -= (float) (m*60); + _m += m; + } + //sr_out<=60 ) + { h = _m/60; + _m -= h*60; + _h += h; + } + //sr_out< +# define SR_TRACE_OSTREAM sr_out +# endif +# endif + +# ifndef SR_TRACE_MSG +# if defined(SR_TRACE_FILENAME) +# define SR_TRACE_MSG __FILE__ << "::" << __LINE__ << '\t' +# else +# define SR_TRACE_MSG "line " << __LINE__ << ": " +# endif +# endif + +# if defined(SR_USE_TRACE1) || defined(SR_USE_TRACE) +# define SR_TRACE1(msg) SR_TRACE_OSTREAM << SR_TRACE_MSG << msg << '\n' +# else +# define SR_TRACE1(msg) +# endif + +# if defined(SR_USE_TRACE2) || defined(SR_USE_TRACE) +# define SR_TRACE2(msg) SR_TRACE_OSTREAM << SR_TRACE_MSG << msg << '\n' +# else +# define SR_TRACE2(msg) +# endif + +# if defined(SR_USE_TRACE3) || defined(SR_USE_TRACE) +# define SR_TRACE3(msg) SR_TRACE_OSTREAM << SR_TRACE_MSG << msg << '\n' +# else +# define SR_TRACE3(msg) +# endif + +# if defined(SR_USE_TRACE4) || defined(SR_USE_TRACE) +# define SR_TRACE4(msg) SR_TRACE_OSTREAM << SR_TRACE_MSG << msg << '\n' +# else +# define SR_TRACE4(msg) +# endif + +# if defined(SR_USE_TRACE5) || defined(SR_USE_TRACE) +# define SR_TRACE5(msg) SR_TRACE_OSTREAM << SR_TRACE_MSG << msg << '\n' +# else +# define SR_TRACE5(msg) +# endif + +# if defined(SR_USE_TRACE6) || defined(SR_USE_TRACE) +# define SR_TRACE6(msg) SR_TRACE_OSTREAM << SR_TRACE_MSG << msg << '\n' +# else +# define SR_TRACE6(msg) +# endif + +# if defined(SR_USE_TRACE7) || defined(SR_USE_TRACE) +# define SR_TRACE7(msg) SR_TRACE_OSTREAM << SR_TRACE_MSG << msg << '\n' +# else +# define SR_TRACE7(msg) +# endif + +# if defined(SR_USE_TRACE8) || defined(SR_USE_TRACE) +# define SR_TRACE8(msg) SR_TRACE_OSTREAM << SR_TRACE_MSG << msg << '\n' +# else +# define SR_TRACE8(msg) +# endif + +# if defined(SR_USE_TRACE9) || defined(SR_USE_TRACE) +# define SR_TRACE9(msg) SR_TRACE_OSTREAM << SR_TRACE_MSG << msg << '\n' +# else +# define SR_TRACE9(msg) +# endif + +#endif // SR_TRACE_H + diff --git a/source/dcdt/se/sr_trackball.cpp b/source/dcdt/se/sr_trackball.cpp new file mode 100644 index 0000000000..6fe075fd49 --- /dev/null +++ b/source/dcdt/se/sr_trackball.cpp @@ -0,0 +1,86 @@ +#include "precompiled.h" +# include + +# include "sr_trackball.h" +# include "sr_mat.h" + +//# define SR_USE_TRACE1 +# include "sr_trace.h" + +//=================================== SrTrackball =================================== + +SrTrackball::SrTrackball () + { + init (); + } + +SrTrackball::SrTrackball ( const SrTrackball& t ) + :rotation(t.rotation), last_spin(t.last_spin) + { + } + +void SrTrackball::init () + { + rotation = SrQuat::null; + } + +SrMat& SrTrackball::get_mat ( SrMat &m ) const + { + rotation.get_mat ( m ); + return m; + } + +/* Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet + if we are away from the center of the sphere. */ +static float tb_project_to_sphere ( float r, float x, float y ) + { + float d, t, z; + d = sqrtf(x*x + y*y); + if ( d < r*srsqrt2/2.0f ) z=sqrtf(r*r-d*d); // Inside sphere + else { t=r/srsqrt2; z=t*t/d; } // On hyperbola + return z; + } + +void SrTrackball::get_spin_from_mouse_motion ( float p1x, float p1y, float p2x, float p2y, SrQuat& spin ) + { + SrVec p1, p2, d, a; // a==Axes of rotation + const float size=0.8f; + float t; + if ( p1x==p2x && p1y==p2y ) { spin=SrQuat::null; return; } + // figure out z-coordinates for projection of P1 and P2 to deformed sphere + + p1.set ( p1x, p1y, tb_project_to_sphere(size,p1x,p1y) ); + p2.set ( p2x, p2y, tb_project_to_sphere(size,p2x,p2y) ); + a = cross ( p1, p2 ); // axis + + d = p1-p2; // how much to rotate around that axis. + t = d.norm() / (2.0f*size); + t = SR_BOUND ( t, -1.0f, 1.0f ); // avoid problems with out-of-control values... + float ang = 2.0f * asinf(t); + + spin.set ( a, ang ); // a is normalized inside rot() : + } + +void SrTrackball::increment_from_mouse_motion ( float lwinx, float lwiny, float winx, float winy ) + { + get_spin_from_mouse_motion ( lwinx, lwiny, winx, winy, last_spin ); + rotation = last_spin * rotation; + } + +void SrTrackball::increment_rotation ( const SrQuat& spin ) + { + last_spin = spin; + rotation = last_spin * rotation; + } + +//=============================== friends ========================================== + +SrOutput& operator<< ( SrOutput& out, const SrTrackball& tb ) + { + out << "rotat: " << tb.rotation << srnl << + "spin: " << tb.last_spin << srnl; + + return out; + } + +//================================ End of File ========================================= diff --git a/source/dcdt/se/sr_trackball.h b/source/dcdt/se/sr_trackball.h new file mode 100644 index 0000000000..bab7b706c4 --- /dev/null +++ b/source/dcdt/se/sr_trackball.h @@ -0,0 +1,59 @@ + +# ifndef SR_TRACKBALL_H +# define SR_TRACKBALL_H + +/** \file sr_trackball.h + * trackball manipulation + */ + +# include "sr_vec.h" +# include "sr_quat.h" + +class SrMat; + +/*! \class SrTrackball sr_trackball.h + \brief trackball manipulation + + SrTrackball maintains a rotation with methods implementing a + trackball-like manipulations, and etc. */ +class SrTrackball + { public : + SrQuat rotation; //!< current rotation + SrQuat last_spin; + + public : + + /*! Initialize the trackball with the default parameters, see init(). */ + SrTrackball (); + + /*! Copy constructor. */ + SrTrackball ( const SrTrackball& t ); + + /*! Set the parameters to their default values, which is a null rotation. */ + void init (); + + /*! Set m to be the equivalent transformation matrix. A reference to m + is returned. */ + SrMat& get_mat ( SrMat& m ) const; + + /*! Gets the rotation induced by a mouse displacement, + according to the trackball metaphor. Window coordinates must be + normalized in [-1,1]x[-1,1]. */ + static void get_spin_from_mouse_motion ( float lwinx, float lwiny, float winx, float winy, SrQuat& spin ); + + /*! Accumulates the rotation induced by a mouse displacement, + according to the trackball metaphor. Window coordinates must be + normalized in [-1,1]x[-1,1]. */ + void increment_from_mouse_motion ( float lwinx, float lwiny, float winx, float winy ); + + /*! Accumulates the rotation with the given quaternion (left-multiplied) */ + void increment_rotation ( const SrQuat& spin ); + + /*! Outputs trackball data for inspection. */ + friend SrOutput& operator<< ( SrOutput& out, const SrTrackball& tb ); + }; + +//================================ End of File ================================================= + +# endif // SR_TRACKBALL_H + diff --git a/source/dcdt/se/sr_tree.cpp b/source/dcdt/se/sr_tree.cpp new file mode 100644 index 0000000000..84d1866cbb --- /dev/null +++ b/source/dcdt/se/sr_tree.cpp @@ -0,0 +1,459 @@ +#include "precompiled.h" +# include "sr_tree.h" + +//# define SR_USE_TRACE1 +# include "sr_trace.h" + +# define BLACK(x) ((x)->color==SrTreeNode::Black) +# define RED(x) ((x)->color==SrTreeNode::Red) +# define NIL SrTreeNode::null + +/*========================================================================== + Note: + A binary search tree is a red-black tree if it satisfies : + 1. Every node is either red or black + 2. Every null leaf is black + 3. If a node is red then both its children are black + 4. Every simple path from a node to a descendant leaf contains the + same number of black nodes +============================================================================*/ + +//=============================== SrTreeNode ==================================== + +static SrTreeNode static_null(SrTreeNode::Black); + +SrTreeNode* SrTreeNode::null = &static_null; + +//=============================== SrTreeBase ==================================== + +//----------------------------- private methods ------------------------------ + +/*! Returns 0 and leaves _cur pointing to the found matching node if there is one. + Returns >0 if the key is to be inserted under _cur->right. + Returns <0 if the key is to be inserted under _cur->left. + If the tree is empty, _cur will be null and the integer returned is 1. */ +int SrTreeBase::_search_node ( const SrTreeNode *key ) + { + int cmp; + _cur = _root; + + if ( _cur==NIL ) return 1; + + while ( true ) + { + cmp = _man->compare(key,_cur); + if ( cmp>0 ) + { if ( _cur->right!=NIL ) _cur=_cur->right; else return cmp; + } + else if ( cmp<0 ) + { if ( _cur->left!=NIL ) _cur=_cur->left; else return cmp; + } + else return cmp; + } + } + +/*! Method for right rotation of the tree about a given node. */ +void SrTreeBase::_rotate_right ( SrTreeNode *x ) + { + SR_TRACE1("Rotate Right"); + + SrTreeNode *y = x->left; + + x->left = y->right; + if ( y->right!=NIL ) y->right->parent=x; + y->parent = x->parent; + + if ( x->parent!=NIL ) + { if ( x==x->parent->right ) x->parent->right=y; + else x->parent->left=y; + } + else _root = y; + + y->right = x; + x->parent = y; + } + +/*! Method for left rotation of the tree about a given node. */ +void SrTreeBase::_rotate_left ( SrTreeNode *x ) + { + SR_TRACE1("Rotate Left"); + + SrTreeNode *y = x->right; + + x->right = y->left; + if ( y->left!=NIL ) y->left->parent=x; + y->parent = x->parent; + + if ( x->parent!=NIL ) + { if ( x==x->parent->left ) x->parent->left=y; + else x->parent->right=y; + } + else _root = y; + + y->left = x; + x->parent = y; + } + +/*! Rebalance the tree after insertion of a node. */ +void SrTreeBase::_rebalance ( SrTreeNode *x ) + { + SR_TRACE1("Rebalance"); + + SrTreeNode *y; + + while ( x!=_root && RED(x->parent) ) + { // if ( !x->parent->parent ) REPORT_ERROR + if ( x->parent==x->parent->parent->left ) + { y = x->parent->parent->right; + if ( RED(y) ) + { // handle case 1 (see CLR book, pp. 269) + x->parent->color = SrTreeNode::Black; + y->color = SrTreeNode::Black; + x->parent->parent->color = SrTreeNode::Red; + x = x->parent->parent; + } + else + { if ( x==x->parent->right ) + { // transform case 2 into case 3 (see CLR book, pp. 269) + x = x->parent; + _rotate_left ( x ); + } + // handle case 3 (see CLR book, pp. 269) + x->parent->color = SrTreeNode::Black; + x->parent->parent->color = SrTreeNode::Red; + _rotate_right ( x->parent->parent ); + } + } + else + { y = x->parent->parent->left; + if ( RED(y) ) + { // handle case 1 (see CLR book, pp. 269) + x->parent->color = SrTreeNode::Black; + y->color = SrTreeNode::Black; + x->parent->parent->color = SrTreeNode::Red; + x = x->parent->parent; + } + else + { if ( x==x->parent->left ) + { // transform case 2 into case 3 (see CLR book, pp. 269) + x = x->parent; + _rotate_right ( x ); + } + // handle case 3 (see CLR book, pp. 269) + x->parent->color = SrTreeNode::Black; + x->parent->parent->color = SrTreeNode::Red; + _rotate_left ( x->parent->parent ); + } + } + } + } + +/*! Method for restoring red-black properties after deletion. */ +void SrTreeBase::_fix_remove ( SrTreeNode *x ) + { + SR_TRACE1("Fix Remove"); + + while ( x!=_root && BLACK(x) ) + { + if ( x==x->parent->left ) + { SrTreeNode *w = x->parent->right; + if ( RED(w) ) + { w->color = SrTreeNode::Black; + x->parent->color = SrTreeNode::Red; + _rotate_left ( x->parent ); + w = x->parent->right; + } + if ( BLACK(w->left) && BLACK(w->right) ) + { w->color = SrTreeNode::Red; + x = x->parent; + } + else + { if ( BLACK(w->right) ) + { w->left->color = SrTreeNode::Black; + w->color = SrTreeNode::Red; + _rotate_right ( w ); + w = x->parent->right; + } + w->color = x->parent->color; + x->parent->color = SrTreeNode::Black; + w->right->color = SrTreeNode::Black; + _rotate_left ( x->parent ); + x = _root; + } + } + else + { SrTreeNode *w = x->parent->left; + if ( RED(w) ) + { w->color = SrTreeNode::Black; + x->parent->color = SrTreeNode::Red; + _rotate_right ( x->parent ); + w = x->parent->left; + } + if ( BLACK(w->left) && BLACK(w->right) ) + { w->color = SrTreeNode::Red; + x = x->parent; + } + else + { if ( BLACK(w->left) ) + { w->right->color = SrTreeNode::Black; + w->color = SrTreeNode::Red; + _rotate_left ( w ); + w = x->parent->left; + } + w->color = x->parent->color; + x->parent->color = SrTreeNode::Black; + w->left->color = SrTreeNode::Black; + _rotate_right ( x->parent ); + x = _root; + } + } + } + + x->color = SrTreeNode::Black; + } + +//----------------------------- constructors ------------------------------ + +SrTreeBase::SrTreeBase ( SrClassManagerBase* m ) + { + _root = _cur = NIL; + _elements = 0; + _man = m; + _man->ref(); + } + +SrTreeBase::SrTreeBase ( const SrTreeBase& t ) + { + _root = _cur = NIL; + _elements = 0; + _man = t._man; + _man->ref(); + insert_tree ( t ); + } + +SrTreeBase::~SrTreeBase () + { + init (); + _man->unref(); + } + +void SrTreeBase::init () + { + _cur = _root; + SrTreeNode *curp; + + while ( _cur!=NIL ) + { // 1. descend _cur to a leaf + while ( _cur->left!=NIL || _cur->right!=NIL ) // while cur is not a leaf + _cur = _cur->left!=NIL? _cur->left:_cur->right; + + // 2. unlink _cur + curp = _cur->parent; + if ( _cur!=_root ) + { if ( curp->left==_cur ) curp->left=NIL; + else curp->right=NIL; + } + + // 3. delete and update _cur + _man->free ( _cur ); + _cur = curp; + } + + _elements = 0; + _root = _cur = SrTreeNode::null; + } + +SrTreeNode *SrTreeBase::get_min ( SrTreeNode *x ) const + { + if ( x==NIL ) return x; + while ( x->left!=NIL ) x=x->left; + return x; + } + +SrTreeNode *SrTreeBase::get_max ( SrTreeNode *x ) const + { + if ( x==NIL ) return x; + while ( x->right!=NIL ) x=x->right; + return x; + } + +SrTreeNode *SrTreeBase::get_next ( SrTreeNode *x ) const + { + if ( x->right!=NIL ) return get_min ( x->right ); + + SrTreeNode *y = x->parent; + while ( y!=NIL && x==y->right ) + { x = y; + y = y->parent; + } + + return y; + } + +SrTreeNode *SrTreeBase::get_prior ( SrTreeNode *x ) const + { + if ( x->left!=NIL ) return get_max ( x->left ); + + SrTreeNode *y = x->parent; + while ( y!=NIL && x==y->left ) + { x = y; + y = y->parent; + } + + return y; + } + +SrTreeNode *SrTreeBase::search ( const SrTreeNode *key ) + { + if ( _root==NIL ) return 0; + return _search_node(key)==0? _cur:0; + } + +SrTreeNode *SrTreeBase::insert ( SrTreeNode *key ) + { + int cmp = _search_node ( key ); + + if ( _cur!=NIL ) + { if ( cmp>0 ) // key>_cur + { // if (_cur->right) REPORT_ERROR; + _cur->right = key; + key->parent = _cur; + _rebalance ( key ); + _root->color = SrTreeNode::Black; + _elements++; + return key; + } + else if ( cmp<0 ) // key<_cur + { // if (_cur->left) REPORT_ERROR + _cur->left = key; + key->parent = _cur; + _rebalance ( key ); + _root->color = SrTreeNode::Black; + _elements++; + return key; + } + else return 0; // not inserted, already in the tree + } + else // tree empty + { _root = key; + _root->init (); + _root->color = SrTreeNode::Black; + _elements++; + return key; + } + } + +SrTreeNode *SrTreeBase::insert_or_del ( SrTreeNode *key ) + { + if ( !insert(key) ) + { _man->free ( key ); + return 0; + } + return key; + } + +void SrTreeBase::insert_tree ( const SrTreeBase& t ) + { + if ( this==&t ) return; + + SrTreeIteratorBase it(t); + for ( it.first(); it.inrange(); it.next() ) + { insert_or_del ( (SrTreeNode*)_man->alloc(it.get()) ); + } + } + +SrTreeNode *SrTreeBase::extract ( SrTreeNode *z ) + { + SrTreeNode *x, *y; + + y = ( z->left==NIL || z->right==NIL )? z : get_next(z); + + x = ( y->left!=NIL )? y->left : y->right; + + x->parent = y->parent; + + if ( y->parent!=NIL ) + { if ( y==y->parent->left ) y->parent->left=x; + else y->parent->right=x; + } + else _root = x; + + SrTreeNode::Color ycolor = y->color; + + if ( y!=z ) // make y be z + { y->left=z->left; y->right=z->right; y->parent=z->parent; y->color=z->color; + if ( z->left ) z->left->parent=y; + if ( z->right ) z->right->parent=y; + if ( z->parent ) + { if ( z->parent->left==z ) z->parent->left=y; + else z->parent->right=y; + } + if ( _root==z ) _root=y; + } + + if ( ycolor==SrTreeNode::Black ) _fix_remove ( x ); + + _elements--; + _cur = z; + z->init(); + return z; + } + +void SrTreeBase::remove ( SrTreeNode* z ) + { + extract ( z ); + _man->free ( z ); + } + +SrTreeNode* SrTreeBase::search_and_extract ( const SrTreeNode* key ) + { + int cmp = _search_node ( key ); + if ( cmp!=0 ) return 0; // not found + return extract ( _cur ); + } + +bool SrTreeBase::search_and_remove ( const SrTreeNode* key ) + { + int cmp = _search_node ( key ); + if ( cmp==0 ) { remove ( _cur ); return true; } + return false; + } + +void SrTreeBase::take_data ( SrTreeBase& t ) + { + _root = t._root; t._root = 0; + _cur = t._cur; t._cur = 0; + _elements = t._elements; t._elements = 0; + } + +void SrTreeBase::operator= ( const SrTreeBase& t ) + { + init (); + insert_tree ( t ); // need to write a copy routine instead of inserting all nodes... + } + +SrOutput& operator<< ( SrOutput& o, const SrTreeBase& t ) + { + o<<'['; + SrTreeIteratorBase it(t); + for ( it.first(); it.inrange(); it.next() ) + { t._man->output ( o, it.cur() ); + if ( !it.inlast() ) o << ' '; + } + return o<<']'; + } + +//=========================== SrTreeIteratorBase ================================ + +SrTreeIteratorBase::SrTreeIteratorBase ( const SrTreeBase& t ) : _tree(t) + { + reset (); + } + +void SrTreeIteratorBase::reset () + { + _first = _cur = _tree.first(); + _last = _tree.last(); + } + +//============================ End of File ================================= diff --git a/source/dcdt/se/sr_tree.h b/source/dcdt/se/sr_tree.h new file mode 100644 index 0000000000..49c7a88e3d --- /dev/null +++ b/source/dcdt/se/sr_tree.h @@ -0,0 +1,289 @@ + +/** \file sr_tree.h + * A red-black balanced search tree */ + +# ifndef SR_TREE_H +# define SR_TREE_H + +# include "sr_class_manager.h" +# include "sr_output.h" + +/*! \class SrTreeNode sr_tree_node.h + \brief A red-black node for SrTree + + SrTreeNode is the node that classes should derive in order to be + inserted in SrTree. This class has all its data members as public, + so that the user can use them as needed. There are no methods for + manipulation of the nodes inside this class, all such manipulation + methods are in SrTreeBase. */ +class SrTreeNode + { public : + /*! The color type of the node. */ + enum Color { Red, Black }; + + /*! SrTreeNode::null is initialized in sr_tree_node.cpp, and serves + as a sentinel indicating a null node. SrTreeNode::null is a black + node where its pointers are initialized as pointing to itself, but + the parent pointer may keep arbitrary values during manipulation + inside SrTreeBase. */ + static SrTreeNode* null; + public : + Color color; //!> Color of the node. + SrTreeNode* parent; //!> Pointer to node's parent. + SrTreeNode* left; //!> Pointer to node's left child. + SrTreeNode* right; //!> Pointer to node's righ child. + public : + /*! Default Constructor. */ + SrTreeNode ( Color c=Red ) : color(c), parent(null), left(null), right(null) {} + public : + /*! Sets all links to SrTreeNode::null, but leaves color unchanged. */ + void init () { parent=left=right=null; } + }; + +/*! \class SrTreeBase sr_tree.h + \brief red-black tree base class + + This class contains all methods for tree manipulation. The user should + however use the template class SrTree for an implementation that + includes automatic type casts for the user type. A manager to the + user data is required, which must be a class deriving SrTreeNode. + For details about red black trees, see: + T. H. Cormen, C. E. Leiserson, and R. L. Rivest, "Introduction to Algorithms" + 1990, MIT, chapter 14, ISBN 0-262-03141-8 +

+ Note: + A binary search tree is a red-black tree if it satisfies :
+ 1. Every node is either red or black
+ 2. Every null leaf is black
+ 3. If a node is red then both its children are black
+ 4. Every simple path from a node to a descendant leaf contains the same number of black nodes
*/ +class SrTreeBase + { private : + SrTreeNode *_root; // the root of the tree + SrTreeNode *_cur; // the current element of the tree + SrClassManagerBase* _man; // manager of user data, that derives SrListNode + int _elements; // number of elements of the tree + int _search_node ( const SrTreeNode* key ); + void _rotate_right ( SrTreeNode* x ); + void _rotate_left ( SrTreeNode* x ); + void _rebalance ( SrTreeNode* x ); + void _fix_remove ( SrTreeNode* x ); + + public : + + /*! Initiates an empty tree. The class manager must manage a user class + deriving from SrTreeNode. */ + SrTreeBase ( SrClassManagerBase* m ); + + /*! Copy constructor. The class manager of t is shared. */ + SrTreeBase ( const SrTreeBase& t ); + + /*! Destructor. */ + ~SrTreeBase (); + + /*! Deletes all elements of the tree. */ + void init (); + + /*! Returns true iff there is no nodes in the tree. */ + bool empty () const { return _root==SrTreeNode::null? true:false; } + + /*! Returns the number of elements of the tree. */ + int elements () const { return _elements; } + + /*! Method to find the minimum node of the subtree rooted at the given node x. */ + SrTreeNode* get_min ( SrTreeNode* x ) const; + + /*! Method to find the maximum node of the subtree rooted at the given node x. */ + SrTreeNode* get_max ( SrTreeNode* x ) const; + + /*! Method to find the successor node of the given node x in the tree. */ + SrTreeNode* get_next ( SrTreeNode* x ) const; + + /*! Method to find the predecessor node of the given node x in the tree. */ + SrTreeNode* get_prior ( SrTreeNode* x ) const; + + /*! Returns the current element being pointed, that will be SrTreeNode::null + if the tree is emptyis returned. */ + SrTreeNode* cur () const { return _cur; } + + /*! Sets the current element to be c, which must be a node of the tree. */ + void cur ( SrTreeNode* c ) { _cur = c; } + + /*! Returns the root of the tree, that will be SrTreeNode::null if the tree is empty. */ + SrTreeNode* root () { return _root; } + + /*! Returns the first element of the tree, ie, the minimum according + to the comparison function. */ + SrTreeNode* first () const { return get_min(_root); } + + /*! Returns the last element of the tree, ie, the maximum. */ + SrTreeNode* last () const { return get_max(_root); } + + /*! Will put the current position cur() pointing to the node with minimum value. + If the list is empty, cur will point to SrTreeNode::null. */ + void gofirst () { _cur = get_min(_root); } + + /*! Will put the current position cur() pointing to the node with maximum value. + If the list is empty, cur will point to SrTreeNode::null. */ + void golast () { _cur = get_max(_root); } + + /*! Returns the next element of the current position cur(). + If cur points to null, SrTreeNode::Null is returned. */ + SrTreeNode* curnext () const { return get_next(_cur); } + + /*! Returns the prior element of the current position cur(). + If cur points to null, SrTreeNode::null is returned. */ + SrTreeNode* curprior () const { return get_prior(_cur); } + + /*! Will put the current position cur() pointing to the next + node curnext(), cur can become SrTreeNode::null. */ + void gonext () { _cur = get_next(_cur); } + + /*! Will put the current position cur() pointing to the prior + link curprior(), cur can become SrTreeNode::null. */ + void goprior () { _cur = get_prior(_cur); } + + /*! Returns a pointer to the item that is equal to the given key, or 0 + if it could not find the key in the tree. cur will be the last node + visited during the search. */ + SrTreeNode* search ( const SrTreeNode *key ); + + /*! If inserted, will return key, otherwise will return 0, and cur will + point to key. Duplication is not allowed. */ + SrTreeNode* insert ( SrTreeNode* key ); + + /*! Tries to insert key using method insert(). In case of duplication + key is not inserted, key is deleted, and 0 is returned. The returned + node will be pointing to key in case of sucees, or pointing to the + node in the tree that is equal to key in case of failure. In all cases, + cur will be the last node visited during the search for key. */ + SrTreeNode* insert_or_del ( SrTreeNode* key ); + + /*! Duplicates and inserts all elements of t in the tree. */ + void insert_tree ( const SrTreeBase& t ); + + /*! Extracts node z that must be inside the tree; z is returned. */ + SrTreeNode* extract ( SrTreeNode* z ); + + /*! Removes and delete node z, which must be inside the tree. */ + void remove ( SrTreeNode* z ); + + /*! Extracts the item equal to the key and return it or 0 if the + key was not found. */ + SrTreeNode* search_and_extract ( const SrTreeNode* key ); + + /*! Removes and deletes the item equal to the key. Returns true if a node + is found and deleted, otherwise false is returned. */ + bool search_and_remove ( const SrTreeNode* key ); + + /*! Take control of the tree in t, and set t to an empty tree. Both + trees must manage the same derived class of SrTreeNode. */ + void take_data ( SrTreeBase& t ); + + /*! Copy operator */ + void operator= ( const SrTreeBase& t ); + + /*! Outputs the tree in the format: [e1 e2 en] */ + friend SrOutput& operator<< ( SrOutput& o, const SrTreeBase& t ); + }; + +/*! \class SrTree sr_tree.h + \brief red-black balanced tree + + SrTree defines automatic type casts to the user type, which must + derive SrTreeNode. To traverse the tree, the first, last, next, + and prior keywords are related to the order defined by the comparison + method in the class manager. For documentation of the methods + see the documentation of the base class SrTreeBase methods. */ +template +class SrTree : public SrTreeBase + { public: + + /*! Default constructor that automatically creates a SrClassManager. */ + SrTree () : SrTreeBase ( new SrClassManager ) {} + + /*! Constructor with a given class manager. */ + SrTree ( SrClassManagerBase* m ) : SrTreeBase ( m ) {} + + /*! Copy constructor with class manager sharing. */ + SrTree ( const SrTree& t ) : SrTreeBase ( t ) {} + + X* cur () { return (X*)SrTreeBase::cur(); } + void cur ( X* c ) { SrTreeBase::cur((SrTreeNode*)c); } + X* root () { return (X*)SrTreeBase::root(); } + X* first () { return (X*) SrTreeBase::first(); } + X* last () { return (X*) SrTreeBase::last(); } + X* curnext () const { return (X*)SrTreeBase::curnext(); } + X* curprior () const { return (X*)SrTreeBase::curprior(); } + X* search ( const X* key ) { return (X*)SrTreeBase::search(key); } + X* insert ( X* key ) { return (X*)SrTreeBase::insert(key); } + X* insert_or_del ( X* key ) { return (X*)SrTreeBase::insert_or_del(key); } + X* extract ( X* n ) { return (X*) SrTreeBase::extract(n); } + void remove ( X* n ) { SrTreeBase::remove(n); } + X* search_and_extract ( const X* key ) { return (X*) SrTreeBase::search_and_extract(key); } + bool search_and_remove ( const X* key ) { return SrTreeBase::search_and_remove(key); } + }; + +/*! Base class for iterating over trees. */ +class SrTreeIteratorBase + { private : + SrTreeNode* _cur; + SrTreeNode* _first; + SrTreeNode* _last; + const SrTreeBase& _tree; + + public : + /*! Constructor */ + SrTreeIteratorBase ( const SrTreeBase& t ); + + /*! Returns the current node being pointed by the iterator */ + SrTreeNode* cur () const { return _cur; } + + /*! Returns the first node in the associated tree */ + SrTreeNode* getfirst () const { return _first; } + + /*! Returns the last node in the associated tree */ + SrTreeNode* getlast () const { return _last; } + + /*! Must be called each time the associate tree is changed */ + void reset (); + + /*! Points the iterator to the first element. */ + void first () { _cur=_first; } + + /*! Points the iterator to the last element. */ + void last () { _cur=_last; } + + /*! Advances the current position of the iterator to the next one */ + void next () { _cur=_tree.get_next(_cur); } + + /*! Walk back the current position of the iterator of one position */ + void prior () { _cur=_tree.get_prior(_cur); } + + /*! Returns true if get() points to a valid position */ + bool inrange () { return _cur==SrTreeNode::null? false:true; } + + /*! Returns the current element, can return SrTreeNode::null */ + SrTreeNode* get () { return _cur; } + + /*! Returns true if the current position is pointing to the last element. */ + bool inlast () const { return _cur==_last? true:false; } + + /*! Returns true if the current position is pointing to the first element */ + bool infirst () const { return _cur==_first? true:false; } + }; + +/*! Derives SrTreeIteratorBase providing correct type casts for the user type */ +template +class SrTreeIterator : public SrTreeIteratorBase + { public : + SrTreeIterator ( const SrTree& s ) : SrTreeIteratorBase(s) {} + X* get () { return (X*)SrTreeIteratorBase::get(); } + X* operator-> () { return (X*)SrTreeIteratorBase::get(); } + }; + + + +//============================ End of File ================================= + +# endif // SR_TREE_H diff --git a/source/dcdt/se/sr_triangle.cpp b/source/dcdt/se/sr_triangle.cpp new file mode 100644 index 0000000000..207f012e49 --- /dev/null +++ b/source/dcdt/se/sr_triangle.cpp @@ -0,0 +1,93 @@ +#include "precompiled.h" +//# include +# include "sr_triangle.h" +//# include "sr_vec2.h" + +// ==================== static funcs ==================================== + +#define EPSILON srtiny + +static void matinverse ( float M[9], const SrVec &l1, const SrVec &l2, const SrVec &l3 ) + { + M[0]=l2.y*l3.z-l2.z*l3.y; M[3]=l2.z*l3.x-l2.x*l3.z; M[6]=l2.x*l3.y-l2.y*l3.x; + M[1]=l1.z*l3.y-l1.y*l3.z; M[4]=l1.x*l3.z-l1.z*l3.x; M[7]=l1.y*l3.x-l1.x*l3.y; + M[2]=l1.y*l2.z-l1.z*l2.y; M[5]=l1.z*l2.x-l1.x*l2.z; M[8]=l1.x*l2.y-l1.y*l2.x; + + float d = l1.x*M[0] + l1.y*M[3] + l1.z*M[6]; + SR_ASSERT ( d<-EPSILON || d>EPSILON ); // check if singular matrix + d = 1.0f/d; + + M[0]*=d; M[1]*=d; M[2]*=d; M[3]*=d; M[4]*=d; M[5]*=d; M[6]*=d; M[7]*=d; M[8]*=d; + } + +static void matposmult ( const SrVec& v, const float M[9], SrVec &r ) // r = v M + { + r.x = v.x*M[0] + v.y*M[3] + v.z*M[6]; + r.y = v.x*M[1] + v.y*M[4] + v.z*M[7]; + r.z = v.x*M[2] + v.y*M[5] + v.z*M[8]; + } + +//============================== SrTriangle ==================================== + +/*! The following version was tested with 2d triangle and works: + Returns k, such that a*k.x + b*k.y + c*k.z == p, k.x+k.y+k.z==1 */ +/* +static SrVec barycentric ( const SrPnt2& a, const SrPnt2& b, const SrPnt2& c, const SrPnt2& p ) + { + # define DET3(a,b,c,d,e,f,g,h,i) a*e*i +b*f*g +d*h*c -c*e*g -b*d*i -a*f*h + float A = DET3 ( a.x, b.x, c.x, a.y, b.y, c.y, 1, 1, 1 ); + float A1 = DET3 ( p.x, b.x, c.x, p.y, b.y, c.y, 1, 1, 1 ); + float A2 = DET3 ( a.x, p.x, c.x, a.y, p.y, c.y, 1, 1, 1 ); + float A3 = DET3 ( a.x, b.x, p.x, a.y, b.y, p.y, 1, 1, 1 ); + return SrVec ( A1/A, A2/A, A3/A ); + # undef DET3 + }*/ + +SrVec SrTriangle::barycentric ( const SrVec &p ) const + { + float m[9]; + SrVec k; + matinverse ( m, a, b, c ); + matposmult ( m, p, k ); + return k; + } + +void SrTriangle::translate ( const SrVec &k, const SrVec& v ) + { + float k2 = k.x*k.x + k.y*k.y + k.z*k.z; + + a += (k.x+1.0f-k2)*v; + b += (k.y+1.0f-k2)*v; + c += (k.z+1.0f-k2)*v; + } + +void SrTriangle::translate ( const SrVec& v ) + { + a += v; + b += v; + c += v; + } + +SrVec SrTriangle::normal () const + { + SrVec n; + n.cross ( b-a, c-a ); + n.normalize(); + return n; + } + +SrOutput& operator<< ( SrOutput& o, const SrTriangle& t ) + { + return o << t.a <<','<< t.b <<','<< t.c; + } + +SrInput& operator>> ( SrInput& in, SrTriangle& t ) + { + in >> t.a; in.getd(); + in >> t.b; in.getd(); + in >> t.c; + return in; + } + +//================================== End of File =========================================== + diff --git a/source/dcdt/se/sr_triangle.h b/source/dcdt/se/sr_triangle.h new file mode 100644 index 0000000000..ada1fde565 --- /dev/null +++ b/source/dcdt/se/sr_triangle.h @@ -0,0 +1,53 @@ + +/** \file sr_triangle.h + * A triangle in 3d space */ + +# ifndef SR_TRIANGLE_H +# define SR_TRIANGLE_H + +# include "sr_vec.h" + +/*! \class SrTriangle sr_triangle.h + \brief A triangle in 3d space. + + */ +class SrTriangle + { public : + SrPnt a, b, c; + + public : + + /*! Initializes SrTriangle as the triangle (i,j,k). Implemented inline. */ + SrTriangle () : a(SrPnt::i), b(SrPnt::j), c(SrPnt::k) {} + + /*! Constructor from three points. */ + SrTriangle ( const SrPnt& p1, const SrPnt& p2, const SrPnt& p3 ) : a(p1), b(p2), c(p3) {} + + /*! Sets vertices coordinates. */ + void set ( const SrPnt& p1, const SrPnt& p2, const SrPnt& p3 ) { a=p1; b=p2; c=p3; } + + /*! Returns k, such that a*k.x + b*k.y + c*k.z == p. */ + SrVec barycentric ( const SrPnt &p ) const; + + /*! Update the position of triangle [a,b,c] in order to keep the baricentric + coordinates of an interior point that moved. Let p=a*k.x + b*k.y + c*k.z. + When p is displaced to p+v, the triangle vertices [a,b,c] are updated so + to achieve a*k.x + b*k.y + c*k.z == p+v. */ + void translate ( const SrVec &k, const SrVec& v ); + + /*! Apply the translation v to each triangle vertex. */ + void translate ( const SrVec& v ); + + /*! Returns the normalized triangle normal. */ + SrVec normal () const; + + /*! Outputs in format: "a,b,c". */ + friend SrOutput& operator<< ( SrOutput& o, const SrTriangle& t ); + + /*! Inputs from format: "a,b,c". */ + friend SrInput& operator>> ( SrInput& in, SrTriangle& t ); + }; + +//============================== end of file =============================== + +# endif // SR_TRIANGLE_H diff --git a/source/dcdt/se/sr_triangulation.cpp b/source/dcdt/se/sr_triangulation.cpp new file mode 100644 index 0000000000..1b62c23145 --- /dev/null +++ b/source/dcdt/se/sr_triangulation.cpp @@ -0,0 +1,379 @@ +#include "precompiled.h" +# include "sr_random.h" +# include "sr_geo2.h" +# include "sr_triangulation.h" + +//# define SR_USE_TRACE1 // search triangle +//# define SR_USE_TRACE2 // recover delaunay +# include "sr_trace.h" + +# define PRINT(t) { sr_out<vid(); sr_out<<" "<nxt()->vid()<pri(); } +# define DELPREC 1.0E-10 +# define LINPREC 1.0E-5 + +//============================== SrTri ======================================= + +void SrTri::set ( SrVtx* v0, SrVtx* v1, SrVtx* v2, SrTri* t0, SrTri* t1, SrTri* t2, int id ) + { + _vtx[0] = v0; + _vtx[1] = v1; + _vtx[2] = v2; + _side[0] = t0; + _side[1] = t1; + _side[2] = t2; + _mark[0] = _mark[1] = _mark[2] = 0; + _id = id; + } + +// returns the j index to achieve _side[i]->_side[j]==this +int SrTri::sideindex ( int i ) + { + SrTri* s = _side[i]; + SrVtx* v = _vtx[i]; + return s->_vtx[1]==v? 0 : + s->_vtx[2]==v? 1 : 2; + } + +//=========================== SrTravel ============================== + +SrTravel SrTravel::sym () const + { + SrVtx* ov = _t->_vtx[_v]; // original vertex + SrTri* at = _t->_side[_v]; // adjacent triangle + return SrTravel ( at, at->_vtx[0]==ov? 2 : + at->_vtx[1]==ov? 0 : 1 ); + } + +bool SrTravel::is_delaunay () const + { + SrVtx* p = vtx(2); // opposite vtx + SrTravel s = sym(); + SrVtx* a = s.vtx(); + SrVtx* b = s.vtx(1); + SrVtx* c = s.vtx(2); + + //sr_out<<"Del test: "<id()<id()<id()<<":"<id()<x(), a->y(), b->x(), b->y(), c->x(), c->y(), p->x(), p->y() ); + } + +bool SrTravel::is_flippable () const + { + double p1x, p2x, p3x, p4x, p1y, p2y, p3y, p4y; + + p1x=x(); p1y=y(); SrTravel s=sym(); + p2x=s.x(); p2y=s.y(); s=s.pri(); + p3x=s.x(); p3y=s.y(); s=pri(); + p4x=s.x(); p4y=s.y(); + + return SR_CCW(p3x,p3y,p2x,p2y,p4x,p4y)>0 && + SR_CCW(p4x,p4y,p1x,p1y,p3x,p3y)>0 ? true:false; + } + +//============================== SrTriangulation ======================================= + +SrTriangulation::SrTriangulation () + { + } + +SrTriangulation::~SrTriangulation () + { + while ( _tri.size() ) delete _tri.pop(); + while ( _vtx.size() ) delete _vtx.pop(); + } + +const SrArray& SrTriangulation::edges() + { + int i; + SrTravel t; + _stack.size(0); + for ( i=2; i<_tri.size(); i++ ) // skip the first two backfaces + { t.set ( _tri[i], 0 ); + if ( !t.marked() ) { t.mark(); t.sym().mark(); _stack.push()=t; } + t = t.nxt(); + if ( !t.marked() ) { t.mark(); t.sym().mark(); _stack.push()=t; } + t = t.nxt(); + if ( !t.marked() ) { t.mark(); t.sym().mark(); _stack.push()=t; } + } + reset_markers(); + return _stack; + } + +void SrTriangulation::init ( double ax, double ay, double bx, double by ) + { + while ( _tri.size()>4 ) delete _tri.pop(); + while ( _vtx.size()>4 ) delete _vtx.pop(); + while ( _tri.size()<4 ) _tri.push() = new SrTri; + while ( _vtx.size()<4 ) _vtx.push() = new SrVtx; + + # define TRISET(i,v0,v1,v2,t0,t1,t2) _tri[i]->set(_vtx[v0],_vtx[v1],_vtx[v2],_tri[t0],_tri[t1],_tri[t2],i) + + _vtx[0]->set ( ax, ay, 0 ); + _vtx[1]->set ( bx, ay, 1 ); + _vtx[2]->set ( bx, by, 2 ); + _vtx[3]->set ( ax, by, 3 ); + + TRISET ( 0, 0, 2, 1, 1, 2, 2 ); + TRISET ( 1, 0, 3, 2, 3, 3, 0 ); + TRISET ( 2, 0, 1, 2, 0, 0, 3 ); + TRISET ( 3, 0, 2, 3, 2, 1, 1 ); + + # undef TRISET + } + +SrTravel SrTriangulation::border () + { + return SrTravel ( _tri[0], 2 ); // travel vertices: ( 1, 0 ) + } + +void SrTriangulation::reset_markers () + { + int i; + for ( i=0; i<_tri.size(); i++ ) + { _tri[i]->_mark[0] = 0; + _tri[i]->_mark[1] = 0; + _tri[i]->_mark[2] = 0; + } + } + +void SrTriangulation::addv ( SrTravel& tri, double x, double y ) + { + SrVtx* v = _vtx.push() = new SrVtx; + v->set ( x, y, _vtx.size()-1 ); + + SrTri* n0 = tri._t; + SrTri* n1 = _tri.push() = new SrTri; + SrTri* n2 = _tri.push() = new SrTri; + + SrVtx* v0 = n0->_vtx[0]; + SrVtx* v1 = n0->_vtx[1]; + SrVtx* v2 = n0->_vtx[2]; + + SrTri* s0 = n0->_side[0]; + SrTri* s1 = n0->_side[1]; + SrTri* s2 = n0->_side[2]; + + int i0 = n0->sideindex ( 0 ); + int i1 = n0->sideindex ( 1 ); + int i2 = n0->sideindex ( 2 ); + + n0->set ( v0, v1, v, s0, n1, n2, n0->_id ); + n1->set ( v1, v2, v, s1, n2, n0, _tri.size()-2 ); + n2->set ( v2, v0, v, s2, n0, n1, _tri.size()-1 ); + + s0->_side[i0] = n0; + s1->_side[i1] = n1; + s2->_side[i2] = n2; + + tri._v = 2; + } + +void SrTriangulation::flip ( SrTravel& edg ) + { + SrVtx *v0, *v1, *v2, *v3; + SrTri *s0, *s1, *s2, *s3, *n0, *n1; + + SrTravel d = edg; + SrTravel d0 = d.nxt(); + SrTravel d2 = d.pri(); + SrTravel d1 = d.sym().pri(); + SrTravel d3 = d1.pri(); + + n0=d0.tri(); n1=d1.tri(); + + v0=d0.vtx(); v1=d1.vtx(); v2=d2.vtx(); v3=d3.vtx(); + + s0=d0.adt(); s1=d1.adt(); s2=d2.adt(); s3=d3.adt(); + + int i0 = n0->sideindex ( d0._v ); + int i1 = n1->sideindex ( d1._v ); + int i2 = n0->sideindex ( d2._v ); + int i3 = n1->sideindex ( d3._v ); + + n0->set ( v0, v2, v1, s0, n1, s1, n0->_id ); + n1->set ( v3, v1, v2, s3, n0, s2, n1->_id ); + s0->_side[i0] = s1->_side[i1] = n0; + s2->_side[i2] = s3->_side[i3] = n1; + + edg._v = 1; + } + +/* +static void ptri ( const Travel* t ) + { + static float p1x, p2x, p3x, p4x, p1y, p2y, p3y, p4y; + Travel* s=t->dup(); + + sr_out.fmt_float ( "%+5.3f" ); + p1x=s->x(); p1y=s->y(); s->nxt(); + p2x=s->x(); p2y=s->y(); s->nxt(); + p3x=s->x(); p3y=s->y(); s->nxt(); + sr_out<x()!=p1x || s->y()!=p1y ) sr_out<<"(Not a Triangle) "; + sr_out<dup(); + + sr_out.fmt_float ( "%+5.3f" ); + x=s->x(); y=s->y(); + sr_out<nxt(); + x=s->x(); y=s->y(); + s->pri(); + sr_out<rot(); + } while ( !s->equal(t) ); + + sr_out<back() ) continue; + SR_TRACE2 ( "pop: ("<x(); p2y=s.vtx(1)->y(); + p3x=s.vtx(2)->x(); p3y=s.vtx(2)->y(); + + if ( sr_next(x,y,p1x,p1y,prec) ) { SR_TRACE1("Found Vertex."); return VertexFound; } + if ( sr_next(x,y,p2x,p2y,prec) ) { s=s.nxt(); SR_TRACE1("Found Vertex."); return VertexFound; } + if ( sr_next(x,y,p3x,p3y,prec) ) { s=s.pri(); SR_TRACE1("Found Vertex."); return VertexFound; } + + _stack.size(0); + if ( SR_CCW(x,y,p1x,p1y,p2x,p2y)<0 ) _stack.push()=s.sym(); + if ( SR_CCW(x,y,p2x,p2y,p3x,p3y)<0 ) _stack.push()=s.nxt().sym(); + if ( SR_CCW(x,y,p3x,p3y,p1x,p1y)<0 ) _stack.push()=s.pri().sym(); + + if ( _stack.empty() ) + { SR_TRACE1("Found Triangle."); return TriangleFound; } + else if ( _stack.size()==1 ) + { s = _stack[0]; } + else // delaunay searches, in principle, do not need this extra check... + { s = _stack [ SrRandom::randf()<0.5f? 0:1 ]; } + } + } + +bool SrTriangulation::insert_point ( double x, double y, SrTravel& t, double prec ) + { + LocateResult res = locate_point(x,y,t,prec); + + if ( res==TriangleFound ) + { addv ( t, x, y ); + recover_delaunay ( t ); + return true; + } + + return res==NotFound? false:true; + } + +void SrTriangulation::output ( SrOutput& out ) + { + int i; + for ( i=0; i<_vtx.size(); i++ ) + { out<x()<y()<_vtx[0]->id()<_vtx[1]->id()<_vtx[2]->id()<<" : "; + out<<_tri[i]->_side[0]->id()<_side[1]->id()<_side[2]->id()< stack(0,0,32); + stack.push() = get_border_travel()->dup(); + Travel* t = stack.top()->dup(); + begin_marking(); + int n=0; + + while ( stack.size()>0 ) + { Travel* ti = stack.pop(); + t->set ( ti ); + + do { if ( !is_delaunay(t) ) n++; + t->mark(); + t->sym(); + if ( !t->marked() ) stack.push() = t->dup(); + t->sym(); + t->nxt(); + } while ( !t->equal(ti) ); + + delete ti; + } + + delete t; + end_marking(); + return n; + } +*/ \ No newline at end of file diff --git a/source/dcdt/se/sr_triangulation.h b/source/dcdt/se/sr_triangulation.h new file mode 100644 index 0000000000..219c63cf7f --- /dev/null +++ b/source/dcdt/se/sr_triangulation.h @@ -0,0 +1,170 @@ + +# ifndef SR_TRIANGULATION +# define SR_TRIANGULATION + +# include "sr_array.h" + +class SrTravel; +class SrTriangulation; + +//=========================== SrVtx ================================== + +/*! A vertex of the triangulation */ +class SrVtx + { private : + double _x, _y; + int _id; + private : + friend SrTriangulation; + void set ( double x, double y, int id ) { _x=x; _y=y; _id=id; } + public : + double x() const { return _x; } + double y() const { return _y; } + int id() const { return _id; } + bool border() const { return _id<4? true:false; } + }; + +//=========================== SrTri ================================== + +/*! A triangle of the triangulation */ +class SrTri + { private : + SrVtx* _vtx[3]; // The three vertices of this triangle + SrTri* _side[3]; // The three adjacent triangles of this triangle + char _mark[3]; // Used for marking traversal elements + int _id; + private : + friend SrTravel; + friend SrTriangulation; + void set ( SrVtx* v0, SrVtx* v1, SrVtx* v2, SrTri* t0, SrTri* t1, SrTri* t2, int id ); + int sideindex ( int i ); + + public : + int id() const { return _id; } + bool back() const { return _id==0||_id==1? true:false; } + const SrVtx* v0 () { return _vtx[0]; } + const SrVtx* v1 () { return _vtx[1]; } + const SrVtx* v2 () { return _vtx[2]; } + }; + +//=========================== SrTravel ================================== + +/*! The traversal element of the triangulation always references one + triangle, one edge and one vertex. */ +class SrTravel + { private : + SrTri* _t; // the referenced triangle + int _v; // the referenced vertex id of the triangle + friend SrTriangulation; + + public : + SrTravel () : _t(0), _v(0) {} + SrTravel ( SrTri* t, int v ) : _t(t), _v(v) {} + SrTravel ( const SrTravel& tr ) : _t(tr._t), _v(tr._v) {} + + void set ( SrTri* t, int v ) { _t=t; _v=v; } + + bool border () { return _t->back()^_t->_side[_v]->back(); } /*! Returns true if the travel is referencing an edge in the border of the triangulation (^is xor) */ + bool back () { return _t->back(); } /*! Returns true if the travel is referencing one of the two triangles describing the 'backfaces' of the triangulation */ + + SrVtx* vtx () const { return _t->_vtx[_v]; } // Returns the referenced vertex + SrVtx* vtx ( unsigned n ) const { return _t->_vtx[(_v+n)%3]; } // referenced vertex after 'n' nxt() + int vid () const { return _t->_vtx[_v]->id(); } // Returns the id of the referenced vertex + int vnid () const { return _t->_vtx[(_v+1)%3]->id(); } // Returns the id of the next vertex referenced in the triangle (as if nxt() was applied) + double x () const { return _t->_vtx[_v]->x(); } /*! Returns the x coordinate of the referenced vertex */ + double y () const { return _t->_vtx[_v]->y(); } /*! Returns the y coordinate of the referenced vertex */ + + SrTri* tri () const { return _t; } /*! Returns the referenced triangle */ + SrTri* adt () const { return _t->_side[_v]; } + int tid () const { return _t->id(); } /*! Returns the id of the referenced triangle */ + + SrTravel sym () const; + SrTravel nxt () const { return SrTravel ( _t, (_v+1)%3 ); } + SrTravel pri () const { return SrTravel ( _t, (_v+2)%3 ); } + SrTravel rot () const { return pri().sym(); } + SrTravel ret () const { return sym().nxt(); } + + void mark () const { _t->_mark[_v]=1; } + void unmark () const { _t->_mark[_v]=0; } + bool marked () const { return _t->_mark[_v]? true:false; } + + /*! true if the circle passing trough the triangle's vertices does not contain sym().pri().vtx() */ + bool is_delaunay () const; + + /*! true if the 'outside polygon formed by the two adjacent triangles' is convex, allowing a flip */ + bool is_flippable () const; + + bool operator == ( const SrTravel& tr ) const { return _t==tr._t && _v==tr._v? true:false; } + bool operator != ( const SrTravel& tr ) const { return _t==tr._t && _v==tr._v? false:true; } + void operator = ( const SrTravel& tr ) { _t=tr._t; _v=tr._v; } + }; + + +//============================= SrTriangulation =================================== + +/*! This class constructs and maintains a Delaunay triangulation of given 2D points. */ +class SrTriangulation + { private : + SrArray _vtx; + SrArray _tri; + SrArray _stack; + + public : + SrTriangulation (); + ~SrTriangulation (); + + const SrArray& vertices() const { return _vtx; } + const SrArray& triangles() const { return _tri; } + + /*! Returns a reference to a temporary internal array that will become unusable + if any other method changing the triangulation is called. + The back edge is not returned in the array. */ + const SrArray& edges(); + + /*! Initializes with the triangulation of the square with lower left corner (ax,ay), + and upper right corner (bx,by). It creates 4 vertices and 4 triangles: the 2 + triangles in the back have the CW orientation, and the two triangles in the + front have CCW orientation. All points inserted after init() will be inserted in + the CCW triangles and must be inside this bounding square. + The 4 vertices created are: 0:(ax,ay), 1:(bx,ay), 2:(bx,by), 3:(ax,by), and the + border travel is specified as the travel referencing vertex 1, in the back + triangle <1,0,2> */ + void init ( double ax, double ay, double bx, double by ); + + /*! The border traverse element in the back triangle 0, 'from vertex 1 to vertex 0'. + This method can only be called after init(). */ + SrTravel border (); + + /*! Sets all markers to 0. Markers can be used from a travel. */ + void reset_markers (); + + /*! tri becomes a travel referencing the new vertex (x,y) */ + void addv ( SrTravel& tri, double x, double y ); + + /*! edge becomes a travel referencing the 'rotated edge' */ + void flip ( SrTravel& edge ); + + /*! recover delaunay criterion by applying flips to adjacent triangles of vtx. + Travel vtx remains adjacent to the original vertex */ + void recover_delaunay ( SrTravel& vtx ); + + enum LocateResult { NotFound, TriangleFound, VertexFound }; + + /*! Returns in s the triangle containing (x,y); if false is returned (x,y) is + already there and the found duplicated vertex is returned in s. + Optional parameter prec specifies precision for considering points equal */ + LocateResult locate_point ( double x, double y, SrTravel& s, double prec=1.0E-5 ); + + /*! Insert (x,y) if not duplicated, and returns the new or existing vertex containing (x,y). + Optional parameter prec specifies precision for considering points equal. + Check the returned vertex id and vertices array for knowing if a new vertex was inserted. + false is returned in case (x,y) is outside the triangulation */ + bool insert_point ( double x, double y, SrTravel& t, double prec=1.0E-5 ); + + /*! Output list of vertices and triangles for data inspection */ + void output ( SrOutput& out ); + }; + +//============================= EOF =================================== + +# endif // SR_TRIANGULATION diff --git a/source/dcdt/se/sr_var.cpp b/source/dcdt/se/sr_var.cpp new file mode 100644 index 0000000000..c07907fd4a --- /dev/null +++ b/source/dcdt/se/sr_var.cpp @@ -0,0 +1,505 @@ +#include "precompiled.h" +# include +# include +# include + +# include "sr_var.h" +# include "sr_output.h" + +//======================================== SrVar ======================================= + +SrVar::SrVar () + { + _name = 0; + _type = 'i'; + } + +SrVar::SrVar ( char type ) + { + _name = 0; + _type = type; + switch ( _type ) + { case 'i': break; + case 'f': break; + case 's': break; + case 'b': break; + default: _type = 'i'; + } + } + +SrVar::SrVar ( const char* name, char type ) + { + _name = 0; + sr_string_set ( _name, name ); + _type = type; + switch ( _type ) + { case 'i': break; + case 'f': break; + case 's': break; + case 'b': break; + default: _type = 'i'; + } + } + +SrVar::SrVar ( const char* name, bool value ) + { + _name = 0; + sr_string_set ( _name, name ); + _type = 'b'; + _data.push().b = value; + } + +SrVar::SrVar ( const char* name, int value ) + { + _name = 0; + sr_string_set ( _name, name ); + _type = 'i'; + _data.push().i = value; + } + +SrVar::SrVar ( const char* name, float value ) + { + _name = 0; + sr_string_set ( _name, name ); + _type = 'f'; + _data.push().f = value; + } + +SrVar::SrVar ( const char* name, const char* value ) + { + _name = 0; + sr_string_set ( _name, name ); + _type = 's'; + _data.push().s = 0; + sr_string_set ( _data[0].s, value ); + } + +SrVar::SrVar ( const SrVar& v ) + { + _type = 'i'; + _name = 0; + init ( v ); + } + +SrVar::~SrVar () + { + name ( 0 ); + init ( 'i' ); + } + +void SrVar::name ( const char* n ) + { + sr_string_set ( _name, n ); + } + +const char* SrVar::name () const + { + return _name? _name:""; + } + +void SrVar::init ( char type ) + { + if ( _type=='s' ) // delete all used strings + { while ( _data.size() ) delete[] _data.pop().s; + } + + _data.size ( 0 ); + _type = type; + + switch ( _type ) + { case 'i': break; + case 'f': break; + case 's': break; + case 'b': break; + default: _type = 'i'; + } + } + +void SrVar::init ( const SrVar& v ) + { + name ( v.name() ); + init ( v.type() ); + + if ( _type!='s' ) + { _data = v._data; + } + else + { int i; + _data.size ( v.size() ); + for ( i=0; i<_data.size(); i++ ) + { _data[i].s = 0; + sr_string_set ( _data[i].s, v._data[i].s ); + } + } + } + +void SrVar::size ( int ns ) + { + int i, s = _data.size(); + + if ( nss ) + { _data.size ( ns ); + for ( i=s; i=_data.size() ) + { _data.push().b = b; } + else + { _data[index].b = b; } + break; + + case 'i': set ( (int)b, index ); + case 'f': set ( (float)b, index ); + case 's': set ( b? "true":"false", index ); break; + } + } + +void SrVar::set ( int i, int index ) + { + switch ( _type ) + { case 'i': + if ( index<0 || index>=_data.size() ) + { _data.push().i = i; } + else + { _data[index].i = i; } + break; + + case 'f': set ( float(i), index ); + case 'b': set ( bool(i? true:false), index ); + case 's': { SrString s; s<=_data.size() ) + { _data.push().f = f; } + else + { _data[index].f = f; } + break; + + case 'i': set ( int(f), index ); + case 'b': set ( bool(f==1.0f? true:false), index ); + case 's': { SrString s; s<=_data.size() ) + { _data.push().s = 0; + index = _data.size()-1; + } + sr_string_set ( _data[index].s, s ); + break; + + case 'i': set ( atoi(s), index ); + case 'f': set ( atof(s), index ); + case 'b': set ( bool(sr_compare(s,"true")==0? true:false), index ); + } + } + +void SrVar::push ( bool b ) + { + SR_ASSERT ( _type=='b' ); + _data.push().b = b; + } + +void SrVar::push ( int i ) + { + SR_ASSERT ( _type=='i' ); + _data.push().i = i; + } + +void SrVar::push ( float f ) + { + SR_ASSERT ( _type=='f' ); + _data.push().f = f; + } + +void SrVar::push ( const char* s ) + { + SR_ASSERT ( _type=='s' ); + + _data.push().s = 0; + sr_string_set ( _data.top().s, s ); + } + +//--------------------------- get value --------------------------- + +bool SrVar::getb ( int index ) const + { + if ( index<0 || index>=_data.size() ) return 0; + switch ( _type ) + { case 's': if ( !_data[index].s ) return false; + if ( sr_compare(_data[index].s,"true")==0 ) return true; + return false; + case 'i': return _data[index].i? true:false; + case 'f': return _data[index].f==0? false:true; + case 'b': return _data[index].b; + } + return false; + } + +int SrVar::geti ( int index ) const + { + if ( index<0 || index>=_data.size() ) return 0; + switch ( _type ) + { case 's': return _data[index].s? atoi(_data[index].s):0; + case 'i': return _data[index].i; + case 'f': return (int)_data[index].f; + case 'b': return (int)_data[index].b; + } + return 0; + } + +float SrVar::getf ( int index ) const + { + if ( index<0 || index>=_data.size() ) return 0; + switch ( _type ) + { case 's': return _data[index].s? (float)atof(_data[index].s):0; + case 'i': return (float)_data[index].i; + case 'f': return _data[index].f; + case 'b': return (float)_data[index].b; + } + return 0; + } + +const char* SrVar::gets ( int index ) const + { + if ( index<0 || index>=_data.size() ) return 0; + if ( _type=='s' && _data[index].s ) + return _data[index].s; + return ""; + } + +//----------------------------------------------------------------- + +void SrVar::remove ( int i, int n ) + { + if ( n<=0 || i<0 || i>=_data.size() ) return; + if ( _type=='s' ) + { int pi, pe=i+n; + for ( pi=i; pi<=pe && pi<_data.size(); pi++ ) + delete[] _data[pi].s; + } + _data.remove ( i, n ); + } + +void SrVar::insert ( int i, int n ) + { + if ( n<=0 || i<0 || i>_data.size() ) return; + _data.insert ( i, n ); + while ( i16 ) { o<20 ) { o<80 ) { o<80 ) { o<> ( SrInput& in, SrVar& v ) + { + SrString buf; + + while ( true ) + { in.get_token(); + if ( in.finished() ) return in; + if ( in.last_token()[0] == '=' ) break; + buf << in.last_token(); + } + + v.name ( buf ); + v.init ( 'i' ); + + bool negative = false; + + in.get_token(); + SrString& tok = in.last_token(); + + if ( in.last_token().len()==0 ) return in; + + // get first token and determine SrVar type: + switch ( in.last_token_type() ) + { case SrInput::Name : + if ( sr_compare(tok,"true")==0 ) + { v.init('b'); v.set(true); break; } + else if ( sr_compare(tok,"false")==0 ) + { v.init('b'); v.set(false); break; } + else + { // this will be a string, let enter the String case + } + + case SrInput::String : + v.init ('s'); + v.set ( (const char*)tok ); + break; + + case SrInput::Delimiter : // can be int or float type + if ( tok[0]=='-' || tok[0]=='+' ) + { in.unget_token(); + SrString& s=in.getn(); + if ( in.last_token_type()==SrInput::Integer ) + { v.init('i'); v.set(s.atoi()); } + else if ( in.last_token_type()==SrInput::Real ) + { v.init('f'); v.set(s.atof()); } + break; + } + case SrInput::Integer : + v.init ('i'); + v.set (tok.atoi()); + break; + case SrInput::Real : + v.init ('f'); + v.set (tok.atof()); + break; + default : break; + }; + + // read while ';' is found: + in.get_token (); + if ( in.last_token().len()==0 ) return in; + while ( in.last_token()[0]!=';' ) + { + switch ( v._type ) + { case 'f': in.unget_token(); + v.push ( in.getn().atof() ); + break; + case 'i': in.unget_token(); + v.push ( in.getn().atoi() ); + break; + case 's': v.push ( in.last_token() ); + break; + case 'b': if ( sr_compare(in.last_token(),"true")==0 ) + v.push(true); + else + v.push(false); + break; + } + in.get_token(); + if ( in.last_token().len()==0 ) return in; + } + + return in; + } + +int sr_compare ( const SrVar* v1, const SrVar* v2 ) + { + return sr_compare ( v1->name(), v2->name() ); + +/* + int cmp = v1->type()-v2->type(); + if ( cmp!=0 ) return cmp; + + switch ( v1->type() ) + { case 'f': cmp = SR_COMPARE ( v1->getf(), v2->getf() ); break; + case 'i': cmp = SR_COMPARE ( v1->geti(), v2->geti() ); break; + case 's': cmp = sr_compare ( v1->gets(), v2->gets() ); break; + case 'b': cmp = SR_COMPARE ( v1->getb(), v2->getb() ); break; + } + + return cmp; +*/ + } + +//================================ End of File ================================================= diff --git a/source/dcdt/se/sr_var.h b/source/dcdt/se/sr_var.h new file mode 100644 index 0000000000..3d36ec471d --- /dev/null +++ b/source/dcdt/se/sr_var.h @@ -0,0 +1,145 @@ + +# ifndef SR_VAR_H +# define SR_VAR_H + +/** \file sr_var.h + * a multi type variable + */ + +# include "sr.h" +# include "sr_array.h" + +/*! \class SrVar sr_var.h + \brief generic type variable + + SrVar keeps generic type and multi dimension variable values. */ +class SrVar + { private : + union Var { bool b; int i; float f; char* s; }; + + SrArray _data; // keeps elements + char* _name; // contains name, if used + char _type; // type 'b', 'i', 'f', or 's' + + public : + + /*! Constructs an empty SrVar as type 'i' and name "" */ + SrVar (); + + /*! Constructs an empty SrVar as the given type and name "". + If the given type is unknown, type 'i' is set. */ + SrVar ( char type ); + + /*! Constructs an empty SrVar as the given type and name. + If the given type is unknown, type 'i' is set. */ + SrVar ( const char* name, char type ); + + /*! Constructs a type 'b' SrVar with the given name and value */ + SrVar ( const char* name, bool value ); + + /*! Constructs a type 'i' SrVar with the given name and value */ + SrVar ( const char* name, int value ); + + /*! Constructs a type 'f' SrVar with the given name and float value */ + SrVar ( const char* name, float value ); + + /*! Constructs a type 'f' SrVar with the given name and double value */ + SrVar ( const char* name, double value ) { SrVar::SrVar ( name, float(value) ); } + + /*! Constructs a type 's' SrVar with the given name and value */ + SrVar ( const char* name, const char* value ); + + /*! Copy constructor */ + SrVar ( const SrVar& v ); + + /*! Deletes all used internal data */ + ~SrVar (); + + /*! Sets a name to be kept by SrVar. Null can be passed to clear the name. */ + void name ( const char* n ); + + /*! Returns the associated name. "" is returned if no name was set. */ + const char* name () const; + + /*! Inits SrVar with the given type. Types are specified by the first + letter of: bool, int, float, or string. + This method will not free extra spaces existing in internal arrays. + For such purpose, compress() should be used. + SrVar values can then be defined with one of the set methods. + Note: if a non recognized type is given SrVar is set to type 'i'. */ + void init ( char type ); + + /*! Makes SrVar be identical to v, same as operator= */ + void init ( const SrVar& v ); + + /*! Frees any extra spaces existing in internal arrays */ + void compress () { _data.compress(); } + + /*! Returns the actual array size of the variable */ + int size () const { return _data.size(); } + + /*! Set a new array size */ + void size ( int ns ); + + /*! Returns the letter corresponding to the type of the variable, which + is one of: 'b', 'i', 'f' or 's' */ + char type () const { return _type; } + + /*! Methods set() are used to update a value or to push a new value in SrVar. + Parameter index gives the position to update the value and has + the default value of 0. + If the index is out of range, the value is pushed after + the last current value, increasing the array size of SrVar. + Automatic type casts are performed in case of type mixing */ + void set ( bool b, int index=0 ); + void set ( int i, int index=0 ); + void set ( float f, int index=0 ); + void set ( double d, int index=0 ) { set(float(d),index); } + void set ( const char* s, int index=0 ); + + /*! Methods push() are used to push a new value in SrVar. */ + void push ( bool b ); + void push ( int i ); + void push ( float f ); + void push ( double d ) { push(float(d)); } + void push ( const char* s ); + + /*! Methods get() are used to get a value from SrVar. + Parameter index gives the position to retrieve the value and has + the default value of 0. + If the index is out of range, zero is returned ("" for gets() ) */ + bool getb ( int index=0 ) const; + int geti ( int index=0 ) const; + float getf ( int index=0 ) const; + const char* gets ( int index=0 ) const; + + /*! Starting at position i (included), this method removes n positions. + Parameter n has default value of 1 */ + void remove ( int i, int n=1 ); + + /*! Insert at position i, n positions. + Parameter n has default value of 1 */ + void insert ( int i, int n=1 ); + + /*! Makes SrVar be identical to v, same as init(v) */ + SrVar& operator= ( const SrVar& v ); + + /*! Outputs SrVar. + If SrVar has no name, name "var" is used. If SrVar is empty, + a 0 value ("" for string type) is used. */ + friend SrOutput& operator<< ( SrOutput& o, const SrVar& v ); + + /*! Input SrVar. The type of v will change according to the first element + found in the input after the '=' sign. For example, the input "point=1.0 2;" + will set v with name 'point', type float, and values 1.0f and 2.0f */ + friend SrInput& operator>> ( SrInput& in, SrVar& v ); + + /*! C style comparison function compares the names of variables, + returning <0, 0, or >0. */ + friend int sr_compare ( const SrVar* v1, const SrVar* v2 ); + }; + +//================================ End of File ================================================= + +# endif // SR_VAR_H + diff --git a/source/dcdt/se/sr_var_table.cpp b/source/dcdt/se/sr_var_table.cpp new file mode 100644 index 0000000000..b61447ae48 --- /dev/null +++ b/source/dcdt/se/sr_var_table.cpp @@ -0,0 +1,182 @@ +#include "precompiled.h" +# include +# include "sr_var_table.h" + +//====================================== SrVarTable ======================================= + +SrVarTable::SrVarTable ( const SrVarTable& vt ) + { + _table.size ( vt.size() ); + int i; + for ( i=0; i<_table.size(); i++ ) + _table[i] = new SrVar ( vt[i] ); + + _name = 0; + sr_string_set ( _name, vt._name ); + } + +SrVarTable::~SrVarTable () + { + delete _name; + init(); + } + +void SrVarTable::name ( const char* n ) + { + sr_string_set ( _name, n ); + } + +const char* SrVarTable::name () const + { + return _name? _name:""; + } + +void SrVarTable::init () + { + while ( _table.size()>0 ) + delete _table.pop(); + } + +SrVar* SrVarTable::get ( const char* name ) const + { + int i = search ( name ); + return i<0? 0 : _table[i]; + } + +SrVar* SrVarTable::set ( const char* name, int value, int index ) + { + int i = search ( name ); + if ( i<0 ) return 0; + _table[i]->set(value,index); + return _table[i]; + } + +SrVar* SrVarTable::set ( const char* name, bool value, int index ) + { + int i = search ( name ); + if ( i<0 ) return 0; + _table[i]->set(value,index); + return _table[i]; + } + +SrVar* SrVarTable::set ( const char* name, float value, int index ) + { + int i = search ( name ); + if ( i<0 ) return 0; + _table[i]->set(value,index); + return _table[i]; + } + +SrVar* SrVarTable::set ( const char* name, const char* value, int index ) + { + int i = search ( name ); + if ( i<0 ) return 0; + _table[i]->set(value,index); + return _table[i]; + } + +static int fcmp ( SrVar*const* pt1, SrVar*const* pt2 ) + { + return sr_compare ( *pt1, *pt2 ); + } + +int SrVarTable::search ( const char* name ) const + { + SrVar v; + v.name ( name ); + return _table.bsearch ( &v, fcmp ); + } + +int SrVarTable::insert ( SrVar* v ) + { + int pos; + pos = _table.insort ( v, fcmp, true ); + return pos; + } + +void SrVarTable::remove ( int i ) + { + if ( i<0 || i>=_table.size() ) return; + delete _table[i]; + _table.remove ( i ); + } + +void SrVarTable::merge ( SrVarTable& vt ) + { + int i, id; + + for ( i=0; i=0 ) // found with same name + { get(id).init ( vt.get(i) ); // get the value + delete vt._table[i]; + vt._table[i] = 0; + } + } + + for ( i=0; i=_table.size() ) return 0; + SrVar* v = _table[i]; + _table.remove ( i ); + return v; + } + +SrOutput& operator<< ( SrOutput& o, const SrVarTable& vt ) + { + int i; + SrString buf; + + if ( vt.name()[0] ) + { buf.make_valid_string ( vt.name() ); + o << buf << srspc; + } + + o << '[' << srnl; + + for ( i=0; i> ( SrInput& in, SrVarTable& vt ) + { + vt.init (); + vt.name ( 0 ); + + in.get_token(); // name or '[' + + if ( in.last_token()[0]!='[' ) + { vt.name ( in.last_token() ); + in.get_token (); // now it should come '[' + } + + while ( true ) + { in.get_token (); + if ( in.last_token()[0]==']' ) break; + in.unget_token (); + vt._table.push () = new SrVar; + in >> *vt._table.top(); + } + + // sort elements in case data was edited by hand + vt._table.sort ( fcmp ); + + return in; + } + + +//================================ End of File ================================================= + + diff --git a/source/dcdt/se/sr_var_table.h b/source/dcdt/se/sr_var_table.h new file mode 100644 index 0000000000..270315d455 --- /dev/null +++ b/source/dcdt/se/sr_var_table.h @@ -0,0 +1,116 @@ + +# ifndef SR_VAR_TABLE_H +# define SR_VAR_TABLE_H + +/** \file sr_var_table.h + * a table of SrVars + */ + +# include "sr.h" +# include "sr_var.h" +# include "sr_shared_class.h" + +/*! \class SrVarTable sr_var_table.h + \brief a table of generic type variables + + SrVarTable has methods to manage a table of variables, + which is always kept sorted by name. */ +class SrVarTable : public SrSharedClass + { private : + SrArray _table; + char* _name; + public : + + /*! Constructor */ + SrVarTable () { _name=0; } + + /*! Copy constructor */ + SrVarTable ( const SrVarTable& vt ); + + /*! Destructor */ + ~SrVarTable (); + + /*! Sets a name for the var table. Null can be passed to clear the name. */ + void name ( const char* n ); + + /*! Returns the associated name. "" is returned if no name was set. */ + const char* name () const; + + /*! Clear the table */ + void init (); + + /*! Compress free spaces */ + void compress () { _table.compress(); } + + /*! Returns size */ + int size () const { return _table.size(); } + + /*! Get a const reference to a var from an index, which must be in valid range */ + const SrVar& const_get ( int i ) const { return *_table[i]; } + + /*! Get a reference to a var from an index, which must be in valid range */ + SrVar& get ( int i ) const { return *_table[i]; } + + /* Does a binary search for the name, and returns a pointer to the found var. + If not found, null is returned. */ + SrVar* get ( const char* name ) const; + + /* Does a binary search for the name, and returns the value if the name was + found. If not found, 0 or "" is returned. */ + int geti ( const char* name ) const { SrVar* v=get(name); return v? v->geti():0; } + bool getb ( const char* name ) const { SrVar* v=get(name); return v? v->getb():0; } + float getf ( const char* name ) const { SrVar* v=get(name); return v? v->getf():0; } + const char* gets ( const char* name ) const { SrVar* v=get(name); return v? v->gets():""; } + + /* Does a binary search for the name and set the value if the var is found. + Returns a pointer to the found var or null if no var was found. + Automatic type casts are performed if required. */ + SrVar* set ( const char* name, int value, int index=0 ); + SrVar* set ( const char* name, bool value, int index=0 ); + SrVar* set ( const char* name, float value, int index=0 ); + SrVar* set ( const char* name, const char* value, int index=0 ); + SrVar* set ( const char* name, double value, int index=0 ) + { return set(name,float(value),index); } + + /*! Access operator. The index must be in a valid range */ + SrVar& operator [] (int i ) const { return get(i); } + + /* Does a binary search and returns the index of the + found variable. Returns -1 if not found. */ + int search ( const char* name ) const; + + /*! Insert v in the table. Variable v should be created with operator + new, and will be managed by the table after insertion. + Returns the allocated position of v */ + int insert ( SrVar* v ); + + /* Allocated a new SrVar and insert it to the table with method insert(). */ + int insert ( const char* name, int value ) { return insert(new SrVar(name,value)); } + int insert ( const char* name, bool value ) { return insert(new SrVar(name,value)); } + int insert ( const char* name, float value ) { return insert(new SrVar(name,value)); } + int insert ( const char* name, double value ) { return insert(new SrVar(name,float(value))); } + int insert ( const char* name, const char* value ) { return insert(new SrVar(name,value)); } + + /*! Remove variable with index i. If i is out of range nothing is done */ + void remove ( int i ); + + /*! Take the vars of vt. If duplicated names are found, the values found + in vt takes place. Var table vt becomes empty. */ + void merge ( SrVarTable& vt ); + + /*! Extract the variable with index i from the table. The user becomes + responsible of managing the deletion of the returned pointer. + If i is out of range, 0 is returned. */ + SrVar* extract ( int i ); + + /*! Output the SrVarTable (between delimiters [ ... ]) */ + friend SrOutput& operator<< ( SrOutput& o, const SrVarTable& v ); + + /*! Input a SrVarTable */ + friend SrInput& operator>> ( SrInput& in, SrVarTable& v ); + }; + +//================================ End of File ================================================= + +# endif // SR_VAR_TABLE_H + diff --git a/source/dcdt/se/sr_var_table_window.h b/source/dcdt/se/sr_var_table_window.h new file mode 100644 index 0000000000..8056cb5d1d --- /dev/null +++ b/source/dcdt/se/sr_var_table_window.h @@ -0,0 +1,52 @@ + +# ifndef SR_VAR_TABLE_WINDOW_H +# define SR_VAR_TABLE_WINDOW_H + +/** \file sr_var_table_window.h + * Fltk window for SrVarTable edition. */ + +class Fl_Window; +class Fl_Browser; +class SrVTWin; +class SrVarTable; + +/*! \class SrVarTableWindow sr_var_table_window.h + \brief Fltk window for SrVarTable edition. */ +class SrVarTableWindow + { private : + SrVTWin* _ui; + + public : + + /*! Constructor */ + SrVarTableWindow (); + + /*! Destructor */ + ~SrVarTableWindow (); + + /*! Set window title */ + void title ( const char* s ); + + /*! Access to the fltk window */ + Fl_Window* window (); + + /*! Calls window()->show() */ + void show (); + + /*! Start using the given vt, sharing it (vt can be null). */ + void use_var_table ( SrVarTable* vt ); + + /*! Update all the user interface */ + void update (); + + /* Called when the window closes */ + virtual void window_closed (); + + /* Called each time a var (of index i) is changed */ + virtual void value_modified ( int i ) {}; + }; + +//================================ End of File ================================================= + +# endif // SR_VAR_TABLE_WINDOW_H + diff --git a/source/dcdt/se/sr_vec.cpp b/source/dcdt/se/sr_vec.cpp new file mode 100644 index 0000000000..d0dda80972 --- /dev/null +++ b/source/dcdt/se/sr_vec.cpp @@ -0,0 +1,203 @@ +#include "precompiled.h" +# include +# include "sr_vec.h" +# include "sr_vec2.h" + +//=========================================================================== + +const SrVec SrVec::null(0,0,0); +const SrVec SrVec::minusone(-1.0f,-1.0f,-1.0f); +const SrVec SrVec::one(1.0f,1.0f,1.0f); +const SrVec SrVec::i(1.0f,0,0); +const SrVec SrVec::j(0,1.0f,0); +const SrVec SrVec::k(0,0,1.0f); + +//============================== SrVec ==================================== + +SrVec::SrVec ( const SrVec2& v ) + { + x = v.x; + y = v.y; + z = 0; + } + +void SrVec::set ( const SrVec2& v ) + { + x = v.x; + y = v.y; + z = 0; + } + +void SrVec::rotx ( float sa, float ca ) + { + set ( x, y*ca-z*sa, y*sa+z*ca ); + } + +void SrVec::roty ( float sa, float ca ) + { + set ( x*ca+z*sa, y, -x*sa+z*ca ); + } + +void SrVec::rotz ( float sa, float ca ) + { + set ( x*ca-y*sa, x*sa+y*ca, z ); + } + +void SrVec::rotx ( float radians ) + { + rotx ( sinf(radians), cosf(radians) ); + } + +void SrVec::roty ( float radians ) + { + roty ( sinf(radians), cosf(radians) ); + } + +void SrVec::rotz ( float radians ) + { + rotz ( sinf(radians), cosf(radians) ); + } + +void SrVec::abs () + { + x=SR_ABS(x); y=SR_ABS(y); z=SR_ABS(z); + } + +void SrVec::normalize () + { + float f = sqrtf(x*x + y*y + z*z); + if ( f>0 ) { x/=f; y/=f; z/=f; } + } + +float SrVec::len ( float n ) + { + float f = sqrtf (x*x + y*y + z*z); + if ( f>0 ) { n/=f; x*=n; y*=n; z*=n; } + return f; + } + +float SrVec::norm () const + { + return sqrtf ( x*x + y*y + z*z ); + } + +float SrVec::norm2 () const + { + return x*x + y*y + z*z; + } + +float SrVec::norm_max () const + { + float a = SR_ABS(x); + float b = SR_ABS(y); + float c = SR_ABS(z); + return SR_MAX3 ( a, b, c ); + } + +//=================================== Friend Functions =================================== + +void swap ( SrVec& v1, SrVec& v2 ) + { + float tmp; + SR_SWAP(v1.x,v2.x); + SR_SWAP(v1.y,v2.y); + SR_SWAP(v1.z,v2.z); + } + +float dist_max ( const SrVec& v1, const SrVec& v2 ) + { + float a = v1.x-v2.x; + float b = v1.y-v2.y; + float c = v1.z-v2.z; + a = SR_ABS(a); + b = SR_ABS(b); + c = SR_ABS(c); + return a>b? (a>c? a:c) : (b>c? b:c); + } + +float dist ( const SrVec& v1, const SrVec& v2 ) + { + float dx, dy, dz; + + dx=v1.x-v2.x; dy=v1.y-v2.y; dz=v1.z-v2.z; + + return sqrtf (dx*dx + dy*dy + dz*dz); + } + +float dist2 ( const SrVec& v1, const SrVec& v2 ) + { + float dx, dy, dz; + dx=v1.x-v2.x; dy=v1.y-v2.y; dz=v1.z-v2.z; + return dx*dx + dy*dy + dz*dz; + } + +float angle ( const SrVec& v1, const SrVec& v2 ) + { + return acosf ( dot(v1,v2)/(v1.norm()*v2.norm()) ); + } + +float angle_fornormvecs ( const SrVec& v1, const SrVec& v2 ) + { + return acosf ( dot(v1,v2) ); + } + +SrVec cross ( const SrVec& v1, const SrVec& v2 ) + { + return SrVec ( v1.y*v2.z - v1.z*v2.y, + v1.z*v2.x - v1.x*v2.z, + v1.x*v2.y - v1.y*v2.x ); + } + +void SrVec::cross ( const SrVec& v1, const SrVec& v2 ) + { + x = v1.y*v2.z - v1.z*v2.y; + y = v1.z*v2.x - v1.x*v2.z; + z = v1.x*v2.y - v1.y*v2.x; + } + +float dot ( const SrVec& v1, const SrVec& v2 ) + { + return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z; + } + +SrVec triangle_normal ( const SrPnt& a, const SrPnt& b, const SrPnt& c ) + { + SrVec n; + n.cross ( b-a, c-a ); + n.normalize(); + return n; + } + +int compare ( const SrVec& v1, const SrVec& v2 ) + { + if ( v1.x > v2.x ) return 1; + if ( v1.x < v2.x ) return -1; + if ( v1.y > v2.y ) return 1; + if ( v1.y < v2.y ) return -1; + if ( v1.z > v2.z ) return 1; + if ( v1.z < v2.z ) return -1; + return 0; + } + +int compare ( const SrVec* v1, const SrVec* v2 ) + { + return compare ( *v1, *v2 ); + } + +SrVec lerp ( const SrVec& v1, const SrVec& v2, float t ) + { + return v1*(((float)1.0)-t) + v2*t; + } + +SrOutput& operator<< ( SrOutput& o, const SrVec& v ) + { + return o << v.x <<' '<< v.y <<' '<< v.z; + } + +SrInput& operator>> ( SrInput& in, SrVec& v ) + { + return in >> v.x >> v.y >> v.z; + } + +//================================== End of File =========================================== + diff --git a/source/dcdt/se/sr_vec.h b/source/dcdt/se/sr_vec.h new file mode 100644 index 0000000000..eff309efa4 --- /dev/null +++ b/source/dcdt/se/sr_vec.h @@ -0,0 +1,259 @@ + +/** \file sr_vec.h + * Three dimensional vector */ + +# ifndef SR_VEC_H +# define SR_VEC_H + +# include "sr_input.h" +# include "sr_output.h" + +class SrVec; +class SrVec2; + +/*! This "point typedef" is used mainly to coherently describe variables, + making possible to distiguish points from vectors parameters. */ +typedef SrVec SrPnt; + +/*! \class SrVec sr_vec.h + \brief Three dimensional vector. + + Its three coordinates are of type float, and can be accessed with the + provided methods or as a float array, to be compatible with other libraries. + Access as a float array is done with an overloaded typecast, that should + correctly work with most compilers. */ +class SrVec + { public : + float x, y, z; + public : + static const SrVec null; //!< (0,0,0) null vector + static const SrVec one; //!< (1,1,1) vector + static const SrVec minusone; //!< (-1,-1,-1) vector + static const SrVec i; //!< (1,0,0) vector + static const SrVec j; //!< (0,1,0) vector + static const SrVec k; //!< (0,0,1) vector + public : + + /*! Initializes SrVec as a null vector. Implemented inline. */ + SrVec () : x(0), y(0), z(0) {} + + /*! Initializes SrVec from an int pointer. Implemented inline. */ + SrVec ( const int *v ) : x((float)v[0]), y((float)v[1]), z((float)v[2]) {} + + /*! Initializes from a float pointer. Implemented inline. */ + SrVec ( const float *v ) : x((float)v[0]), y((float)v[1]), z((float)v[2]) {} + + /*! Initializes from a double pointer. Implemented inline. */ + SrVec ( const double *v ) : x((float)v[0]), y((float)v[1]), z((float)v[2]) {} + + /*! Copy constructor. Implemented inline. */ + SrVec ( const SrVec& v ) : x(v.x), y(v.y), z(v.z) {} + + /*! Initializes from a 2d vector: the z coordinate is set to zero. */ + SrVec ( const SrVec2& v ); + + /*! Initializes with the three given float coordinates. Implemented inline. */ + SrVec ( float a, float b, float c ) : x(a), y(b), z(c) {} + + /*! Sets coordinates from an int pointer. Implemented inline. */ + void set ( const int *v ) { x=(float)v[0]; y=(float)v[1]; z=(float)v[2]; } + + /*! Sets coordinates from a float pointer. Implemented inline. */ + void set ( const float *v ) { x=(float)v[0]; y=(float)v[1]; z=(float)v[2]; } + + /*! Sets coordinates from a double pointer. Implemented inline. */ + void set ( const double *v ) { x=(float)v[0]; y=(float)v[1]; z=(float)v[2]; } + + /*! Sets coordinates from the three given float values. Implemented inline. */ + void set ( float a, float b, float c ) { x=a; y=b; z=c; } + + /*! Sets coordinates from another SrVec. Implemented inline. */ + void set ( const SrVec& v ) { x=v.x; y=v.y; z=v.z; } + + /*! Sets coordinates from a 2d vector: the z coordinate is set to zero. */ + void set ( const SrVec2& v ); + + /*! Put in f the coordinate values. */ + void get ( float f[3] ) const { f[0]=x; f[1]=y; f[2]=z; } + + /*! Apply a rotation to SrVec around the X axis with + the angle 'a' where sa=sin(a) and ca=cos(a). */ + void rotx ( float sa, float ca ); + + /*! Apply a rotation to SrVec around the Y axis with + the angle 'a' where sa=sin(a) and ca=cos(a). */ + void roty ( float sa, float ca ); + + /*! Apply a rotation to SrVec around the Z axis with + the angle 'a' where sa=sin(a) and ca=cos(a). */ + void rotz ( float sa, float ca ); + + /*! Same as the other rotx() method, but with one parameter + angle in radians. */ + void rotx ( float radians ); + + /*! Same as the other roty() method, but with one parameter + angle in radians. */ + void roty ( float radians ); + + /*! Same as the other rotz() method, but with one parameter + angle in radians. */ + void rotz ( float radians ); + + /*! Makes each coordinate to have its absolute value. */ + void abs (); + + /*! Normalizes the vector. If its norm is 0, the vector is not changed. */ + void normalize (); + + /*! Sets the length to be n and returns the previous length */ + float len ( float n ); + + /*! Returns the length of the vector. Equivalent to norm() */ + float len () { return norm(); } + + /*! Returns the norm (distance to zero) of the vector. If the vector is + null, zero is returned. */ + float norm () const; + + /*! Returns the square of the norm (x*x + y*y + z*z). */ + float norm2 () const; + + /*! Returns the norm, but using the max function of distance, which does not require sqrt() */ + float norm_max () const; + + /*! Returns a 2d orthogonal vector to the 2d vector(x,y) that is (-y,x) + The z coordinate of SrVec is not considered. Implemented inline. */ + SrVec ortho2d () const { return SrVec(-y,x,0); } + + /*! Returns true if all coordinates are zero; false otherwise. */ + bool iszero () const { return x==0.0 && y==0.0 && z==0.0? true:false; } + + /*! Returns true if the vector is inside the closed neighboorhood of point 0,0,0 and radius ds. */ + bool nextzero ( float ds ) const { return norm2()<=ds*ds? true:false; } + + /*! Allows member access like a vector. */ + float& operator[] ( int i ) { return *((&x)+i); } + + /*! Convertion to a float pointer. */ + operator float* () const { return (float*)&x; } + + /*! Copy operator from another SrVec. Implemented inline. */ + void operator = ( const SrVec& v ) { set(v); } + + /*! Copy operator from a 2d vector: the z coordinate is set to zero. */ + void operator = ( const SrVec2& v ) { set(v); } + + /*! Copy operator from an int pointer. Implemented inline. */ + void operator = ( const int *v ) { set(v); } + + /*! Copy operator from a float pointer. Implemented inline. */ + void operator = ( const float *v ) { set(v); } + + /*! Copy operator from a double pointer. Implemented inline. */ + void operator = ( const double *v ) { set(v); } + + /*! Operator-=() with another SrVec. Implemented inline. */ + void operator -= ( const SrVec& v ) { x-=v.x; y-=v.y; z-=v.z; } + + /*! Operator+=() with another SrVec. Implemented inline. */ + void operator += ( const SrVec& v ) { x+=v.x; y+=v.y; z+=v.z; } + + /*! Operator*=() with a float. Implemented inline. */ + void operator *= ( float r ) { x*=r; y*=r; z*=r; } + + /*! Operator/=() with a float. Implemented inline. */ + void operator /= ( float r ) { x/=r; y/=r; z/=r; } + + /*! Unary minus. Implemented inline. */ + friend SrVec operator - ( const SrVec& v ) + { return SrVec ( -v.x, -v.y, -v.z ); } + + /*! Adds two vectors. Implemented inline. */ + friend SrVec operator + ( const SrVec& v1, const SrVec& v2 ) + { return SrVec (v1.x+v2.x, v1.y+v2.y, v1.z+v2.z); } + + /*! Subtracts two vectors. Implemented inline. */ + friend SrVec operator - ( const SrVec& v1, const SrVec& v2 ) + { return SrVec (v1.x-v2.x, v1.y-v2.y, v1.z-v2.z); } + + /*! Comparison operator. Implemented inline. */ + friend bool operator == ( const SrVec& v1, const SrVec& v2 ) + { return v1.x==v2.x && v1.y==v2.y && v1.z==v2.z? true:false; } + + /*! Comparison operator. Implemented inline. */ + friend bool operator != ( const SrVec& v1, const SrVec& v2 ) + { return v1.x!=v2.x || v1.y!=v2.y || v1.z!=v2.z? true:false; } + + /*! Pos multiplication by scalar. Implemented inline. */ + friend SrVec operator * ( const SrVec& v, float r ) + { return SrVec (v.x*r, v.y*r, v.z*r); } + + /*! Pre multiplication by scalar. Implemented inline. */ + friend SrVec operator * ( float r, const SrVec& v ) + { return SrVec (v.x*r, v.y*r, v.z*r); } + + /*! Division by scalar. Implemented inline. */ + friend SrVec operator / ( const SrVec& v, float r ) + { return SrVec (v.x/r, v.y/r, v.z/r); } + + /*! Swaps the contents of v1 with v2. */ + friend void swap ( SrVec& v1, SrVec& v2 ); + + /*! Returns true if dist(v1,v2)<=ds, otherwise returns false. Implemented inline. */ + friend bool next ( const SrVec& v1, const SrVec& v2, float ds ) + { return dist2(v1,v2)<=ds*ds? true:false; } + + /*! Returns the distance between v1 and v2 using norm_max(). */ + friend float dist_max ( const SrVec& v1, const SrVec& v2 ); + + /*! Returns the distance between v1 and v2. */ + friend float dist ( const SrVec& v1, const SrVec& v2 ); + + /*! Returns the square of the distance between v1 and v2. */ + friend float dist2 ( const SrVec& v1, const SrVec& v2 ); + + /*! Returns the angle between v1 and v2, that is inside the interval [0,pi], + using acosf. The returned angle is in radians. */ + friend float angle ( const SrVec& v1, const SrVec& v2 ); + + /*! Returns the angle between v1 and v2, but considering that + v1 and v2 are already normalized, ie, computes acosf(dot(v1,v2)). */ + friend float angle_fornormvecs ( const SrVec& v1, const SrVec& v2 ); + + /*! Returns the cross product of v1 and v2. */ + friend SrVec cross ( const SrVec& v1, const SrVec& v2 ); + + /*! Inline version of cross(), but as a member. + Attention: v1 or v2 cannot be the same as SrVec. */ + void cross ( const SrVec& v1, const SrVec& v2 ); + + /*! Returns the dot product of v1 and v2 (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z). + Its interpretation is the length of the projection of v1 onto v2 + ( v1.v2 = |v1||v2|cos(ang) ). */ + friend float dot ( const SrVec& v1, const SrVec& v2 ); + + /* Returns the normalized normal of triangle (a,b,c) */ + friend SrVec triangle_normal ( const SrPnt& a, const SrPnt& b, const SrPnt& c ); + + /*! Returns -1,0,1 depending if v1 is less, equal or greater than v2, + using a comparison by coordinates. Parameters are passed by reference. */ + friend int compare ( const SrVec& v1, const SrVec& v2 ); + + /*! Returns -1,0,1 depending if v1 is less, equal or greater than v2, + using a comparison by coordinates. Parameters are passed by pointers. */ + friend int compare ( const SrVec* v1, const SrVec* v2 ); + + /*! Returns the linear interpolation of v1 v2 in parameter t ( v1(1-t)+v2(t) ). */ + friend SrVec lerp ( const SrVec& v1, const SrVec& v2, float t ); + + /*! Outputs in format: "x y z". */ + friend SrOutput& operator<< ( SrOutput& o, const SrVec& v ); + + /*! Inputs from format: "x y z". */ + friend SrInput& operator>> ( SrInput& in, SrVec& v ); + }; + +//============================== end of file =============================== + +# endif // SR_VEC_H diff --git a/source/dcdt/se/sr_vec2.cpp b/source/dcdt/se/sr_vec2.cpp new file mode 100644 index 0000000000..45fb93fbde --- /dev/null +++ b/source/dcdt/se/sr_vec2.cpp @@ -0,0 +1,359 @@ +#include "precompiled.h" +# include +# include "sr_vec2.h" +# include "sr_geo2.h" + +//============================= static data ================================ + +const SrVec2 SrVec2::null(0,0); +const SrVec2 SrVec2::minusone(-1.0f,-1.0f); +const SrVec2 SrVec2::one(1.0f,1.0f); +const SrVec2 SrVec2::i(1.0f,0); +const SrVec2 SrVec2::j(0,1.0f); + +//============================== SrVec2 ==================================== + +void SrVec2::rot ( float sa, float ca ) + { + set ( x*ca-y*sa, x*sa+y*ca ); + } + +void SrVec2::rot ( const SrVec2& cent, float sa, float ca ) + { + x-=cent.x; y-=cent.y; + set ( x*ca-y*sa, x*sa+y*ca ); + x+=cent.x; y+=cent.y; + } + +void SrVec2::rot ( float radians ) + { + rot ( sinf(radians), cosf(radians) ); + } + +void SrVec2::rot ( const SrVec2& cent, float radians ) + { + rot ( cent, sinf(radians), cosf(radians) ); + } + +void SrVec2::abs () + { + x=SR_ABS(x); y=SR_ABS(y); + } + +void SrVec2::normalize () + { + float f = x*x + y*y; + if ( f==1.0 || f==0.0 ) return; + f = sqrtf ( f ); + x/=f; y/=f; + } + +float SrVec2::len ( float n ) + { + float f = sqrtf (x*x + y*y); + if ( f>0 ) { n/=f; x*=n; y*=n; } + return f; + } + +float SrVec2::norm () const + { + float f = x*x + y*y; + if ( f==1.0 || f==0.0 ) return f; + return sqrtf ( f ); + } + + +float SrVec2::norm_max () const + { + float a = SR_ABS(x); + float b = SR_ABS(y); + return SR_MAX ( a, b ); + } + +float SrVec2::angle () const + { + float ang=atan2f(y,x); + if ( ang<0 ) ang += sr2pi; + return ang; + } + +float SrVec2::angle_max () const + { + float comp, b, a; + a=SR_ABS(x); b=SR_ABS(y); + if (b==0.0 && x>=0.0) return 0.0; + comp = b>=a? ((float)2.0)-(a/b) : (b/a); + if (x<0.0) comp = ((float)4.0)-comp; + if (y<0.0) comp = ((float)8.0)-comp; + return comp; + } + +//=================================== Friend Functions =================================== + +void swap ( SrVec2 &v1, SrVec2 &v2 ) + { + float tmp; + SR_SWAP(v1.x,v2.x); + SR_SWAP(v1.y,v2.y); + } + +float dist_max ( const SrVec2 &v1, const SrVec2 &v2 ) + { + float a = v1.x-v2.x; + float b = v1.y-v2.y; + a = SR_ABS(a); + b = SR_ABS(b); + return SR_MAX(a,b); + } + +float dist ( const SrVec2 &v1, const SrVec2 &v2 ) + { + float dx, dy; + + dx=v1.x-v2.x; dy=v1.y-v2.y; + + return sqrtf (dx*dx + dy*dy); + } + +float dist2 ( const SrVec2 &v1, const SrVec2 &v2 ) + { + float dx, dy; + dx=v1.x-v2.x; dy=v1.y-v2.y; + return dx*dx + dy*dy; + } + +float angle ( const SrVec2 &v1, const SrVec2 &v2 ) + { + return acosf ( dot(v1,v2)/(v1.norm()*v2.norm()) ); + } + +float angle_fornormvecs ( const SrVec2 &v1, const SrVec2 &v2 ) + { + return acosf ( dot(v1,v2) ); + } + +float angle_ori ( const SrVec2& v1, const SrVec2& v2 ) // (-pi,pi] + { + float a = acosf ( dot(v1,v2)/(v1.norm()*v2.norm()) ); + if ( SR_CCW(v1.x,v1.y,0,0,v2.x,v2.y)>0 ) a=-a; + return a; + } + +float angle_max ( const SrVec2 &v1, const SrVec2 &v2 ) // [0,4] + { + float a1 = v1.angle_max(); + float a2 = v2.angle_max(); + float a = a1>a2? a1-a2:a2-a1; + if ( a>4.0f ) a = 8.0f-a; + return a; + } + +float angle_max_ori ( const SrVec2 &v1, const SrVec2 &v2 ) // (-4,4] + { + float a = v2.angle_max() - v1.angle_max(); + +// sr_out<<" ["<4.0f ) a -= 8.0f; + if ( a<=-4.0f ) a += 8.0f; + return a; + } + +float cross ( const SrVec2 &v1, const SrVec2 &v2 ) + { + return v1.x*v2.y - v1.y*v2.x; + } + +float dot ( const SrVec2 &v1, const SrVec2 &v2 ) + { + return v1.x*v2.x + v1.y*v2.y; + } + +SrVec2 lerp ( const SrVec2 &v1, const SrVec2 &v2, float t ) + { + return v1*(1.0f-t) + v2*t; + } + +int compare ( const SrVec2 &v1, const SrVec2 &v2 ) + { + if ( v1.x > v2.x ) return 1; + if ( v1.x < v2.x ) return -1; + if ( v1.y > v2.y ) return 1; + if ( v1.y < v2.y ) return -1; + return 0; + } + +int compare ( const SrVec2* v1, const SrVec2* v2 ) + { + return compare ( *v1, *v2 ); + } + +int compare_polar ( const SrVec2 &v1, const SrVec2 &v2 ) + { + float a1 = angle_max(v1,SrVec2::i); + float a2 = angle_max(v2,SrVec2::i); + if ( a1 > a2 ) return 1; + if ( a1 < a2 ) return -1; + return 0; + } + +int compare_polar ( const SrVec2* v1, const SrVec2* v2 ) + { + return compare_polar ( *v1, *v2 ); + } + +void barycentric ( const SrPnt2& p1, const SrPnt2& p2, const SrPnt2& p3, const SrPnt2& p, + float& u, float& v, float& w ) + { + # define DET3(a,b,c,d,e,f,g,h,i) a*e*i +b*f*g +d*h*c -c*e*g -b*d*i -a*f*h + float A = DET3 ( p1.x, p2.x, p3.x, p1.y, p2.y, p3.y, 1, 1, 1 ); + float A1 = DET3 ( p.x, p2.x, p3.x, p.y, p2.y, p3.y, 1, 1, 1 ); + float A2 = DET3 ( p1.x, p.x, p3.x, p1.y, p.y, p3.y, 1, 1, 1 ); + //float A3 = DET3 ( p1.x, p2.x, p.x, p1.y, p2.y, p.y, 1, 1, 1 ); + # undef DET3 + u = A1/A; + v = A2/A; + w = 1.0f-u-v; // == A3/A; + } + +float ccw ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3 ) + { + return (float) sr_ccw(p1.x,p1.y,p2.x,p2.y,p3.x,p3.y); + } + +bool segments_intersect ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3, const SrVec2& p4 ) + { + return sr_segments_intersect ( p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, p4.x, p4.y ); + } + +bool segments_intersect ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3, const SrVec2& p4, SrVec2& p ) + { + double x, y; + bool b = sr_segments_intersect ( p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, p4.x, p4.y, x, y ); + p.set ( float(x), float(y) ); + return b; + } + +bool lines_intersect ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3, const SrVec2& p4 ) + { + return sr_lines_intersect ( p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, p4.x, p4.y ); + } + +bool lines_intersect ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3, const SrVec2& p4, SrVec2& p ) + { + double x, y; + bool b = sr_lines_intersect ( p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, p4.x, p4.y, x, y ); + p.set ( float(x), float(y) ); + return b; + } + +void line_projection ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p, SrVec2& q ) + { + double x, y; + sr_line_projection ( p1.x, p1.y, p2.x, p2.y, p.x, p.y, x, y ); + q.set ( float(x), float(y) ); + } + +bool segment_projection ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p, SrVec2& q, float epsilon ) + { + double x, y; + bool b = sr_segment_projection ( p1.x, p1.y, p2.x, p2.y, p.x, p.y, x, y, epsilon ); + q.set ( float(x), float(y) ); + return b; + } + +bool in_segment ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p, float epsilon ) + { + return sr_in_segment ( p1.x, p1.y, p2.x, p2.y, p.x, p.y, epsilon ); + } + +bool in_segment ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p, float epsilon, float& dist2 ) + { + double d; + bool b = sr_in_segment ( p1.x, p1.y, p2.x, p2.y, p.x, p.y, epsilon, d ); + dist2 = (float) d; + return b; + } + +bool in_triangle ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3, const SrVec2& p ) + { + return sr_in_triangle ( p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, p.x, p.y ); + } + +bool in_circle ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3, const SrVec2& p ) + { + return sr_in_circle ( p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, p.x, p.y ); + } + +SrOutput& operator<< ( SrOutput& o, const SrVec2& v ) + { + return o << v.x <<' '<< v.y; + } + +SrInput& operator>> ( SrInput& in, SrVec2& v ) + { + return in >> v.x >> v.y; + } + +//================================== End of File =========================================== + +/* +double Area ( const OVec2d &p1, const OVec2d &p2, const OVec2d &p3 ) + { + return 0.5 * ( (p1.x*p2.y) + (p2.x*p3.y) + (p3.x*p1.y) - + (p1.x*p3.y) - (p3.x*p2.y) - (p2.x*p1.y) ); + } + +double Ccw ( const OVec2d &p1, const OVec2d &p2, const OVec2d &p3 ) + { + return ( (p1.x*p2.y) + (p2.x*p3.y) + (p3.x*p1.y) - + (p1.x*p3.y) - (p3.x*p2.y) - (p2.x*p1.y) ); + } + +void LineCoefs ( const OVec2d &p1, const OVec2d &p2, + double &a, double &b, double &c ) + { + // need to test if this is correct : + a = p2.y-p1.y; + b = p1.x-p2.x; + c = p2.x*p1.y - p1.x*p2.y; + } + +double Dist ( const OVec2d &p1, const OVec2d &p2, const OVec2d &p ) + { + double a, b, c, abs; + LineCoefs ( p1, p2, a, b, c ); + abs = a*p.x + b*p.y + c; + return ABS(abs) / sqrt ( a*a + b*b ); + } + +OVec2d CircleCenter ( const OVec2d &p1, const OVec2d &p2, const OVec2d &p3 ) + { + OVec2d v1 = ((p2-p1)*0.5); + OVec2d v2 = ((p3-p1)*0.5); + OVec2d m1 = ( p1 + v1 ); + OVec2d m2 = ( p1 + v2 ); + OVec2d inter; + InterLines ( m1, m1+v1.Orthogonal(), m2, m2+v2.Orthogonal(), inter ); + return inter; + } + +static float dist_pt_line2 ( float x, float y, float x1, float y1, float x2, float y2 ) + { + float vx, vy, u; + + vx=x2-x1; vy=y2-y1; + float n2 = vx*vx + vy*vy; + + if ( n2==0 ) return 0; + + u = ( (x-x1)*(x2-x1) + (y-y1)*(y2-y1) ) / n2; + + vx = x1 + u * (x2 - x1); + vy = y1 + u * (y2 - y1); + + x = vx-x; + y = vy-y; + return x*x + y*y; + } + +*/ diff --git a/source/dcdt/se/sr_vec2.h b/source/dcdt/se/sr_vec2.h new file mode 100644 index 0000000000..ca1bd6cf72 --- /dev/null +++ b/source/dcdt/se/sr_vec2.h @@ -0,0 +1,260 @@ +/** \file sr_vec2.h + * Two dimensional vec2tor */ + +# ifndef SR_VEC2_H +# define SR_VEC2_H + +# include "sr_input.h" +# include "sr_output.h" + +class SrVec2; + +/*! This "point typedef" is used only to coherently describe variables, + making possible to distiguish points from vectors parameters when + interpreting the meaning of parameters. */ +typedef SrVec2 SrPnt2; + +/*! \class SrVec2 sr_vec2.h + \brief Two dimensional vector. + + Its two coordinates are of type float. */ +class SrVec2 + { public : + float x, y; + public : + static const SrVec2 null; //!< (0,0) null vector + static const SrVec2 one; //!< (1,1) vector + static const SrVec2 minusone; //!< (-1,-1) vector + static const SrVec2 i; //!< (1,0) vector + static const SrVec2 j; //!< (0,1) vector + public : + + /*! Initializes SrVec2 as a null vector. Implemented inline. */ + SrVec2 () : x(0), y(0) {} + + /*! Copy constructor. Implemented inline. */ + SrVec2 ( const SrVec2& v ) : x(v.x), y(v.y) {} + + /*! Initializes with the two given float coordinates. Implemented inline. */ + SrVec2 ( float a, float b ) : x(a), y(b) {} + + /*! Initializes from a float pointer. Implemented inline. */ + SrVec2 ( float p[2] ) : x(p[0]), y(p[1]) {} + + /*! Sets coordinates from the given vector. Implemented inline. */ + void set ( const SrVec2& v ) { x=v.x; y=v.y; } + + /*! Sets coordinates from the two given float values. Implemented inline. */ + void set ( float a, float b ) { x=a; y=b; } + + /*! Apply a rotation to SrVec2 by angle a, where sa=sin(a), and ca=cos(a). */ + void rot ( float sa, float ca ); + + /*! Apply a rotation around cent. */ + void rot ( const SrVec2& cent, float sa, float ca ); + + /*! Apply a rotation to SrVec2 with the angle in radians. */ + void rot ( float radians ); + + /*! Apply a rotation around cent, with the angle in radians. */ + void rot ( const SrVec2& cent, float radians ); + + /*! Makes each coordinate to have its absolute value. */ + void abs (); + + /*! Normalize the vector. If its norm is 0 or 1, the vector is not changed. + Note that the norm is checked to be 1 or 0 using norm2(), an so without + evaluating the square root. */ + void normalize (); + + /*! Sets the length to be n and returns the previous length */ + float len ( float n ); + + /*! Returns the length of the vector. Equivalent to norm() */ + float len () { return norm(); } + + /*! Returns the norm (distance to zero) of the vector. */ + float norm () const; + + /*! Returns the square of the norm (x*x + y*y). Implemented inline. */ + float norm2 () const { return x*x + y*y; } + + /*! Returns the norm, but using the max function of distance, which does not require sqrt() */ + float norm_max () const; + + /*! Returns the angle from the vector (0,0) to SrVec2. The angle will be a + value in radians inside the interval [0,pi], and uses atan2. */ + float angle () const; // [0,pi] + + /*! Returns the angle from the vector (0,0) to SrVec2, but using the max norm. The angle + will then be a value inside the interval [0,8). */ + float angle_max () const; // [0,8) + + /*! Returns the orthogonal vector (-y,x). */ + SrVec2 ortho () const { return SrVec2(-y,x); } + + /*! Returns true if all coordinates are zero; false otherwise. */ + bool iszero () const { return x==0.0 && y==0.0? true:false; } + + /*! Returns true if the vector is inside the closed neighboorhood of point 0,0 and radius ds. */ + bool nextzero ( float ds ) const { return norm2()<=ds*ds? true:false; } + + /*! Allows member access like a vector */ + float& operator[] ( int i ) { return *((&x)+i); } + + /*! Convertion to a float pointer. */ + operator float* () const { return (float*)&x; } + + /*! Assignment operator from another SrVec2. Implemented inline. */ + void operator = ( const SrVec2& v ) { set(v); } + + /*! Assignment operator from a const float *. Implemented inline. */ + void operator = ( const float *v ) { x=(float)v[0]; y=(float)v[1]; } + + /*! -= operator with another SrVec2. Implemented inline. */ + void operator -= ( const SrVec2& v ) { x-=v.x; y-=v.y; } + + /*! += operator with another SrVec2. Implemented inline. */ + void operator += ( const SrVec2& v ) { x+=v.x; y+=v.y; } + + /*! *= operator with another SrVec2. Implemented inline. */ + void operator *= ( float r ) { x*=r; y*=r; } + + /*! /= operator with another SrVec2. Implemented inline. */ + void operator /= ( float r ) { x/=r; y/=r; } + + /*! Unary minus. Implemented inline. */ + friend SrVec2 operator - ( const SrVec2& v ) + { return SrVec2 ( -v.x, -v.y ); } + + /*! Adds two vectors. Implemented inline. */ + friend SrVec2 operator + ( const SrVec2& v1, const SrVec2& v2 ) + { return SrVec2 (v1.x+v2.x, v1.y+v2.y); } + + /*! Subtracts two vectors. Implemented inline. */ + friend SrVec2 operator - ( const SrVec2& v1, const SrVec2& v2 ) + { return SrVec2 (v1.x-v2.x, v1.y-v2.y); } + + /*! Comparison operator. Implemented inline. */ + friend bool operator == ( const SrVec2& v1, const SrVec2& v2 ) + { return v1.x==v2.x && v1.y==v2.y? true:false; } + + /*! Comparison operator. Implemented inline. */ + friend bool operator != ( const SrVec2& v1, const SrVec2& v2 ) + { return v1.x!=v2.x || v1.y!=v2.y? true:false; } + + /*! Multiplication by scalar. Implemented inline. */ + friend SrVec2 operator * ( const SrVec2& v, float r ) + { return SrVec2 (v.x*r, v.y*r); } + + /*! Division by scalar. Implemented inline. */ + friend SrVec2 operator / ( const SrVec2& v, float r ) + { return SrVec2 (v.x/r, v.y/r); } + + /*! Returns true if dist(v1,v2)<=ds, otherwise returns false. Implemented inline. */ + friend bool next ( const SrVec2& v1, const SrVec2& v2, float ds ) + { return dist2(v1,v2)<=ds*ds? true:false; } + + /*! Swaps the contents of v1 with v2. */ + friend void swap ( SrVec2& v1, SrVec2& v2 ); + + /*! Returns the distance between v1 and v2 using norm_max(). */ + friend float dist_max ( const SrVec2& v1, const SrVec2& v2 ); + + /*! Returns the distance between p1 and p2. */ + friend float dist ( const SrPnt2& p1, const SrPnt2& p2 ); + + /*! Returns the square of the distance between p1 and p2. */ + friend float dist2 ( const SrPnt2& p1, const SrPnt2& p2 ); + + /*! Returns the angle in radians between v1 and v2, + which is inside the interval [0,pi] */ + friend float angle ( const SrVec2& v1, const SrVec2& v2 ); + + friend float angle_fornormvecs ( const SrVec2 &v1, const SrVec2 &v2 ); + + /*! Returns the oriented angle in radians between v1 and v2, + which is inside the interval (-pi,pi] */ + friend float angle_ori ( const SrVec2& v1, const SrVec2& v2 ); + + /*! Returns the angle between v1 and v2, but using the max norm. + The angle will be a value inside the interval [0,4]. */ + friend float angle_max ( const SrVec2& v1, const SrVec2& v2 ); // [0,4] + + /*! Returns the oriented angle between v1 and v2, but using the max norm. + The angle will be a value inside the interval (-4,4]. */ + friend float angle_max_ori ( const SrVec2 &v1, const SrVec2 &v2 ); // (-4,4] + + /*! Returns the cross product z coordinate of v1 and v2: a zero value means + that the vectors are colinear, otherwise, a positive number means that + v2 is on the left side of v1, and a negative number means that v2 is on + the right side of v1. */ + friend float cross ( const SrVec2& v1, const SrVec2& v2 ); + + /*! Returns the dot product of v1 and v2 (v1.x*v2.x + v1.y*v2.y). */ + friend float dot ( const SrVec2& v1, const SrVec2& v2 ); + + /*! Returns the linear interpolation of v1 v2 in parameter t ( v1(1-t)+v2(t) ). */ + friend SrVec2 lerp ( const SrVec2& v1, const SrVec2& v2, float t ); + + /*! Returns -1,0,1 depending if v1 is less, equal or greater than v2, + using a comparison by coordinates. */ + friend int compare ( const SrVec2& v1, const SrVec2& v2 ); + + /*! Pointer version of the compare function. */ + friend int compare ( const SrVec2* v1, const SrVec2* v2 ); + + /*! Returns -1,0,1 depending if v1 is less, equal or greater than v2, + using a 2d polar comparison with în relation to (1,0) (using angle_max()). */ + friend int compare_polar ( const SrVec2& v1, const SrVec2& v2 ); + + /*! Pointer version of the compare_polar function. */ + friend int compare_polar ( const SrVec2* v1, const SrVec2* v2 ); + + /*! Returns u,v,w==1-u-v, u+v+w==1, such that p1*u + p2*v + p3*w == p */ + friend void barycentric ( const SrPnt2& p1, const SrPnt2& p2, const SrPnt2& p3, const SrPnt2& p, + float& u, float& v, float& w ); + + /*! Wrapper for the sr_ccw() function in sr_geo2.h */ + friend float ccw ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3 ); + + /*! Wrapper for the equivalent sr_segments_intersect function in sr_geo2.h */ + friend bool segments_intersect ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3, const SrVec2& p4 ); + + /*! Wrapper for the equivalent sr_segments_intersect function in sr_geo2.h */ + friend bool segments_intersect ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3, const SrVec2& p4, SrVec2& p ); + + /*! Wrapper for the equivalent sr_lines_intersect function in sr_geo2.h */ + friend bool lines_intersect ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3, const SrVec2& p4 ); + + /*! Wrapper for the equivalent sr_lines_intersect function in sr_geo2.h */ + friend bool lines_intersect ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3, const SrVec2& p4, SrVec2& p ); + + /*! Wrapper for the equivalent sr_line_projection function in sr_geo2.h */ + friend void line_projection ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p, SrVec2& q ); + + /*! Wrapper for the equivalent sr_segment_projection function in sr_geo2.h */ + friend bool segment_projection ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p, SrVec2& q, float epsilon ); + + /*! Wrapper for the equivalent sr_in_segment function in sr_geo2.h */ + friend bool in_segment ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p, float epsilon ); + + /*! Wrapper for the equivalent sr_in_segment function in sr_geo2.h */ + friend bool in_segment ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p, float epsilon, float& dist2 ); + + /*! Wrapper for the equivalent sr_in_triangle function in sr_geo2.h */ + friend bool in_triangle ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3, const SrVec2& p ); + + /*! Wrapper for the equivalent sr_in_circle function in sr_geo2.h */ + bool in_circle ( const SrVec2& p1, const SrVec2& p2, const SrVec2& p3, const SrVec2& p ); + + /*! Outputs in format: "x y". */ + friend SrOutput& operator<< ( SrOutput& o, const SrVec2& v ); + + /*! Inputs from format: "x y". */ + friend SrInput& operator>> ( SrInput& in, SrVec2& v ); + }; + +//============================== end of file =============================== + +# endif // SR_VEC2_H diff --git a/source/dcdt/se/sr_viewer.h b/source/dcdt/se/sr_viewer.h new file mode 100644 index 0000000000..8dd68482d1 --- /dev/null +++ b/source/dcdt/se/sr_viewer.h @@ -0,0 +1,197 @@ + +/** \file sr_viewer.h + * A fltk-opengl viewer + */ + +# ifndef SR_VIEWER_H +# define SR_VIEWER_H + +# include +# include + +class SrQuat; +class SrEvent; +class SrCamera; +class SrSn; +class SrViewerData; + +/*! \class SrViewer sr_viewer.h + \brief A fltk-opengl viewer + + SrViewer implements a viewer to render objects derived from SrSn. + The viewer has currently a planar and a tridimensional examiner mode. + In ModePlanar, only transformation on the XY plane are accepted. + In all modes, mouse interaction is done together with Ctrl and Shift modifiers. + A popup menu appears with a right button click or ctrl+shift+m. */ +class SrViewer : public Fl_Gl_Window + { + public : // enumerators + + enum ViewMode { ModeExaminer, + ModePlanar + }; + + enum RenderMode { ModeAsIs, + ModeDefault, + ModeSmooth, + ModeFlat, + ModeLines, + ModePoints + }; + + enum MenuCmd { CmdHelp, + CmdViewAll, + CmdBackground, + CmdExaminer, //!< examiner model manipulation + CmdPlanar, //!< planar manipulation + CmdAsIs, + CmdDefault, + CmdSmooth, + CmdFlat, + CmdLines, + CmdPoints, + CmdAxis, + CmdBoundingBox, + CmdStatistics, + CmdSpinAnim, + }; + + private : // internal data + + SrViewerData *_data; + + public : //----> public methods + + /*! Constructor needs the size and location of the window. */ + SrViewer ( int x, int y, int w, int h, const char *label=0 ); + + /*! Destructs all internal data, and calls unref() for the root node. */ + ~SrViewer (); + + /*! Retreave the scene root pointer, without calling unref() for it. Note that + if the user does not give any root node to SrViewer, an empty (but valid) + SrSnGroup is returned. */ + SrSn *root (); + + /*! Changes the scene root pointer. When the new node r is given, r->ref() is + called, and the old root node has its unref() method called. If r is null, + an empty SrSnGroup is created and used as root */ + void root ( SrSn *r ); + + /*! Draws string in the graphics window. If s==0 it will erase current string. */ + void draw_message ( const char* s ); + + /*! Shows the viewer pop up menu. */ + void show_menu (); + + /*! Activates an option available from the right button mouse menu of the viewer. */ + void menu_cmd ( MenuCmd c ); + + /*! Returns true if the cmd is currently activated. */ + bool menu_cmd_activated ( MenuCmd c ); + + void update_bbox (); + + void update_axis (); + + /*! Sets the camera to see the whole bounding box of the scene. The camera + center is put in the center of the bounding box, and the eye is put in + the line passing throught the center and parallel to the z axis, in a + sufficient distance from the center to visualize the entire bounding, + leaving the camera with a 60 degreed fovy. The up vector is set to (0,1,0). */ + void view_all (); + + /*! Will make SrViewer to render the scene in the next fltk loop. If the + current scene is spinning, then the scene is already being rendered by the + spin animation timeout with a defined frequency, an then a call to render() + will have no effect. */ + void render (); + + /*! Returns true if the window is iconized, false otherwise. */ + bool iconized (); + + /*! Returns true if the scene is spinning, false otherwise. When the scene is + spinning, then a spin animation timeout function is created in fltk that + stays calling render() with a calculated frequency. */ + bool spinning () ; + + /*! Sets data to be considered by the spin animation. */ + void set_spin_data ( const SrQuat &delta, float interval, float activation=0.1f ); + + /*! Gets the current spin animation data being used. */ + void get_spin_data ( SrQuat &delta, double &interval, double &activation ); + + /*! Forces spin animation to start or stop. */ + void spinning ( bool onoff ); + + void allow_spin_animation ( bool b ); + + void increment_model_rotation ( const SrQuat &dq ); + + float fps (); + sruint curframe (); + + SrColor background (); + void background ( SrColor c ); + + ViewMode get_view_mode (); + void get_camera ( SrCamera &cam ); + void set_camera ( const SrCamera &cam ); + + void export_all_models ( const char* prefix=0, const char* dir=0 ); + + public : // virtual methods + + /*! When the window manager asks the window to close. + The default implementation calls exit(0). */ + virtual void close_requested (); + + /*! draw() sets the viewer options and render the scene root. + This is a derived FLTK method and should not be called directly. + To draw the window contents, use render() instead. */ + virtual void draw (); + + /*! Called to initialize opengl and to set the viewport to (w,h). This + method is called also each time the window is reshaped. */ + virtual void init_opengl ( int w, int h ); + + /*! hande(int) controls the activation of the button menu, controls the + spinning animation, and translates fltk events into the SrEvent class + to then call handle_event(). Note that all handle methods should return + 1 when the event was understood and executed, and return 0 otherwise. + This is a derived FLTK method. */ + virtual int handle ( int event ); + + /*! handle_event() will just call the other handle methods, according + to the viewer configuration and generated event. This is the first method + that will be called from the fltk derived handle() method. */ + virtual int handle_event ( const SrEvent &e ); + + /*! Takes mouse events to rotate, scale and translate the scene. + If the event is not used, it is passed to the scene by calling + handle_scene_event(). */ + virtual int handle_examiner_manipulation ( const SrEvent &e ); + + /*! Takes mouse events to rotate, scale and translate the scene in planar mode. + If the event is not used, it is passed to the scene by calling + handle_scene_event(). */ + virtual int handle_planar_manipulation ( const SrEvent &e ); + + /*! Applies an event action to the scene */ + virtual int handle_scene_event ( const SrEvent &e ); + + /*! All keyboard events are passed to this method. The SrViewer + implementation checks if crtl+shift+m is pressed to display + the mouse menu, crtl+shift+x to exit the application, + crtl+shift+e to call the eps export action; + otherwise it passes the event to the scene graph. */ + virtual int handle_keyboard ( const SrEvent &e ); + + /*! Will be called each time a spin animation accured. The SrViewer + implementation does nothing. */ + virtual void spin_animation_occured (); + }; + +//================================ End of File ================================================= + +# endif // SR_VIEWER_H diff --git a/source/dcdt/se/terms.html b/source/dcdt/se/terms.html new file mode 100644 index 0000000000..d1490e146d --- /dev/null +++ b/source/dcdt/se/terms.html @@ -0,0 +1,31 @@ + + +Terms of SR/SE/SK Download + + + 
+  SR/SE/SK Toolkits terms of distribution. Marcelo Kallmann 2004.
+
+  Permission to use this software for educational, research and non-
+  profit purposes, without fee, and without a written agreement is
+  hereby granted, provided that the above notice and the following
+  three paragraphs appear in all copies.
+
+  IN NO EVENT SHALL THE AUTHOR OR HIS CURRENT OR PAST EMPLOYERS BE
+  LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
+  CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
+  USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE AUTHOR OR
+  HIS CURRENT OR PAST EMPLOYERS HAVE BEEN ADVISED OF THE POSSIBILITY
+  OF SUCH DAMAGES.
+
+  THE AUTHOR AND HIS CURRENT OR PAST EMPLOYERS SPECIFICALLY DISCLAIM ANY
+  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE
+  PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THERE IS NO OBLIGATIONS
+  TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
+
+  The author may be contacted via the following e-mail: kallmann(at)ict.usc.edu.
+
+ + + diff --git a/source/pch/se/precompiled.cpp b/source/pch/se/precompiled.cpp new file mode 100644 index 0000000000..7ff8df544a --- /dev/null +++ b/source/pch/se/precompiled.cpp @@ -0,0 +1 @@ +#include "precompiled.h" diff --git a/source/pch/se/precompiled.h b/source/pch/se/precompiled.h new file mode 100644 index 0000000000..25eb215657 --- /dev/null +++ b/source/pch/se/precompiled.h @@ -0,0 +1,7 @@ +#include "lib/precompiled.h" // common precompiled header + +// se PCH: + +#if HAVE_PCH + +#endif // HAVE_PCH diff --git a/source/ps/GameSetup/Config.cpp b/source/ps/GameSetup/Config.cpp index 62379fc050..75f51b42b4 100644 --- a/source/ps/GameSetup/Config.cpp +++ b/source/ps/GameSetup/Config.cpp @@ -42,6 +42,10 @@ bool g_VSync = false; bool g_Quickstart = false; +// flag to switch on drawing terrain overlays +bool g_showOverlay = false; + +// If non-empty, specified map will be automatically loaded CStr g_AutostartMap = ""; @@ -156,6 +160,9 @@ static void ProcessCommandLineArgs(const CmdLineArgs& args) if (args.Has("vsync")) g_ConfigDB.CreateValue(CFG_COMMAND, "vsync")->m_String = "true"; + + if (args.Has("showOverlay")) + g_showOverlay = true; } diff --git a/source/ps/GameSetup/Config.h b/source/ps/GameSetup/Config.h index 5b8c8a5e61..f3ddea3fc2 100644 --- a/source/ps/GameSetup/Config.h +++ b/source/ps/GameSetup/Config.h @@ -52,3 +52,5 @@ extern CStr g_CursorName; class CmdLineArgs; extern void CONFIG_Init(const CmdLineArgs& args); + +extern bool g_showOverlay; diff --git a/source/renderer/TerrainOverlay.cpp b/source/renderer/TerrainOverlay.cpp index ca9cdc22ce..54bcf25b12 100644 --- a/source/renderer/TerrainOverlay.cpp +++ b/source/renderer/TerrainOverlay.cpp @@ -10,6 +10,12 @@ #include +#include "ps/GameSetup/Config.h" + + + + + // Handy things for STL: /// Functor for sorting pairs, using the <-ordering of their second values. @@ -68,6 +74,99 @@ TerrainOverlay::~TerrainOverlay() g_TerrainOverlayList.erase(newEnd, g_TerrainOverlayList.end()); } + + + +//initial test to draw out entity boundaries +//it shows how to retrieve object boundary postions for triangulation +//NOTE: it's a test to see how to retrieve bounadry locations for static objects on the terrain. +//disabled +void TerrainOverlay::RenderEntityEdges() +{ + //Kai: added for line drawing + //use a function to encapsulate all the entity boundaries + std::vector results; + + + + g_EntityManager.GetExtant(results); + + glColor3f( 1, 1, 1 ); // Colour outline with player colour + + + + for(int i =0 ; i < results.size(); i++) + { + glBegin(GL_LINE_LOOP); + + CEntity* tempHandle = results[i]; + debug_printf("Entity position: %f %f %f\n", tempHandle->m_position.X,tempHandle->m_position.Y,tempHandle->m_position.Z); + + CVector2D p, q; + CVector2D u, v; + q.x = tempHandle->m_position.X; + q.y = tempHandle->m_position.Z; + float d = ((CBoundingBox*)tempHandle->m_bounds)->m_d; + float w = ((CBoundingBox*)tempHandle->m_bounds)->m_w; + + u.x = sin( tempHandle->m_graphics_orientation.Y ); + u.y = cos( tempHandle->m_graphics_orientation.Y ); + v.x = u.y; + v.y = -u.x; + + CBoundingObject* m_bounds = tempHandle->m_bounds; + + switch( m_bounds->m_type ) + { + case CBoundingObject::BOUND_CIRCLE: + { + break; + } + case CBoundingObject::BOUND_OABB: + { + //glVertex3f( tempHandle->m_position.X, tempHandle->GetAnchorLevel( tempHandle->m_position.X, tempHandle->m_position.Z ) + 10.0f, tempHandle->m_position.Z ); // lower left vertex + + //glVertex3f( 5, tempHandle->GetAnchorLevel( 5, 5 ) + 0.25f, 5 ); // upper vertex + + p = q + u * d + v * w; + glVertex3f( p.x, tempHandle->GetAnchorLevel( p.x, p.y ) + + 10.0f, p.y ); + + p = q - u * d + v * w ; + glVertex3f( p.x, tempHandle->GetAnchorLevel( p.x, p.y ) + + 10.0f, p.y ); + + p = q - u * d - v * w; + glVertex3f( p.x, tempHandle->GetAnchorLevel( p.x, p.y ) + + 10.0f, p.y ); + + p = q + u * d - v * w; + glVertex3f( p.x, tempHandle->GetAnchorLevel( p.x, p.y ) + + 10.0f, p.y ); + + + break; + } + + + + }//end switch + + glEnd(); + }//end for loop + + CTerrain* m_Terrain = g_Game->GetWorld()->GetTerrain(); + CEntity* tempHandle = results[0]; + glBegin(GL_LINE_LOOP); + + int width = m_Terrain->GetVerticesPerSide()*4; + glVertex3f( 0, tempHandle->GetAnchorLevel( 0, 0 ) + 0.25f, 0 ); + glVertex3f( width, tempHandle->GetAnchorLevel( width, 0 ) + 0.25f, 0 ); + glVertex3f( width, tempHandle->GetAnchorLevel(width,width ) + 0.25f,width ); + glVertex3f( 0, tempHandle->GetAnchorLevel( 0, width ) + 0.25f, width ); + glEnd(); + + //---------------------- +} + + + void TerrainOverlay::RenderOverlays() { if (g_TerrainOverlayList.size() == 0) @@ -95,6 +194,11 @@ void TerrainOverlay::RenderOverlays() glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); glDepthMask(GL_TRUE); glDisable(GL_BLEND); + + + //Kai: invoking the auxiliary function to draw out entity edges + //RenderEntityEdges(); + } ////////////////////////////////////////////////////////////////////////// @@ -110,21 +214,24 @@ void TerrainOverlay::GetTileExtents( void TerrainOverlay::Render() { - m_Terrain = g_Game->GetWorld()->GetTerrain(); + if(g_showOverlay) + { + m_Terrain = g_Game->GetWorld()->GetTerrain(); - int min_i, min_j, max_i, max_j; - GetTileExtents(min_i, min_j, max_i, max_j); - // Clamp the min to 0, but the max to -1 - so tile -1 can never be rendered, - // but if unclamped_max<0 then no tiles at all will be rendered. And the same - // for the upper limit. - min_i = clamp(min_i, 0, (int)m_Terrain->GetTilesPerSide()); - min_j = clamp(min_j, 0, (int)m_Terrain->GetTilesPerSide()); - max_i = clamp(max_i, -1, (int)m_Terrain->GetTilesPerSide()-1); - max_j = clamp(max_j, -1, (int)m_Terrain->GetTilesPerSide()-1); + int min_i, min_j, max_i, max_j; + GetTileExtents(min_i, min_j, max_i, max_j); + // Clamp the min to 0, but the max to -1 - so tile -1 can never be rendered, + // but if unclamped_max<0 then no tiles at all will be rendered. And the same + // for the upper limit. + min_i = clamp(min_i, 0, (int)m_Terrain->GetTilesPerSide()); + min_j = clamp(min_j, 0, (int)m_Terrain->GetTilesPerSide()); + max_i = clamp(max_i, -1, (int)m_Terrain->GetTilesPerSide()-1); + max_j = clamp(max_j, -1, (int)m_Terrain->GetTilesPerSide()-1); - for (m_j = min_j; m_j <= max_j; ++m_j) - for (m_i = min_i; m_i <= max_i; ++m_i) - ProcessTile(m_i, m_j); + for (m_j = min_j; m_j <= max_j; ++m_j) + for (m_i = min_i; m_i <= max_i; ++m_i) + ProcessTile(m_i, m_j); + } } void TerrainOverlay::RenderTile(const CColor& colour, bool draw_hidden) diff --git a/source/renderer/TerrainOverlay.h b/source/renderer/TerrainOverlay.h index d06e187f88..9cdba1a0fb 100644 --- a/source/renderer/TerrainOverlay.h +++ b/source/renderer/TerrainOverlay.h @@ -10,7 +10,23 @@ #ifndef INCLUDED_TERRAINOVERLAY #define INCLUDED_TERRAINOVERLAY +//Kai: added for rendering triangulate Terrain Overlay + + #include "ps/Overlay.h" // for CColor +#include +#include "ps/Vector2D.h" +#include "graphics/Terrain.h" + + +//Kai: added for line drawing +#include "simulation/Entity.h" +#include "simulation/EntityManager.h" +#include "simulation/BoundingObjects.h" +#include "dcdt/se/se_dcdt.h" +#include "lib/ogl.h" + + class CTerrain; @@ -91,10 +107,22 @@ protected: */ void RenderTileOutline(const CColor& colour, int line_width, bool draw_hidden); + + public: + + + /// Draw all TerrainOverlay objects that exist. static void RenderOverlays(); + /** + * Kai: added function to draw out line segments for triangulation + */ + static void RenderEntityEdges(); + + + private: /// Copying not allowed. TerrainOverlay(const TerrainOverlay&); @@ -112,6 +140,238 @@ private: }; +class TriangulationTerrainOverlay : public TerrainOverlay +{ + + SrArray constr; + SrArray unconstr; + + //std::vector path; + + SrPolygon CurPath; +public: + + + void TriangulationTerrainOverlay::RenderCurrentPath() + { + glColor3f(1,1,1); + + for(int i=0; i< CurPath.size()-1; i++) + { + std::vector results; + g_EntityManager.GetExtant(results); + CEntity* tempHandle = results[0]; + + glBegin(GL_LINE_LOOP); + + float x1 = CurPath[i].x; + float y1 = CurPath[i].y; + float x2 = CurPath[i+1].x; + float y2 = CurPath[i+1].y; + glVertex3f(x1,tempHandle->GetAnchorLevel(x1,y1) + 0.2f,y1); + glVertex3f(x2,tempHandle->GetAnchorLevel(x2,y2) + 0.2f,y2); + glEnd(); + + + } + } + + + // + //Kai: added function to draw out constrained line segments in triangulation + // + void TriangulationTerrainOverlay::RenderConstrainedEdges() + { + + std::vector results; + g_EntityManager.GetExtant(results); + CEntity* tempHandle = results[0]; + + glColor3f( 1, 1, 1 ); + + + for(int i=0; iGetAnchorLevel( x1, y1 ) + 0.2f, y1 ); + glVertex3f( x2, tempHandle->GetAnchorLevel( x2, y2 ) + 0.2f, y2 ); + glEnd(); + + } + + + } + + + // + // Kai: added function to draw out unconstrained line segments in triangulation + // + void TriangulationTerrainOverlay::RenderUnconstrainedEdges() + { + std::vector results; + g_EntityManager.GetExtant(results); + CEntity* tempHandle = results[0]; + + + glColor3f( 0, 1, 0 ); + + for(int i=0; iGetAnchorLevel( x1, y1 ) + 0.2f, y1 ); + glVertex3f( x2, tempHandle->GetAnchorLevel( x2, y2 ) + 0.2f, y2 ); + glEnd(); + + } + + } + + void TriangulationTerrainOverlay::setCurrentPath(SrPolygon _CurPath) + { + CurPath = _CurPath; + + } + + + + + + void TriangulationTerrainOverlay::setConstrainedEdges(SrArray _constr) + { + constr = _constr; + + } + + void TriangulationTerrainOverlay::setUnconstrainedEdges(SrArray _unconstr) + { + unconstr = _unconstr; + + } + + void Render() + { + + } + + + TriangulationTerrainOverlay::TriangulationTerrainOverlay() + { + + } + + virtual void GetTileExtents( + int& min_i_inclusive, int& min_j_inclusive, + int& max_i_inclusive, int& max_j_inclusive) + { + min_i_inclusive = 1; + min_j_inclusive = 1; + max_i_inclusive = 2; + max_j_inclusive = 2; + } + + virtual void ProcessTile(int i, int j) + { + + RenderConstrainedEdges(); + RenderUnconstrainedEdges(); + RenderCurrentPath(); + + } + + + + +}; + + + + + + + + + + +class PathFindingTerrainOverlay : public TerrainOverlay +{ +public: + char random[1021]; + std::vector aPath; + + void setPath(std::vector _aPath) + { + aPath =_aPath; + + for(unsigned int k = 0 ; k< aPath.size();k++) + { + aPath[k] = WorldspaceToTilespace( aPath[k] ); + } + + } + + CVector2D WorldspaceToTilespace( const CVector2D &ws ) + { + return CVector2D(floor(ws.x/CELL_SIZE), floor(ws.y/CELL_SIZE)); + } + + bool inPath(int i, int j) + { + for(unsigned int k = 0 ; k results; + + g_EntityManager.GetExtant(results); + + for(int i =0 ; i < results.size(); i++) + { + CEntity* tempHandle = results[i]; + + + + debug_printf("Entity position: %f %f %f\n", tempHandle->m_position.X,tempHandle->m_position.Y,tempHandle->m_position.Z); + + CBoundingObject* m_bounds = tempHandle->m_bounds; + + switch( m_bounds->m_type ) + { + case CBoundingObject::BOUND_CIRCLE: + { + break; + } + case CBoundingObject::BOUND_OABB: + { + + + glColor3f( 1, 1, 1 ); // Colour outline with player colour + + glBegin(GL_LINES); + + glVertex3f( 2, tempHandle->GetAnchorLevel( 2, 2 ) + 0.25f, 2 ); // lower left vertex + + glVertex3f( 5, tempHandle->GetAnchorLevel( 5, 5 ) + 0.25f, 5 ); // upper vertex + + glEnd(); + + + + break; + } + } + + + if(tempHandler->m_bound_type == CBoundingOjbect::BOUND_OABB) + { + debug_printf("Entity bound box: %f\n", tempHandler->m_bound_box.m_v); + } + + + + } + + + + +}*/ bool CAStarEngine::FindPath( const CVector2D &src, const CVector2D &dest, HEntity entity, float radius ) @@ -114,13 +182,15 @@ bool CAStarEngine::FindPath( std::vector::iterator it; for( it = neighbors.begin(); it != neighbors.end(); it++ ) { + + AStarNode* N = GetFreeASNode(); PROFILE_START("initialize neighbor"); N->coord = *it; // Assign f,g,h to neighbor N->g = best->g + mGoal->GetTileCost(best->coord, N->coord); N->h = mGoal->DistanceToGoal(*it); - N->f = N->g + N->h; + N->f = N->g +N->h; N->parent = best; PROFILE_END("initialize neighbor"); @@ -132,7 +202,7 @@ bool CAStarEngine::FindPath( if( C!=NULL && (N->f < C->f) ) { PROFILE_START("remove from closed"); - /* N is on Closed and N->f is better */ + // N is on Closed and N->f is better RemoveFromClosed(C); update = true; PROFILE_END("remove from closed"); @@ -140,7 +210,7 @@ bool CAStarEngine::FindPath( if (C==NULL || update) { PROFILE_START("add to open"); - /* N is not on Closed */ + // N is not on Closed AddToOpen(N); PROFILE_END("add to open"); } @@ -152,7 +222,16 @@ bool CAStarEngine::FindPath( //debug_printf("Number of nodes searched: %d\n", iterations); ConstructPath(best); } - + + //switch on/off grid path drawing by command line arg "-showOverlay" + //it's guarded here to stop setting the drawing path in pathfindingOverlay. + //(efficiency issue) + //the drawing is disable in the render() function in TerraiOverlay.cpp + if(g_showOverlay) + { + pathfindingOverlay.setPath(mPath); + } + Cleanup(); return mSolved; diff --git a/source/simulation/AStarEngine.h b/source/simulation/AStarEngine.h index 4638f70936..5da9840096 100644 --- a/source/simulation/AStarEngine.h +++ b/source/simulation/AStarEngine.h @@ -5,6 +5,13 @@ #include "ps/Player.h" #include +#include "renderer/TerrainOverlay.h" +#include "ps/Game.h" +#include "ps/World.h" +#include "EntityManager.h" + +#include "dcdt/se/se_dcdt.h" + class AStarNode { public: @@ -69,6 +76,15 @@ public: // The maximum number of nodes that will be expanded before failure is declared void SetSearchLimit( int limit ); + //Kai:added tile overlay for pathfinding + PathFindingTerrainOverlay pathfindingOverlay; + + SrPolygon pol; + + //void TAStarTest(); + + + protected: AStarGoalBase* mGoal; private: diff --git a/source/simulation/EntityStateProcessing.cpp b/source/simulation/EntityStateProcessing.cpp index 5cdd9265d5..8d26b8cf68 100644 --- a/source/simulation/EntityStateProcessing.cpp +++ b/source/simulation/EntityStateProcessing.cpp @@ -663,7 +663,10 @@ bool CEntity::ProcessGotoWaypoint( CEntityOrder* current, size_t UNUSED(timestep ChooseMovementSpeed( Distance ); - g_Pathfinder.RequestLowLevelPath( me, path_to, contact, pathfinder_radius, source ); + //Kai: invoking triangulation pathfinding function instead + //g_Pathfinder.RequestLowLevelPath( me, path_to, contact, pathfinder_radius, source ); + + g_Pathfinder.RequestTriangulationPath( me, path_to, contact, pathfinder_radius, source ); return( true ); } diff --git a/source/simulation/PathfindEngine.cpp b/source/simulation/PathfindEngine.cpp index be38dbaea5..215c4bf31e 100644 --- a/source/simulation/PathfindEngine.cpp +++ b/source/simulation/PathfindEngine.cpp @@ -10,13 +10,198 @@ #include "ps/World.h" +#include "ps/GameSetup/Config.h" + + + + +#define EPSILON 0.00001f + +bool initialized = false; + + CPathfindEngine::CPathfindEngine() { + +} + + +//Todo: +// 1; the bouncing problem with the fortress +// 2; update obstacles when things vanishes + +void CPathfindEngine::initBoundary() +{ + SrPolygon boundary ; + + + CTerrain* m_Terrain = g_Game->GetWorld()->GetTerrain(); + int width = m_Terrain->GetVerticesPerSide() * CELL_SIZE ; + + boundary.push().set(0.0f, 0.0f); + boundary.push().set(width, 0.0f); + boundary.push().set(width, width); + boundary.push().set(0.0f, width); + dcdtPathfinder.init(boundary, EPSILON,1); + dcdtPathfinder.InitializeSectors(); + +} + +void CPathfindEngine::insertObstacles() +{ + + std::vector results; + g_EntityManager.GetExtant(results); + SrPolygon poly; + + + for(int i =0 ; i < results.size(); i++) + { + poly.size(0); + + + + + CEntity* tempHandle = results[i]; + //debug_printf("Entity position: %f %f %f\n", tempHandle->m_position.X,tempHandle->m_position.Y,tempHandle->m_position.Z); + + + CVector2D p, q; + CVector2D u, v; + q.x = tempHandle->m_position.X; + q.y = tempHandle->m_position.Z; + float d = ((CBoundingBox*)tempHandle->m_bounds)->m_d; + float w = ((CBoundingBox*)tempHandle->m_bounds)->m_w; + + u.x = sin( tempHandle->m_graphics_orientation.Y ); + u.y = cos( tempHandle->m_graphics_orientation.Y ); + v.x = u.y; + v.y = -u.x; + + CBoundingObject* m_bounds = tempHandle->m_bounds; + + switch( m_bounds->m_type ) + { + case CBoundingObject::BOUND_CIRCLE: + { + if(tempHandle->m_speed == 0) + { + + poly.open(false); + + w = 0.5; + d = 0.5; + + p = q + u * d + v * w; + poly.push().set((float)(p.x), (float)(p.y)); + + p = q - u * d + v * w ; + poly.push().set((float)(p.x), (float)(p.y)); + + p = q - u * d - v * w; + poly.push().set((float)(p.x), (float)(p.y)); + + p = q + u * d - v * w; + poly.push().set((float)(p.x), (float)(p.y)); + + int dcdtId = dcdtPathfinder.insert_polygon(poly); + + } + break; + + } + case CBoundingObject::BOUND_OABB: + { + + + poly.open(false); + + // Tighten the bound so the units will not get stuck near the buildings + //Note: the triangulation pathfinding code will not find a path for the unit if it is pushed into the bound of a unit. + // + w = w * 0.85; + d = d * 0.85; + + p = q + u * d + v * w; + poly.push().set((float)(p.x), (float)(p.y)); + + p = q - u * d + v * w ; + poly.push().set((float)(p.x), (float)(p.y)); + + p = q - u * d - v * w; + poly.push().set((float)(p.x), (float)(p.y)); + + p = q + u * d - v * w; + poly.push().set((float)(p.x), (float)(p.y)); + + int dcdtId = dcdtPathfinder.insert_polygon(poly); + break; + } + + + + }//end switch + + + }//end for loop + dcdtPathfinder.DeleteAbstraction(); + dcdtPathfinder.Abstract(); + + + + + + +} + +void CPathfindEngine::drawTrianulation() +{ + + + int polyNum = dcdtPathfinder.num_polygons(); + + debug_printf("Number of polygons: %d",polyNum); + + if(polyNum) + { + SrArray constrainedEdges; + SrArray unconstrainedEdges; + + dcdtPathfinder.get_mesh_edges(&constrainedEdges, &unconstrainedEdges); + + triangulationOverlay.setConstrainedEdges(constrainedEdges); + triangulationOverlay.setUnconstrainedEdges(unconstrainedEdges); + + } + + } void CPathfindEngine::RequestPath( HEntity entity, const CVector2D& destination, CEntityOrder::EOrderSource orderSource ) { + /* TODO:1. add code to verify the triangulation. done. + 2. add code to convert a path from dcdtpathfinder to world waypoints + */ + if(!initialized) + { + initBoundary(); + insertObstacles(); + initialized =true; + + + //switch on/off triangulation drawing by command line arg "-showOverlay" + //it's guarded here to stop setting constrainedEdges and unconstrainedEdges in triangulationOverlay. + //(efficiency issue) + //the drawing is disable in the render() function in TerraiOverlay.cpp + if(g_showOverlay) + { + drawTrianulation(); + } + } + + + /* TODO: Add code to generate high level path For now, just the one high level waypoint to the final destination is added @@ -25,14 +210,134 @@ void CPathfindEngine::RequestPath( HEntity entity, const CVector2D& destination, waypoint.m_type = CEntityOrder::ORDER_GOTO_WAYPOINT; waypoint.m_source = orderSource; waypoint.m_target_location = destination; - waypoint.m_pathfinder_radius = 0.0f; + + + //Kai: adding radius for pathfinding + CBoundingObject* m_bounds = entity->m_bounds; + + waypoint.m_pathfinder_radius = m_bounds->m_radius +1.0f; + + //waypoint.m_pathfinder_radius = 0.0f; entity->m_orderQueue.push_front( waypoint ); } + +void CPathfindEngine::RequestTriangulationPath( HEntity entity, const CVector2D& destination, bool UNUSED(contact), + float radius, CEntityOrder::EOrderSource orderSource ) +{ + PROFILE_START("Pathfinding"); + + + //Kai: added test for terrain information in entityManager + //mLowPathfinder.TAStarTest(); + + CVector2D source( entity->m_position.X, entity->m_position.Z ); + + + + bool found = mTriangulationPathfinder.FindPath(source, destination, entity,dcdtPathfinder, radius); + + + + + //push the path onto the order process queue. + SrPolygon CurPath; + SrPolygon CurChannel; + + + if ( !found ) + { + // If no path was found, then unsolvable + // TODO: Figure out what to do in this case + + CurPath.size(0); + CurChannel.size(0); + } + else + { + CurChannel = dcdtPathfinder.GetChannelBoundary(); + + CurPath = dcdtPathfinder.GetPath(); + + + //set and draw the path on the terrain + triangulationOverlay.setCurrentPath(CurPath); + + // Make the path take as few steps as possible by collapsing steps in the same direction together. + std::vector path; + + debug_printf("waypoints: %d channel size %d \n ",CurPath.size(),CurChannel.size()); + + for (int i = 0; i < CurPath.size(); i++) + { + CVector2D waypoint ; + + waypoint.x = CurPath[i].x; + waypoint.y = CurPath[i].y; + + debug_printf("waypoints: %f %f \n",waypoint.x, waypoint.y); + + + path.push_back(waypoint); + + + } + + if( path.size() > 0 ) + { + // Push the path onto the front of our order queue in reverse order, + // so that we run through it before continuing other orders. + + CEntityOrder node; + node.m_source = orderSource; + + // Hack to make pathfinding slightly more precise: + // If the radius was 0, make the final node be exactly at the destination + // (otherwise, go to wherever the pathfinder tells us since we just want to be in range) + CVector2D finalDest = (radius==0 ? destination : path[path.size()-1]); + node.m_type = CEntityOrder::ORDER_PATH_END_MARKER; // push end marker (used as a sentinel when repathing) + node.m_target_location = finalDest; + entity->m_orderQueue.push_front(node); + node.m_type = CEntityOrder::ORDER_GOTO_NOPATHING; // push final goto step + node.m_target_location = finalDest; + entity->m_orderQueue.push_front(node); + + for( int i = ((int) path.size()) - 2; i >= 0; i-- ) + { + node.m_type = CEntityOrder::ORDER_GOTO_NOPATHING; // TODO: For non-contact paths, do we want some other order type? + node.m_target_location = path[i]; + entity->m_orderQueue.push_front(node); + } + } + else { + // Hack to make pathfinding slightly more precise: + // If radius = 0, we have an empty path but the user still wants us to move + // within the same tile, so add a GOTO order anyway + if(radius == 0) + { + CEntityOrder node; + node.m_type = CEntityOrder::ORDER_PATH_END_MARKER; + node.m_target_location = destination; + entity->m_orderQueue.push_front(node); + node.m_type = CEntityOrder::ORDER_GOTO_NOPATHING; + node.m_target_location = destination; + entity->m_orderQueue.push_front(node); + } + } + + } + + PROFILE_END("Pathfinding"); +} + void CPathfindEngine::RequestLowLevelPath( HEntity entity, const CVector2D& destination, bool UNUSED(contact), float radius, CEntityOrder::EOrderSource orderSource ) { PROFILE_START("Pathfinding"); + + + //Kai: added test for terrain information in entityManager + //mLowPathfinder.TAStarTest(); CVector2D source( entity->m_position.X, entity->m_position.Z ); diff --git a/source/simulation/PathfindEngine.h b/source/simulation/PathfindEngine.h index ca566efdf8..0f60325d7e 100644 --- a/source/simulation/PathfindEngine.h +++ b/source/simulation/PathfindEngine.h @@ -12,7 +12,11 @@ #include "ps/Singleton.h" #include "EntityHandles.h" #include "ps/Vector2D.h" -#include "AStarEngine.h" +//#include "AStarEngine.h" +#include "dcdt/se/se_dcdt.h" + +#include "TRAStarEngine.h" + #define g_Pathfinder CPathfindEngine::GetSingleton() @@ -28,6 +32,19 @@ enum EPathType class CPathfindEngine : public Singleton { public: + + //Kai: added for dcdt + static SrArray processing; + + SeDcdt dcdtPathfinder; + void initBoundary(); + void insertObstacles(); + void drawTrianulation(); + + //Kai:added tile overlay for pathfinding + TriangulationTerrainOverlay triangulationOverlay; + + CPathfindEngine(); void RequestPath( HEntity entity, const CVector2D& destination, @@ -38,8 +55,14 @@ public: void RequestContactPath( HEntity entity, CEntityOrder* current, float range ); bool RequestAvoidPath( HEntity entity, CEntityOrder* current, float avoidRange ); + + void RequestTriangulationPath( HEntity entity, const CVector2D& destination, bool contact, + float radius, CEntityOrder::EOrderSource orderSource ); + private: CAStarEngineLowLevel mLowPathfinder; + + CTRAStarEngine mTriangulationPathfinder; }; #endif diff --git a/source/simulation/TRAStarEngine.cpp b/source/simulation/TRAStarEngine.cpp new file mode 100644 index 0000000000..034641bba7 --- /dev/null +++ b/source/simulation/TRAStarEngine.cpp @@ -0,0 +1,19 @@ +#include "precompiled.h" +#include "TRAStarEngine.h" + +CTRAStarEngine::CTRAStarEngine(void) +{ + + mGoal = new AStarGoalLowLevel; +} + +CTRAStarEngine::~CTRAStarEngine(void) +{ + delete mGoal; +} + +bool CTRAStarEngine::FindPath(const CVector2D &src, const CVector2D &dest, HEntity entity, SeDcdt& dcdtPathfinder, float radius ) +{ + bool found = dcdtPathfinder.SearchPathFast(src.x, src.y, dest.x, dest.y, radius); + return found; +} diff --git a/source/simulation/TRAStarEngine.h b/source/simulation/TRAStarEngine.h new file mode 100644 index 0000000000..fec9c5d9d3 --- /dev/null +++ b/source/simulation/TRAStarEngine.h @@ -0,0 +1,19 @@ +#ifndef INCLUDED_TRASTARENGINE +#define INCLUDED_TRASTARENGINE + +#include "simulation\astarengine.h" + +class CTRAStarEngine : + public CAStarEngine +{ +public: + CTRAStarEngine(void); + ~CTRAStarEngine(void); + + SeDcdt dcdtPathfinder;; + + bool FindPath( const CVector2D& src, const CVector2D& dest, HEntity entity, SeDcdt& dcdtPathfinder,float radius=0.0f ); +}; + + +#endif \ No newline at end of file