From 4b665fa82b9760c59496398506efd07ee114bc68 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Wed, 29 Oct 2014 13:18:34 -0700 Subject: [PATCH] add btFileUtils::toLower to convert a string (char*) to lower case URDF import demo: add COLLADA .dae file support add FiniteElementMethod demo, extracted from the OpenTissue library (under the zlib license) don't crash if loading an invalid STL file add comparison with Assimp for COLLADA file loading (disabled by default, to avoid library dependency) Gwen: disable some flags that make the build incompatible --- Demos3/AllBullet2Demos/BulletDemoEntries.h | 4 +- Demos3/AllBullet2Demos/main.cpp | 2 +- Demos3/AllBullet2Demos/premake4.lua | 3 +- .../FiniteElementMethod/FiniteElementDemo.cpp | 245 +++++ .../FiniteElementMethod/FiniteElementDemo.h | 59 ++ Demos3/FiniteElementMethod/MyFemMesh.h | 214 +++++ .../collision/collision_geometry_interface.h | 68 ++ .../OpenTissue/configuration.h | 50 ++ .../core/containers/grid/util/grid_aof.h | 366 ++++++++ .../util/grid_approximate_gaussian_filter.h | 86 ++ .../core/containers/grid/util/grid_average.h | 90 ++ .../grid/util/grid_bisection_line_search.h | 76 ++ .../core/containers/grid/util/grid_blockify.h | 139 +++ .../core/containers/grid/util/grid_closing.h | 47 + .../grid/util/grid_compute_sign_function.h | 49 + .../containers/grid/util/grid_coord2idx.h | 65 ++ .../core/containers/grid/util/grid_crop.h | 116 +++ .../grid/util/grid_curvature_flow.h | 199 ++++ .../core/containers/grid/util/grid_dilation.h | 57 ++ .../core/containers/grid/util/grid_div_grad.h | 167 ++++ .../grid/util/grid_enclosing_indices.h | 79 ++ .../core/containers/grid/util/grid_erosion.h | 56 ++ .../containers/grid/util/grid_extrapolation.h | 136 +++ .../containers/grid/util/grid_fast_blur.h | 292 ++++++ .../core/containers/grid/util/grid_gradient.h | 171 ++++ .../grid/util/grid_gradient_at_point.h | 70 ++ .../grid/util/grid_gradient_magnitude.h | 69 ++ .../core/containers/grid/util/grid_hessian.h | 156 ++++ .../containers/grid/util/grid_idx2coord.h | 92 ++ .../containers/grid/util/grid_ignore_region.h | 74 ++ .../grid/util/grid_is_point_inside.h | 43 + .../containers/grid/util/grid_iterators.h | 346 +++++++ .../containers/grid/util/grid_junctions.h | 532 +++++++++++ .../containers/grid/util/grid_local_minima.h | 160 ++++ .../grid/util/grid_mean_curvature.h | 94 ++ .../containers/grid/util/grid_metamorphosis.h | 239 +++++ .../core/containers/grid/util/grid_opening.h | 48 + .../grid/util/grid_poisson_solver.h | 177 ++++ .../containers/grid/util/grid_redistance.h | 368 ++++++++ .../core/containers/grid/util/grid_resample.h | 66 ++ .../grid/util/grid_second_derivative.h | 86 ++ .../containers/grid/util/grid_split2slices.h | 63 ++ .../grid/util/grid_strict_local_minima.h | 160 ++++ .../containers/grid/util/grid_translate.h | 87 ++ .../grid/util/grid_uniform_point_sampling.h | 62 ++ .../grid/util/grid_upwind_gradient.h | 107 +++ .../grid/util/grid_upwind_gradient_field.h | 115 +++ .../grid/util/grid_value_at_point.h | 77 ++ .../grid/util/grid_voxel_plane_clip.h | 54 ++ .../containers/t4mesh/t4mesh_core_access.h | 74 ++ .../t4mesh/t4mesh_default_point_container.h | 70 ++ .../containers/t4mesh/t4mesh_default_traits.h | 38 + .../t4mesh/t4mesh_t4boundary_faces.h | 153 ++++ .../core/containers/t4mesh/t4mesh_t4edges.h | 219 +++++ .../core/containers/t4mesh/t4mesh_t4mesh.h | 202 +++++ .../core/containers/t4mesh/t4mesh_t4node.h | 138 +++ .../containers/t4mesh/t4mesh_t4tetrahedron.h | 105 +++ .../t4mesh/util/t4mesh_block_generator.h | 114 +++ .../t4mesh/util/t4mesh_compute_mesh_quality.h | 0 .../util/t4mesh_delaunay_tetrahedralization.h | 0 .../t4mesh/util/t4mesh_mesh_coupling.h | 280 ++++++ .../util/t4mesh_remove_redundant_nodes.h | 0 ..._constrained_delaunay_tetrahedralization.h | 48 + .../t4mesh/util/t4mesh_tetgen_mesh_lofter.h | 128 +++ ...t4mesh_tetgen_quality_tetrahedralization.h | 48 + .../OpenTissue/core/geometry/geometry_aabb.h | 368 ++++++++ .../core/geometry/geometry_barycentric.h | 394 ++++++++ .../core/geometry/geometry_base_shape.h | 64 ++ .../geometry_compute_tetrahedron_aabb.h | 103 +++ .../core/geometry/geometry_tetrahedron.h | 272 ++++++ .../geometry/geometry_tetrahedron_slicer.h | 157 ++++ .../geometry/geometry_tetrahedron_z_slicer.h | 165 ++++ .../core/geometry/geometry_volume_shape.h | 77 ++ .../OpenTissue/core/math/math_basic_types.h | 52 ++ .../math_compute_contiguous_angle_interval.h | 139 +++ .../OpenTissue/core/math/math_constants.h | 209 +++++ .../OpenTissue/core/math/math_coordsys.h | 320 +++++++ .../OpenTissue/core/math/math_covariance.h | 84 ++ .../core/math/math_dual_quaternion.h | 198 ++++ .../math/math_eigen_system_decomposition.h | 173 ++++ .../OpenTissue/core/math/math_functions.h | 158 ++++ .../OpenTissue/core/math/math_invert4x4.h | 65 ++ .../OpenTissue/core/math/math_is_finite.h | 32 + .../OpenTissue/core/math/math_is_number.h | 38 + .../OpenTissue/core/math/math_matrix3x3.h | 788 ++++++++++++++++ .../core/math/math_polar_decomposition.h | 149 +++ .../OpenTissue/core/math/math_power2.h | 83 ++ .../OpenTissue/core/math/math_precision.h | 42 + .../OpenTissue/core/math/math_prime_numbers.h | 209 +++++ .../OpenTissue/core/math/math_quaternion.h | 847 ++++++++++++++++++ .../OpenTissue/core/math/math_value_traits.h | 48 + .../OpenTissue/core/math/math_vector3.h | 577 ++++++++++++ .../OpenTissue/dynamics/fem/fem.h | 17 + .../dynamics/fem/fem_add_plasticity_force.h | 221 +++++ .../fem/fem_clear_stiffness_assembly.h | 37 + .../OpenTissue/dynamics/fem/fem_compute_b.h | 403 +++++++++ .../fem/fem_compute_isotropic_elasticity.h | 121 +++ .../OpenTissue/dynamics/fem/fem_compute_ke.h | 299 +++++++ .../dynamics/fem/fem_compute_mass.h | 175 ++++ .../dynamics/fem/fem_compute_volume.h | 93 ++ .../dynamics/fem/fem_conjugate_gradients.h | 141 +++ .../dynamics/fem/fem_dynamics_assembly.h | 104 +++ .../OpenTissue/dynamics/fem/fem_init.h | 84 ++ .../dynamics/fem/fem_initialize_plastic.h | 60 ++ .../fem/fem_initialize_stiffness_elements.h | 68 ++ .../OpenTissue/dynamics/fem/fem_mesh.h | 40 + .../OpenTissue/dynamics/fem/fem_node_traits.h | 86 ++ .../dynamics/fem/fem_position_update.h | 41 + .../dynamics/fem/fem_reset_orientation.h | 33 + .../OpenTissue/dynamics/fem/fem_simulate.h | 131 +++ .../dynamics/fem/fem_stiffness_assembly.h | 215 +++++ .../dynamics/fem/fem_tetrahedron_traits.h | 56 ++ .../dynamics/fem/fem_uniform_density.h | 38 + .../dynamics/fem/fem_uniform_poisson.h | 44 + .../dynamics/fem/fem_uniform_young.h | 43 + .../dynamics/fem/fem_update_orientation.h | 222 +++++ .../OpenTissue/utility/utility_class_id.h | 162 ++++ .../OpenTissue/utility/utility_empty_traits.h | 30 + .../OpenTissue/utility/utility_fps_counter.h | 73 ++ .../utility_get_environment_variable.h | 44 + .../utility/utility_get_system_memory_info.h | 108 +++ .../OpenTissue/utility/utility_greater_ptr.h | 33 + .../OpenTissue/utility/utility_identifier.h | 100 +++ .../OpenTissue/utility/utility_less_ptr.h | 33 + .../OpenTissue/utility/utility_material.h | 108 +++ .../utility/utility_pop_boost_filter.h | 10 + .../utility/utility_push_boost_filter.h | 23 + .../OpenTissue/utility/utility_qhull.h | 45 + .../OpenTissue/utility/utility_runtime_type.h | 37 + .../OpenTissue/utility/utility_timer.h | 102 +++ .../ImportColladaDemo/ImportColladaSetup.cpp | 39 +- .../ImportColladaDemo/LoadMeshFromCollada.cpp | 167 +++- .../ImportColladaDemo/LoadMeshFromCollada.h | 9 + Demos3/ImportSTLDemo/LoadMeshFromSTL.h | 10 +- .../ConvertRigidBodies2MultiBody.h | 14 + Demos3/ImportURDFDemo/ImportURDFSetup.cpp | 176 +++- btgui/Bullet3AppSupport/MyDebugDrawer.h | 20 +- btgui/Gwen/premake4.lua | 4 +- src/Bullet3Common/b3FileUtils.h | 22 +- 139 files changed, 17704 insertions(+), 46 deletions(-) create mode 100644 Demos3/FiniteElementMethod/FiniteElementDemo.cpp create mode 100644 Demos3/FiniteElementMethod/FiniteElementDemo.h create mode 100644 Demos3/FiniteElementMethod/MyFemMesh.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/collision/collision_geometry_interface.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/configuration.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_aof.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_approximate_gaussian_filter.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_average.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_bisection_line_search.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_blockify.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_closing.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_compute_sign_function.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_coord2idx.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_crop.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_curvature_flow.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_dilation.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_div_grad.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_enclosing_indices.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_erosion.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_extrapolation.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_fast_blur.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_gradient.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_gradient_at_point.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_gradient_magnitude.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_hessian.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_idx2coord.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_ignore_region.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_is_point_inside.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_iterators.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_junctions.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_local_minima.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_mean_curvature.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_metamorphosis.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_opening.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_poisson_solver.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_redistance.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_resample.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_second_derivative.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_split2slices.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_strict_local_minima.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_translate.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_uniform_point_sampling.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_upwind_gradient.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_upwind_gradient_field.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_value_at_point.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_voxel_plane_clip.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_core_access.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_default_point_container.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_default_traits.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4boundary_faces.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4edges.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4mesh.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4node.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4tetrahedron.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_block_generator.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_compute_mesh_quality.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_delaunay_tetrahedralization.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_mesh_coupling.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_remove_redundant_nodes.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_tetgen_constrained_delaunay_tetrahedralization.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_tetgen_mesh_lofter.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_tetgen_quality_tetrahedralization.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_aabb.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_barycentric.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_base_shape.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_compute_tetrahedron_aabb.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_tetrahedron.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_tetrahedron_slicer.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_tetrahedron_z_slicer.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_volume_shape.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_basic_types.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_compute_contiguous_angle_interval.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_constants.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_coordsys.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_covariance.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_dual_quaternion.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_eigen_system_decomposition.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_functions.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_invert4x4.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_is_finite.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_is_number.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_matrix3x3.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_polar_decomposition.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_power2.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_precision.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_prime_numbers.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_quaternion.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_value_traits.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/core/math/math_vector3.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_add_plasticity_force.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_clear_stiffness_assembly.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_b.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_isotropic_elasticity.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_ke.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_mass.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_volume.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_conjugate_gradients.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_dynamics_assembly.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_init.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_initialize_plastic.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_initialize_stiffness_elements.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_mesh.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_node_traits.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_position_update.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_reset_orientation.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_simulate.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_stiffness_assembly.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_tetrahedron_traits.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_uniform_density.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_uniform_poisson.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_uniform_young.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_update_orientation.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_class_id.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_empty_traits.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_fps_counter.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_get_environment_variable.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_get_system_memory_info.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_greater_ptr.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_identifier.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_less_ptr.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_material.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_pop_boost_filter.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_push_boost_filter.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_qhull.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_runtime_type.h create mode 100644 Demos3/FiniteElementMethod/OpenTissue/utility/utility_timer.h create mode 100644 Demos3/ImportURDFDemo/ConvertRigidBodies2MultiBody.h diff --git a/Demos3/AllBullet2Demos/BulletDemoEntries.h b/Demos3/AllBullet2Demos/BulletDemoEntries.h index c2106dbd8..e7c406b64 100644 --- a/Demos3/AllBullet2Demos/BulletDemoEntries.h +++ b/Demos3/AllBullet2Demos/BulletDemoEntries.h @@ -24,6 +24,7 @@ #include "../bullet2/MultiBodyDemo/TestJointTorqueSetup.h" #include "../bullet2/CollisionDetection/SupportFuncDemo.h" #include "../bullet2/BasicConcepts/CoordinateSystemDemo.h" +#include "../../Demos3/FiniteElementMethod/FiniteElementDemo.h" static BulletDemoInterface* TestJointTorqueCreateFunc(CommonGraphicsApp* app) { @@ -115,7 +116,8 @@ static BulletDemoEntry allDemos[]= { 1, "URDF", MyImportUrdfCreateFunc }, { 1, "STL", MyImportSTLCreateFunc}, { 1, "COLLADA", MyImportColladaCreateFunc}, - + {0,"Finite Element Method", 0}, + {1, "Finite Element Demo", FiniteElementDemo::CreateFunc}, /* {1,"ChainDemo",ChainDemo::MyCreateFunc}, // {0, "Stress tests", 0 }, diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index 52ef44ae2..f8df8c3f1 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -324,7 +324,7 @@ void openURDFDemo(const char* filename) ImportUrdfDemo* physicsSetup = new ImportUrdfDemo(); physicsSetup->setFileName(filename); - sCurrentDemo = new BasicDemo(0, physicsSetup); + sCurrentDemo = new BasicDemo(app, physicsSetup); if (sCurrentDemo) { diff --git a/Demos3/AllBullet2Demos/premake4.lua b/Demos3/AllBullet2Demos/premake4.lua index debd7120a..9b8ab7fc9 100644 --- a/Demos3/AllBullet2Demos/premake4.lua +++ b/Demos3/AllBullet2Demos/premake4.lua @@ -9,7 +9,8 @@ ".", "../../src", "../../btgui", - "../../btgui/lua-5.2.3/src" + "../../btgui/lua-5.2.3/src", + "../../Demos3/FiniteElementMethod" } diff --git a/Demos3/FiniteElementMethod/FiniteElementDemo.cpp b/Demos3/FiniteElementMethod/FiniteElementDemo.cpp new file mode 100644 index 000000000..87e81914e --- /dev/null +++ b/Demos3/FiniteElementMethod/FiniteElementDemo.cpp @@ -0,0 +1,245 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011-2014 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///the Finite Element Method is extracted from the OpenTissue library, +///under the zlib license: http://www.opentissue.org/mediawiki/index.php/Main_Page + + +#include "FiniteElementDemo.h" +#include "OpenGLWindow/CommonRenderInterface.h" +#include "LinearMath/btQuaternion.h" + +#include "MyFemMesh.h" +#include +#include +#include +#include "LinearMath/btAlignedObjectArray.h" + +//typedef OpenTissue::math::BasicMathTypes math_types; +typedef OpenTissue::math::BasicMathTypes math_types; +typedef OpenTissue::fem::Mesh mesh_type; +typedef math_types::vector3_type vector3_type; +typedef math_types::real_type real_type; + + + + +struct FiniteElementDemoInternalData +{ + mesh_type m_mesh1; + + bool m_stiffness_warp_on; ///< Boolean value indicating whether stiffness warping is turned on or off. + + bool m_collideGroundPlane; + + bool m_fixNodes; + + real_type m_gravity; + + real_type m_young;// = 500000; + real_type m_poisson;// = 0.33; + real_type m_density;// = 1000; + + //--- infinite m_c_yield plasticity settings means that plasticity is turned off + real_type m_c_yield;// = .04; //--- should be less than maximum expected elastic strain in order to see effect (works as a minimum). + real_type m_c_creep;// = .20; //--- controls how fast the plasticity effect occurs (it is a rate-like control). + real_type m_c_max;// = 0.2; //--- This is maximum allowed plasticity strain (works as a maximum). + double m_damp; + + FiniteElementDemoInternalData() + { + m_gravity = 9.81; + m_collideGroundPlane = true; + m_stiffness_warp_on= true; + m_young = 500000;//47863;//100000; + m_poisson = 0.33; + + m_density = 1054.00;//1000; + //--- infinite m_c_yield plasticity settings means that plasticity is turned off + m_c_yield = 0;//0.03;//.04; //--- should be less than maximum expected elastic strain in order to see effect (works as a minimum). + m_c_creep = 0;//0.20;//.20; //--- controls how fast the plasticity effect occurs (it is a rate-like control). + m_c_max = 1e30f;//0.2; //--- This is maximum allowed plasticity strain (works as a maximum). + m_damp=0.2f; + } + +}; + +FiniteElementDemo::FiniteElementDemo(CommonGraphicsApp* app) +:m_app(app), +m_x(0), +m_y(0), +m_z(0) +{ + m_app->setUpAxis(2); + m_data = new FiniteElementDemoInternalData; +} +FiniteElementDemo::~FiniteElementDemo() +{ + delete m_data; + m_app->m_renderer->enableBlend(false); +} + +void FiniteElementDemo::initPhysics() +{ + { + + OpenTissue::t4mesh::generate_blocks(10,3,3,0.1,0.1,0.1,m_data->m_mesh1); + + for (int n=0;nm_mesh1.m_nodes.size();n++) + { + m_data->m_mesh1.m_nodes[n].m_coord(1)+=0.1f; + m_data->m_mesh1.m_nodes[n].m_model_coord = m_data->m_mesh1.m_nodes[n].m_coord; + + } + OpenTissue::fem::init(m_data->m_mesh1,m_data->m_young,m_data->m_poisson,m_data->m_density,m_data->m_c_yield,m_data->m_c_creep,m_data->m_c_max); + + } +} +void FiniteElementDemo::exitPhysics() +{ + +} +void FiniteElementDemo::stepSimulation(float deltaTime) +{ + m_x+=0.01f; + m_y+=0.01f; + m_z+=0.01f; + double dt = 1./60.;//double (deltaTime); + //normal gravity + for (int n=0;nm_mesh1.m_nodes.size();n++) + { + + + if (m_data->m_fixNodes) + { + if (m_data->m_mesh1.m_nodes[n].m_model_coord(0) < 0.01) + { + m_data->m_mesh1.m_nodes[n].m_fixed = true; + } + } else + { + if (m_data->m_mesh1.m_nodes[n].m_model_coord(0) < 0.01) + { + m_data->m_mesh1.m_nodes[n].m_fixed = false; + } + } + vector3_type gravity = vector3_type(0.0, 0.0 , 0.0); + gravity(m_app->getUpAxis()) = -(m_data->m_mesh1.m_nodes[n].m_mass * m_data->m_gravity); + m_data->m_mesh1.m_nodes[n].m_f_external =gravity; + //m_data->m_mesh1.m_nodes[n].m_velocity.clear(); + } + + OpenTissue::fem::simulate(m_data->m_mesh1,dt,m_data->m_stiffness_warp_on,m_data->m_damp);//,0.1,20,20);//,1.0,20,20); + +} +void FiniteElementDemo::renderScene() +{ + m_app->m_renderer->renderScene(); +} + +void FiniteElementDemo::physicsDebugDraw() +{ +#if 0 + btVector3 xUnit(1,0,0); + btVector3 yUnit(0,1,0); + btVector3 zUnit(0,0,1); + + btScalar lineWidth=3; + + btQuaternion rotAroundX(xUnit,m_x); + btQuaternion rotAroundY(yUnit,m_y); + btQuaternion rotAroundZ(zUnit,m_z); + + btScalar radius=0.5; + btVector3 toX=radius*quatRotate(rotAroundX,yUnit); + btVector3 toY=radius*quatRotate(rotAroundY,xUnit); + btVector3 toZ=radius*quatRotate(rotAroundZ,xUnit); + + m_app->m_renderer->drawLine(xUnit+toX+quatRotate(rotAroundX,btVector3(0,0.1,-0.2)),xUnit+toX,xUnit,lineWidth); + m_app->m_renderer->drawLine(xUnit+toX+quatRotate(rotAroundX,btVector3(0,-0.2,-0.2)),xUnit+toX,xUnit,lineWidth); + //draw the letter 'x' on the x-axis + //m_app->m_renderer->drawLine(xUnit-0.1*zUnit+0.1*yUnit,xUnit+0.1*zUnit-0.1*yUnit,xUnit,lineWidth); + //m_app->m_renderer->drawLine(xUnit+0.1*zUnit+0.1*yUnit,xUnit-0.1*zUnit-0.1*yUnit,xUnit,lineWidth); + + m_app->m_renderer->drawLine(xUnit+toX+quatRotate(rotAroundX,btVector3(0,-0.2,-0.2)),xUnit+toX,xUnit,lineWidth); + + m_app->m_renderer->drawLine(yUnit+toY+quatRotate(rotAroundY,btVector3(-0.2,0,0.2)),yUnit+toY,yUnit,lineWidth); + m_app->m_renderer->drawLine(yUnit+toY+quatRotate(rotAroundY,btVector3(0.1,0,0.2)),yUnit+toY,yUnit,lineWidth); + m_app->m_renderer->drawLine(zUnit+toZ+quatRotate(rotAroundZ,btVector3(0.1,-0.2,0)),zUnit+toZ,zUnit,lineWidth); + m_app->m_renderer->drawLine(zUnit+toZ+quatRotate(rotAroundZ,btVector3(-0.2,-0.2,0)),zUnit+toZ,zUnit,lineWidth); +#endif + + //template + //inline void DrawPointsT4Mesh( point_container const& points, t4mesh const& mesh, double const& scale = 0.95, bool wireframe = false) + { + //geometry::Tetrahedron T; // From OpenTissue/core/geometry/geometry_tetrahederon.h + btAlignedObjectArray m_linePoints; + btAlignedObjectArray m_lineIndices; + + //geometry::Tetrahedron tet; + for (int t=0;tm_mesh1.m_tetrahedra.size();t++) + { + vector3_type v0d = m_data->m_mesh1.m_nodes[m_data->m_mesh1.m_tetrahedra[t].m_nodes[0]].m_coord; + vector3_type v1d = m_data->m_mesh1.m_nodes[m_data->m_mesh1.m_tetrahedra[t].m_nodes[1]].m_coord; + vector3_type v2d = m_data->m_mesh1.m_nodes[m_data->m_mesh1.m_tetrahedra[t].m_nodes[2]].m_coord; + vector3_type v3d = m_data->m_mesh1.m_nodes[m_data->m_mesh1.m_tetrahedra[t].m_nodes[3]].m_coord; + btVector3 v0(v0d(0),v0d(1),v0d(2)); + btVector3 v1(v1d(0),v1d(1),v1d(2)); + btVector3 v2(v2d(0),v2d(1),v2d(2)); + btVector3 v3(v3d(0),v3d(1),v3d(2)); + unsigned int baseIndex = m_linePoints.size(); + m_linePoints.push_back(v0); + m_linePoints.push_back(v1); + m_linePoints.push_back(v2); + m_linePoints.push_back(v3); + m_lineIndices.push_back(baseIndex+0); + m_lineIndices.push_back(baseIndex+1); + m_lineIndices.push_back(baseIndex+0); + m_lineIndices.push_back(baseIndex+2); + m_lineIndices.push_back(baseIndex+0); + m_lineIndices.push_back(baseIndex+3); + + m_lineIndices.push_back(baseIndex+1); + m_lineIndices.push_back(baseIndex+2); + m_lineIndices.push_back(baseIndex+2); + m_lineIndices.push_back(baseIndex+3); + m_lineIndices.push_back(baseIndex+1); + m_lineIndices.push_back(baseIndex+3); + } + + float debugColor[4]={0,0,0.4,1}; + m_app->m_renderer->drawLines(&m_linePoints[0].x(),debugColor, + m_linePoints.size(),sizeof(btVector3), + &m_lineIndices[0], + m_lineIndices.size(), + 1); + + + }; + +} +bool FiniteElementDemo::mouseMoveCallback(float x,float y) +{ + return false; +} +bool FiniteElementDemo::mouseButtonCallback(int button, int state, float x, float y) +{ + return false; +} +bool FiniteElementDemo::keyboardCallback(int key, int state) +{ + return false; +} + diff --git a/Demos3/FiniteElementMethod/FiniteElementDemo.h b/Demos3/FiniteElementMethod/FiniteElementDemo.h new file mode 100644 index 000000000..9a4a5cc49 --- /dev/null +++ b/Demos3/FiniteElementMethod/FiniteElementDemo.h @@ -0,0 +1,59 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011-2014 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///the Finite Element Method is extracted from the OpenTissue library, +///under the zlib license: http://www.opentissue.org/mediawiki/index.php/Main_Page + + +#ifndef FINITE_ELEMENT_DEMO_H +#define FINITE_ELEMENT_DEMO_H + +#include "Bullet3AppSupport/BulletDemoInterface.h" +#include "OpenGLWindow/CommonGraphicsApp.h" + + +///quick demo showing the right-handed coordinate system and positive rotations around each axis +class FiniteElementDemo : public BulletDemoInterface +{ + CommonGraphicsApp* m_app; + float m_x; + float m_y; + float m_z; + + struct FiniteElementDemoInternalData* m_data; +public: + + FiniteElementDemo(CommonGraphicsApp* app); + + virtual ~FiniteElementDemo(); + + static BulletDemoInterface* CreateFunc(CommonGraphicsApp* app) + { + return new FiniteElementDemo(app); + } + + virtual void initPhysics(); + virtual void exitPhysics(); + virtual void stepSimulation(float deltaTime); + virtual void renderScene(); + + + virtual void physicsDebugDraw(); + virtual bool mouseMoveCallback(float x,float y); + virtual bool mouseButtonCallback(int button, int state, float x, float y); + virtual bool keyboardCallback(int key, int state); +}; +#endif //FINITE_ELEMENT_DEMO_H + diff --git a/Demos3/FiniteElementMethod/MyFemMesh.h b/Demos3/FiniteElementMethod/MyFemMesh.h new file mode 100644 index 000000000..ee05d6398 --- /dev/null +++ b/Demos3/FiniteElementMethod/MyFemMesh.h @@ -0,0 +1,214 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011-2014 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///the Finite Element Method is extracted from the OpenTissue library, +///under the zlib license: http://www.opentissue.org/mediawiki/index.php/Main_Page + + +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_MESH_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_MESH_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +//#include +#include +#include +#include + +namespace OpenTissue +{ + namespace fem + { + + template + class Mesh +/* : public OpenTissue::t4mesh::T4Mesh< + math_types + , OpenTissue::fem::detail::NodeTraits + , OpenTissue::fem::detail::TetrahedronTraits + > + */ + { + public: + + + + typedef typename math_types::real_type real_type; + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::matrix3x3_type matrix3x3_type; + + class MyNodeType + { + public: + + MyNodeType() + :m_fixed(false) + { + + } + + typedef typename std::map matrix_container; + + typedef typename matrix_container::iterator matrix_iterator; + + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::real_type real_type; + + vector3_type m_coord; ///< Default Coordinate of tetramesh node. + vector3_type m_model_coord; + vector3_type m_f_external; + vector3_type m_velocity; + real_type m_mass; + bool m_fixed; + // Needed by the ConjugateGradient method + vector3_type m_update; + vector3_type m_prev; + vector3_type m_residual; + matrix_container m_K_row; ///< Currently stored in a map container, key correspond to column and mapped value to 3-by-3 sub block. + matrix_container m_A_row; + + vector3_type m_f0; + vector3_type m_b; + matrix_iterator Kbegin() { return m_K_row.begin(); } + matrix_iterator Kend() { return m_K_row.end(); } + matrix_iterator Abegin() { return m_A_row.begin(); } + matrix_iterator Aend() { return m_A_row.end(); } + + matrix3x3_type & K(int const & column_idx) { return m_K_row[column_idx]; } + matrix3x3_type & A(int const & column_idx) { return m_A_row[column_idx]; } + + int m_idx; + int idx() const { + return m_idx; + } + + + + }; + + class MyTetrahedronType + { + public: + + typedef typename math_types::matrix3x3_type matrix3x3_type; + typedef typename math_types::real_type real_type; + typedef typename math_types::vector3_type vector3_type; + + real_type m_young; + real_type m_poisson; + real_type m_density; + int m_nodes[4]; + + + Mesh* m_owner; + + matrix3x3_type m_Ke[4][4]; ///< Stiffness element matrix + matrix3x3_type m_Re; ///< Rotational warp of tetrahedron. + real_type m_V; ///< Volume of tetrahedron + + vector3_type m_e10; ///< edge from p0 to p1 + vector3_type m_e20; ///< edge from p0 to p2 + vector3_type m_e30; ///< edge from p0 to p3 + + //--- Stuff used exclusive by plastic effects + + vector3_type m_B[4]; ///< placeholders for Jacobian of shapefunctions: B = SN. + vector3_type m_D; ///< Elasticity Matrix in vector from + real_type m_plastic[6]; ///< Plastic strain tensor. + real_type m_yield; + real_type m_creep; + real_type m_max; + + MyNodeType* node(int i) + { + return &m_owner->m_nodes[m_nodes[i]]; + } + + MyNodeType* i() + { + return &m_owner->m_nodes[m_nodes[0]]; + } + MyNodeType* j() + { + return &m_owner->m_nodes[m_nodes[1]]; + } + + MyNodeType* k() + { + return &m_owner->m_nodes[m_nodes[2]]; + } + MyNodeType* m() + { + return &m_owner->m_nodes[m_nodes[3]]; + } + + MyTetrahedronType(int bla, int bla2) + { + + } + + }; + + + typedef std::vector< MyNodeType> node_container; + typedef std::vector< MyTetrahedronType > tetrahedra_container; + typedef MyNodeType node_type; + + public: + + node_container m_nodes; ///< Internal node storage. + + void insert(int a,int b,int c,int d) + { + MyTetrahedronType t(1,1); + t.m_owner = this; + t.m_nodes[0] = a; + t.m_nodes[1] = b; + t.m_nodes[2] = c; + t.m_nodes[3] = d; + m_tetrahedra.push_back(t); + + + } + void insert() + { + MyNodeType n; + n.m_idx = m_nodes.size(); + m_nodes.push_back(n); + + } + void clear() + { + m_nodes.clear(); + m_tetrahedra.clear(); + + } + + tetrahedra_container m_tetrahedra; ///< Internal tetrahedra storage. + + + }; + + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_MESH_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/collision/collision_geometry_interface.h b/Demos3/FiniteElementMethod/OpenTissue/collision/collision_geometry_interface.h new file mode 100644 index 000000000..23bca36d0 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/collision/collision_geometry_interface.h @@ -0,0 +1,68 @@ +#ifndef OPENTISSUE_COLLISION_COLLISION_GEOMETRY_INTERFACE_H +#define OPENTISSUE_COLLISION_COLLISION_GEOMETRY_INTERFACE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace collision + { + + + /** + * Collision Geometry Interface. + * The purpose of this class is to create a common interface for + * collision geometries. + * + * The interface should make it easier to integrate geometry types into + * a collision detection engine. In particular it should help make multiple + * dispatching easier (the class_id interface) and it should support basic + * functionality for computing bounding boxes (compute_collision_aabb method) + * + * + */ + template< typename math_types > + class GeometryInterface + : virtual public OpenTissue::utility::ClassIDInterface + { + public: + + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::matrix3x3_type matrix3x3_type; + + public: + + /** + * Compute Bounding Box. + * This method computes an axis aligned bounding + * box (AABB) that encloses the geometry. + * + * @param r The position of the model frame (i.e the coordinate frame the geometry lives in). + * @param R The orientation of the model frame (i.e the coordinate frame the geometry lives in). + * @param min_coord Upon return holds the minimum corner of the box. + * @param max_coord Upon return holds the maximum corner of the box. + * + */ + virtual void compute_collision_aabb( + vector3_type const & r + , matrix3x3_type const & R + , vector3_type & min_coord + , vector3_type & max_coord + ) const = 0; + + }; + + } // namespace collision + +} // namespace OpenTissue + +// OPENTISSUE_COLLISION_COLLISION_GEOMETRY_INTERFACE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/configuration.h b/Demos3/FiniteElementMethod/OpenTissue/configuration.h new file mode 100644 index 000000000..39ed8cfd5 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/configuration.h @@ -0,0 +1,50 @@ +#ifndef OPENTISSUE_CONFIGURATIOM_H +#define OPENTISSUE_CONFIGURATIOM_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#if (_MSC_VER >= 1200) +# pragma once +# pragma warning(default: 56 61 62 191 263 264 265 287 289 296 347 529 686) +# pragma warning(disable: 503) +#endif + +#ifdef WIN32 +# define WIN32_LEAN_AND_MEAN +# define _USE_MATH_DEFINES +# define NOMINMAX +# include +# undef WIN32_LEAN_AND_MEAN +# undef NOMINMAX +#endif + + +/** + * OpenTissue Version + */ +#define OPENTISSUE_VERSION 0.994 +#define OPENTISSUE_VERSION_MAJOR 0 +#define OPENTISSUE_VERSION_MINOR 994 + +#include + +/** + * OpenTissue Path. + * This is the path where OpenTissue was copied onto ones + * system. It can be used to locate shader programs or data resources. + */ +std::string const opentissue_path = "F:/develop/opentissue/sandbox/"; + +/** + * OpenTissue Version String. + * This string value can be used by end users for compatibility testing. + */ +std::string const opentissue_version = "0.994"; + + +//OPENTISSUE_CONFIGURATIOM_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_aof.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_aof.h new file mode 100644 index 000000000..e8bd0fc97 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_aof.h @@ -0,0 +1,366 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_AOF_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_AOF_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + + +#include +#include +#include +#include + +#include + +namespace OpenTissue +{ + namespace grid + { + + namespace detail + { + + /** + * Compute Average Outward Flux at designated nodal position of a signed distance grid. + * + * + * @param phi The signed distance grid. + * @param i The i'th index of the node. + * @param j The j'th index of the node. + * @param k The k'th index of the node. + * @param scale Default value is 0.5, this determines the scale on which the flux is computed. 0.5 correponds to ``voxel-based''. + * + * @return The flux value. + */ + template + inline typename grid_type::value_type + compute_aof_value( + grid_type const & phi + , size_t const & i + , size_t const & j + , size_t const & k + , double scale = 0.5 + ) + { + using std::min; + using std::sqrt; + + typedef typename grid_type::iterator iterator; + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename grid_type::value_type value_type; + typedef typename grid_type::math_types math_types; + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::real_type real_type; + + real_type dx = phi.dx()*scale; + real_type dy = phi.dy()*scale; + real_type dz = phi.dz()*scale; + + vector3_type n[26]; + vector3_type dp[26]; + + dp[0] = vector3_type( dx, 0, 0); + dp[1] = vector3_type(-dx, 0, 0); + dp[2] = vector3_type( 0, dy, 0); + dp[3] = vector3_type( 0,-dy, 0); + dp[4] = vector3_type( 0, 0, dz); + dp[5] = vector3_type( 0, 0,-dz); + dp[6] = vector3_type( dx, dy, dz); + dp[7] = vector3_type( dx, dy,-dz); + dp[8] = vector3_type( dx,-dy, dz); + dp[9] = vector3_type( dx,-dy,-dz); + dp[10] = vector3_type(-dx, dy, dz); + dp[11] = vector3_type(-dx, dy,-dz); + dp[12] = vector3_type(-dx,-dy, dz); + dp[13] = vector3_type(-dx,-dy,-dz); + dp[14] = vector3_type( dx, dy, 0); + dp[15] = vector3_type( dx,-dy, 0); + dp[16] = vector3_type(-dx, dy, 0); + dp[17] = vector3_type(-dx,-dy, 0); + dp[18] = vector3_type( dx, 0, dz); + dp[19] = vector3_type( dx, 0,-dz); + dp[20] = vector3_type(-dx, 0, dz); + dp[21] = vector3_type(-dx, 0,-dz); + dp[22] = vector3_type( 0, dy, dz); + dp[23] = vector3_type( 0, dy,-dz); + dp[24] = vector3_type( 0,-dy, dz); + dp[25] = vector3_type( 0,-dy,-dz); + for(size_t cnt=0;cnt<26u;++cnt) + n[cnt] = unit(dp[cnt]); + + vector3_type p; + idx2coord(phi,i,j,k,p); + real_type flux = real_type(); + for(size_t cnt=0;cnt<26u;++cnt) + { + vector3_type q = p + dp[cnt]; + vector3_type g = gradient_at_point(phi,q); + flux += unit(g)*n[cnt]; + } + flux /= 26.0; + return static_cast(flux); + } + + } //namespace detail + + + /** + * Average Outward Flux. + * + * @param phi A the distance grid. + * @param F A grid containing the values of the average outward flux. + */ + template + inline void aof( + grid_type const & phi + , grid_type & F + ) + { + using std::min; + using std::sqrt; + + typedef typename grid_type::iterator iterator; + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename grid_type::value_type value_type; + typedef typename grid_type::math_types math_types; + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::real_type real_type; + + value_type const unused = phi.unused(); + size_t const & I = phi.I(); + size_t const & J = phi.J(); + size_t const & K = phi.K(); + F.create(phi.min_coord(),phi.max_coord(),I,J,K); + + real_type dx = phi.dx()*0.5; + real_type dy = phi.dy()*0.5; + real_type dz = phi.dz()*0.5; + //real_type rho = sqrt(dx*dx+dy*dy+dz*dz); + vector3_type n[26]; + vector3_type dp[26]; + dp[0] = vector3_type( dx, 0, 0); + dp[1] = vector3_type(-dx, 0, 0); + dp[2] = vector3_type( 0, dy, 0); + dp[3] = vector3_type( 0,-dy, 0); + dp[4] = vector3_type( 0, 0, dz); + dp[5] = vector3_type( 0, 0,-dz); + dp[6] = vector3_type( dx, dy, dz); + dp[7] = vector3_type( dx, dy,-dz); + dp[8] = vector3_type( dx,-dy, dz); + dp[9] = vector3_type( dx,-dy,-dz); + dp[10] = vector3_type(-dx, dy, dz); + dp[11] = vector3_type(-dx, dy,-dz); + dp[12] = vector3_type(-dx,-dy, dz); + dp[13] = vector3_type(-dx,-dy,-dz); + dp[14] = vector3_type( dx, dy, 0); + dp[15] = vector3_type( dx,-dy, 0); + dp[16] = vector3_type(-dx, dy, 0); + dp[17] = vector3_type(-dx,-dy, 0); + dp[18] = vector3_type( dx, 0, dz); + dp[19] = vector3_type( dx, 0,-dz); + dp[20] = vector3_type(-dx, 0, dz); + dp[21] = vector3_type(-dx, 0,-dz); + dp[22] = vector3_type( 0, dy, dz); + dp[23] = vector3_type( 0, dy,-dz); + dp[24] = vector3_type( 0,-dy, dz); + dp[25] = vector3_type( 0,-dy,-dz); + for(size_t i=0;i<26u;++i) + n[i] = unit(dp[i]); + //n[0] = unit(vector3_type( 1, 0, 0)); + //n[1] = unit(vector3_type(-1, 0, 0)); + //n[2] = unit(vector3_type( 0, 1, 0)); + //n[3] = unit(vector3_type( 0,-1, 0)); + //n[4] = unit(vector3_type( 0, 0, 1)); + //n[5] = unit(vector3_type( 0, 0,-1)); + //n[6] = unit(vector3_type( 1, 1, 1)); + //n[7] = unit(vector3_type( 1, 1,-1)); + //n[8] = unit(vector3_type( 1,-1, 1)); + //n[9] = unit(vector3_type( 1,-1,-1)); + //n[10] = unit(vector3_type(-1, 1, 1)); + //n[11] = unit(vector3_type(-1, 1,-1)); + //n[12] = unit(vector3_type(-1,-1, 1)); + //n[13] = unit(vector3_type(-1,-1,-1)); + //n[14] = unit(vector3_type( 1, 1, 0)); + //n[15] = unit(vector3_type( 1,-1, 0)); + //n[16] = unit(vector3_type(-1, 1, 0)); + //n[17] = unit(vector3_type(-1,-1, 0)); + //n[18] = unit(vector3_type( 1, 0, 1)); + //n[19] = unit(vector3_type( 1, 0,-1)); + //n[20] = unit(vector3_type(-1, 0, 1)); + //n[21] = unit(vector3_type(-1, 0,-1)); + //n[22] = unit(vector3_type( 0, 1, 1)); + //n[23] = unit(vector3_type( 0, 1,-1)); + //n[24] = unit(vector3_type( 0,-1, 1)); + //n[25] = unit(vector3_type( 0,-1,-1)); + //for(size_t i=0;i<26u;++i) + // dp[i] = n[i]*rho; + iterator f = F.begin(); + const_index_iterator v = phi.begin(); + const_index_iterator vend = phi.end(); + for(;v!=vend;++v,++f) + { + if((*v)==unused) + { + (*f) = value_type(); //--- should default to zero!!! + continue; + } + size_t i = v.i(); + size_t j = v.j(); + size_t k = v.k(); + vector3_type p; + idx2coord(phi,i,j,k,p); + real_type flux = real_type(); + for(size_t i=0;i<26u;++i) + { + vector3_type q = p + dp[i]; + vector3_type g = gradient_at_point(phi,q); + flux += unit(g)*n[i]; + } + flux /= 26.0; + (*f) = static_cast(flux); + } + + //vector3_type n_mpp = unit(vector3_type( -1, 1, 1)); + //vector3_type n_mp0 = unit(vector3_type( -1, 1, 0)); + //vector3_type n_mpm = unit(vector3_type( -1, 1, -1)); + //vector3_type n_m0p = unit(vector3_type( -1, 0, 1)); + //vector3_type n_m00 = unit(vector3_type( -1, 0, 0)); + //vector3_type n_m0m = unit(vector3_type( -1, 0, -1)); + //vector3_type n_mmp = unit(vector3_type( -1, -1, 1)); + //vector3_type n_mm0 = unit(vector3_type( -1, -1, 0)); + //vector3_type n_mmm = unit(vector3_type( -1, -1, -1)); + //vector3_type n_0pp = unit(vector3_type( 0, 1, 1)); + //vector3_type n_0p0 = unit(vector3_type( 0, 1, 0)); + //vector3_type n_0pm = unit(vector3_type( 0, 1, -1)); + //vector3_type n_00p = unit(vector3_type( 0, 0, 1)); + //vector3_type n_000 = unit(vector3_type( 0, 0, 0)); + //vector3_type n_00m = unit(vector3_type( 0, 0, -1)); + //vector3_type n_0mp = unit(vector3_type( 0, -1, 1)); + //vector3_type n_0m0 = unit(vector3_type( 0, -1, 0)); + //vector3_type n_0mm = unit(vector3_type( 0, -1, -1)); + //vector3_type n_ppp = unit(vector3_type( 1, 1, 1)); + //vector3_type n_pp0 = unit(vector3_type( 1, 1, 0)); + //vector3_type n_ppm = unit(vector3_type( 1, 1, -1)); + //vector3_type n_p0p = unit(vector3_type( 1, 0, 1)); + //vector3_type n_p00 = unit(vector3_type( 1, 0, 0)); + //vector3_type n_p0m = unit(vector3_type( 1, 0, -1)); + //vector3_type n_pmp = unit(vector3_type( 1, -1, 1)); + //vector3_type n_pm0 = unit(vector3_type( 1, -1, 0)); + //vector3_type n_pmm = unit(vector3_type( 1, -1, -1)); + //vector3_type g_mpp; + //vector3_type g_mp0; + //vector3_type g_mpm; + //vector3_type g_m0p; + //vector3_type g_m00; + //vector3_type g_m0m; + //vector3_type g_mmp; + //vector3_type g_mm0; + //vector3_type g_mmm; + //vector3_type g_0pp; + //vector3_type g_0p0; + //vector3_type g_0pm; + //vector3_type g_00p; + //vector3_type g_000; + //vector3_type g_00m; + //vector3_type g_0mp; + //vector3_type g_0m0; + //vector3_type g_0mm; + //vector3_type g_ppp; + //vector3_type g_pp0; + //vector3_type g_ppm; + //vector3_type g_p0p; + //vector3_type g_p00; + //vector3_type g_p0m; + //vector3_type g_pmp; + //vector3_type g_pm0; + //vector3_type g_pmm; + //iterator f = F.begin(); + //const_index_iterator v = phi.begin(); + //const_index_iterator vend = phi.end(); + //for(;v!=vend;++v,++f) + //{ + // if((*v)==unused) + // { + // (*f) = value_type(); //--- should default to zero!!! + // continue; + // } + // size_t i = v.i(); + // size_t j = v.j(); + // size_t k = v.k(); + // size_t im1 = ( i ) ? i - 1 : 0; + // size_t jm1 = ( j ) ? j - 1 : 0; + // size_t km1 = ( k ) ? k - 1 : 0; + // size_t ip1 = min( i + 1u, I - 1u ); + // size_t jp1 = min( j + 1u, J - 1u ); + // size_t kp1 = min( k + 1u, K - 1u ); + // gradient(phi, im1, jp1, kp1, g_mpp ); + // gradient(phi, im1, jp1, k, g_mp0 ); + // gradient(phi, im1, jp1, km1, g_mpm ); + // gradient(phi, im1, j, kp1, g_m0p ); + // gradient(phi, im1, j, k, g_m00 ); + // gradient(phi, im1, j, km1, g_m0m ); + // gradient(phi, im1, jm1, kp1, g_mmp ); + // gradient(phi, im1, jm1, k, g_mm0 ); + // gradient(phi, im1, jm1, km1, g_mmm ); + // gradient(phi, i, jp1, kp1, g_0pp ); + // gradient(phi, i, jp1, k, g_0p0 ); + // gradient(phi, i, jp1, km1, g_0pm ); + // gradient(phi, i, j, kp1, g_00p ); + // gradient(phi, i, j, k, g_000 ); + // gradient(phi, i, j, km1, g_00m ); + // gradient(phi, i, jm1, kp1, g_0mp ); + // gradient(phi, i, jm1, k, g_0m0 ); + // gradient(phi, i, jm1, km1, g_0mm ); + // gradient(phi, ip1, jp1, kp1, g_ppp ); + // gradient(phi, ip1, jp1, k, g_pp0 ); + // gradient(phi, ip1, jp1, km1, g_ppm ); + // gradient(phi, ip1, j, kp1, g_p0p ); + // gradient(phi, ip1, j, k, g_p00 ); + // gradient(phi, ip1, j, km1, g_p0m ); + // gradient(phi, ip1, jm1, kp1, g_pmp ); + // gradient(phi, ip1, jm1, k, g_pm0 ); + // gradient(phi, ip1, jm1, km1, g_pmm ); + // real_type flux = real_type(); + // flux += unit(g_mpp) * n_mpp; + // flux += unit(g_mp0) * n_mp0; + // flux += unit(g_mpm) * n_mpm; + // flux += unit(g_m0p) * n_m0p; + // flux += unit(g_m00) * n_m00; + // flux += unit(g_m0m) * n_m0m; + // flux += unit(g_mmp) * n_mmp; + // flux += unit(g_mm0) * n_mm0; + // flux += unit(g_mmm) * n_mmm; + // flux += unit(g_0pp) * n_0pp; + // flux += unit(g_0p0) * n_0p0; + // flux += unit(g_0pm) * n_0pm; + // flux += unit(g_00p) * n_00p; + // flux += unit(g_000) * n_000; + // flux += unit(g_00m) * n_00m; + // flux += unit(g_0mp) * n_0mp; + // flux += unit(g_0m0) * n_0m0; + // flux += unit(g_0mm) * n_0mm; + // flux += unit(g_ppp) * n_ppp; + // flux += unit(g_pp0) * n_pp0; + // flux += unit(g_ppm) * n_ppm; + // flux += unit(g_p0p) * n_p0p; + // flux += unit(g_p00) * n_p00; + // flux += unit(g_p0m) * n_p0m; + // flux += unit(g_pmp) * n_pmp; + // flux += unit(g_pm0) * n_pm0; + // flux += unit(g_pmm) * n_pmm; + // flux /= 26.0; + // (*f) = static_cast(flux); + //} + + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_AOF_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_approximate_gaussian_filter.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_approximate_gaussian_filter.h new file mode 100644 index 000000000..d246f8ebd --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_approximate_gaussian_filter.h @@ -0,0 +1,86 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_APPROXIMATE_GAUSSIAN_FILTER_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_APPROXIMATE_GAUSSIAN_FILTER_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * Fast convolution by an approximate "integer" guassian filter. + * This just applies several box_filter's (boxsize=size and order times), + * and is only efficient for low orders. + * If size is even, the order must also be even for the resulting image to lay on the same grid + * (else translated 0.5 voxel towards (0,0,0) ). + * @param src Source grid to be convolved. + * @param order Order of filter. Corresponds to the number of times the boxfilter is applied. + * @param size Size of filter. + * @param dst Upon return, contains the filtered grid. + */ + template + inline void approximate_gaussian_filter(grid_type const& src, size_t order, size_t size, grid_type& dst) + { + typedef typename grid_type::index_vector index_vector; + dst=src; + for(size_t i=0; i0 && (i%2) && !(size%2)) + { + std::cout << "--calling translate()" << std::endl; + translate(dst, index_vector(1,1,1), dst); + } + } + } + + /** + * Approximate Gaussian filter with compensation on image borders. + * Attenuation on border regions is compensated. + * This is equivalent to Gaussian filtering using only filter coefficients + * that are inside the "shape" being considered. + * @param src Source grid to be convolved. + * @param order Order of filter. Corresponds to the number of times the boxfilter is applied. + * @param size Size of filter. + * @param dst Upon return, contains the filtered grid. + */ + template + inline void approximate_gaussian_filter_border_correct(grid_type const& src, size_t order, size_t size, grid_type &dst) + { + grid_type lowFreq=src; + + // apply filter in normal fashion + approximate_gaussian_filter(lowFreq, order, size, lowFreq); + + // compensate for border regions in filtering + // FIXME: refactor this! + /* { + Image3Df smask(src.Size()); + smask.Fill(1); + approximate_gaussian_filter(smask,order,size,smask); + for(typename grid_type::index_iterator p=lowFreq.begin(); p!=lowFreq.end(); ++p) + { + if(smask(p.Pos())) + { + *p/=smask(p.Pos()); + } + } + }*/ + dst=lowFreq; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_APPROXIMATE_GAUSSIAN_FILTER_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_average.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_average.h new file mode 100644 index 000000000..f2d21207e --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_average.h @@ -0,0 +1,90 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_AVERAGE_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_AVERAGE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace grid + { + + template < typename grid_type > + inline void average( grid_type & phi ) + { + using std::min; + + typedef typename grid_type::index_iterator iterator; + typedef typename grid_type::value_type value_type; + + grid_type tmp = phi; + + size_t I = phi.I(); + size_t J = phi.J(); + size_t K = phi.K(); + + iterator pend = phi.end(); + iterator p = phi.begin(); + iterator t = tmp.begin(); + + for(;p!=pend;++p,++t) + { + size_t i = p.i(); + size_t j = p.j(); + size_t k = p.k(); + + static size_t idx[27]; + size_t im1 = ( i ) ? i - 1 : 0; + size_t jm1 = ( j ) ? j - 1 : 0; + size_t km1 = ( k ) ? k - 1 : 0; + size_t ip1 = min( i + 1u, I - 1u ); + size_t jp1 = min( j + 1u, J - 1u ); + size_t kp1 = min( k + 1u, K - 1u ); + idx[0] = ( kp1 * J + jp1 ) * I + im1; + idx[1] = ( k * J + jp1 ) * I + im1; + idx[2] = ( km1 * J + jp1 ) * I + im1; + idx[3] = ( kp1 * J + j ) * I + im1; + idx[4] = ( k * J + j ) * I + im1; + idx[5] = ( km1 * J + j ) * I + im1; + idx[6] = ( kp1 * J + jm1 ) * I + im1; + idx[7] = ( k * J + jm1 ) * I + im1; + idx[8] = ( km1 * J + jm1 ) * I + im1; + idx[9] = ( kp1 * J + jp1 ) * I + i; + idx[10] = ( k * J + jp1 ) * I + i; + idx[11] = ( km1 * J + jp1 ) * I + i; + idx[12] = ( kp1 * J + j ) * I + i; + idx[13] = ( k * J + j ) * I + i; + idx[14] = ( km1 * J + j ) * I + i; + idx[15] = ( kp1 * J + jm1 ) * I + i; + idx[16] = ( k * J + jm1 ) * I + i; + idx[17] = ( km1 * J + jm1 ) * I + i; + idx[18] = ( kp1 * J + jp1 ) * I + ip1; + idx[19] = ( k * J + jp1 ) * I + ip1; + idx[20] = ( km1 * J + jp1 ) * I + ip1; + idx[21] = ( kp1 * J + j ) * I + ip1; + idx[22] = ( k * J + j ) * I + ip1; + idx[23] = ( km1 * J + j ) * I + ip1; + idx[24] = ( kp1 * J + jm1 ) * I + ip1; + idx[25] = ( k * J + jm1 ) * I + ip1; + idx[26] = ( km1 * J + jm1 ) * I + ip1; + value_type avg = value_type(); //--- default constructed zero by standard!!! + for(size_t i=0;i<27u;++i) + avg += phi(idx[i]); + avg /= value_type(25); + *t = avg; + } + phi = tmp; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_AVERAGE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_bisection_line_search.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_bisection_line_search.h new file mode 100644 index 000000000..870dba7ff --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_bisection_line_search.h @@ -0,0 +1,76 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_BISECTION_LINE_SEARCH_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_BISECTION_LINE_SEARCH_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * Grid Bisection Line Search + * + * @param q_a + * @param q_b + * @param phi + * @param maximize If true the bisection method tries to find the maximimum value between q_a and q_b otherwise it tries to find the minimum value. + * + * @return The point that maximizes the value of phi on the line between q_a and q_b. + */ + template + inline vector3_type bisection_line_search(vector3_type q_a, vector3_type q_b, grid_type & phi, bool maximize = true) + { + using std::fabs; + typedef typename vector3_type::value_type real_type; + + real_type const precision = 10e-5;//OpenTissue::math::working_precision(100); + real_type const too_small_interval = sqr_length(q_b-q_a)*0.0001; //--- 1/100'th of distance! + vector3_type n = unit(gradient_at_point(phi,q_a)); + vector3_type r; + + + real_type const sign = maximize? 1.0 : -1.0; + + + bool forever = true; + do + { + vector3_type q_c = (q_a + q_b)*.5; + if( sqr_length(q_a - q_b) < too_small_interval ) + { + r = q_c; + break; + } + vector3_type dir = unit(gradient_at_point(phi,q_c)); + real_type n_dot_dir = inner_prod(n , dir)*sign; + if(fabs(n_dot_dir) < precision) + { + r = q_c; + break; + } + if(n_dot_dir > 0) + { + q_a = q_c; + } + if(n_dot_dir < 0) + { + q_b = q_c; + } + } + while (forever); + return r; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_BISECTION_LINE_SEARCH_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_blockify.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_blockify.h new file mode 100644 index 000000000..4122a72ba --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_blockify.h @@ -0,0 +1,139 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_BLOCKIFY_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_BLOCKIFY_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + + +#include +#include +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * Level Set Blockifier. + * This function takes a level set grid and re-initializes it into + * small cubic blocks (-1 inside and +1 outside). + * + * @param phi The level set grid. + */ + template + inline void blockify(grid_type & phi) + { + typedef typename grid_type::value_type value_type; + typedef typename grid_type::index_iterator iterator; + + value_type unused = phi.unused(); + value_type inside = value_type(-1.0); + value_type outside = value_type( 1.0); + + size_t offset = 7u; + size_t spacing = 11u; + size_t block = offset + spacing; + iterator begin = phi.begin(); + iterator end = phi.end(); + iterator p; + for(p=begin;p!=end;++p) + { + if(*p==unused) + continue; + + bool inside_i = ((p.i() % block)>offset); + bool inside_j = ((p.j() % block)>offset); + bool inside_k = ((p.k() % block)>offset); + if(inside_i && inside_j && inside_k) + *p = inside; + else + *p = outside; + } + } + + /** Initialize a grid with a pattern of boxes of width offset. + * + * @param phi The grid to initialize. + * @param inside Value to fill inside blocks + * @param outside Value to fill outside blocks + * @param offset The offset from the borders. + * @param spacing The width, height, depth and half the spacing of boxes. + */ + template + inline void blockify(grid_type & phi, value_type inside, value_type outside, size_t offset, size_t spacing) + { + typedef typename grid_type::value_type internal_type; + typedef typename grid_type::index_iterator iterator; + + internal_type unused = phi.unused(); + internal_type inside_ = (internal_type)( inside ); + internal_type outside_ = (internal_type)( outside ); + + size_t block = offset + spacing; + iterator begin = phi.begin(); + iterator end = phi.end(); + iterator p; + for(p=begin;p!=end;++p) + { + if(*p==unused) + continue; + + bool inside_i = ((p.i() % block)>offset); + bool inside_j = ((p.j() % block)>offset); + bool inside_k = ((p.k() % block)>offset); + if(inside_i && inside_j && inside_k) + *p = inside_; + else + *p = outside_; + } + } + + /** Initialize a slice of a grid with a pattern of boxes of width offset. + * + * @param phi The grid to initialize. + * @param inside Value to fill inside blocks + * @param outside Value to fill outside blocks + * @param offset The offset from the borders. + * @param spacing The width, height, and half the spacing of boxes. + * @param slice The slice in z-depth that needs to be filled + */ + template + inline void blockify(grid_type & phi, value_type inside, value_type outside, size_t offset, size_t spacing, size_t slice) + { + typedef typename grid_type::value_type internal_type; + typedef typename grid_type::index_iterator iterator; + + internal_type unused = phi.unused(); + internal_type inside_ = (internal_type)( inside ); + internal_type outside_ = (internal_type)( outside ); + + size_t block = offset + spacing; + iterator begin = phi.begin(); + iterator end = phi.end(); + iterator p; + for(p=begin;p!=end;++p) + { + if (p.k()!=slice) + continue; + if(*p==unused) + continue; + bool inside_i = ((p.i() % block)>offset); + bool inside_j = ((p.j() % block)>offset); + if(inside_i && inside_j) + *p = inside_; + else + *p = outside_; + } + } + + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_BLOCKIFY_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_closing.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_closing.h new file mode 100644 index 000000000..9305249ac --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_closing.h @@ -0,0 +1,47 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_CLOSING_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_CLOSING_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * Closing Operation. + * + * @param phi Input level set. + * @param radius Radius of spherical structural element. + * @param dt Time-step to use in update. + * @param psi Output levelset. Note if input is a signed distance map then output may not be a signed distance map, thus you may need to redistance output levelset. + */ + template< + typename grid_type_in + , typename real_type + , typename grid_type_out + > + inline void closing( + grid_type_in const & phi + , real_type const & radius + , real_type const & dt + , grid_type_out & psi + ) + { + erosion(phi,radius,dt,psi); + dilation(psi,radius,dt,psi); + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_CLOSING_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_compute_sign_function.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_compute_sign_function.h new file mode 100644 index 000000000..a559fe93e --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_compute_sign_function.h @@ -0,0 +1,49 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_COMPUTE_SIGN_FUNCTION_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_COMPUTE_SIGN_FUNCTION_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace grid + { + + template < typename grid_type > + inline void compute_sign_function( grid_type const & phi, grid_type & S0 ) + { + using std::sqrt; + + typedef typename grid_type::value_type real_type; + typedef typename grid_type::const_index_iterator const_iterator; + typedef typename grid_type::index_iterator iterator; + + S0.create(phi.min_coord(),phi.max_coord(),phi.I(),phi.J(),phi.K()); + // yellowbook p67. + + real_type delta = phi.dx()*phi.dx() + phi.dy()*phi.dy() + phi.dz()*phi.dz(); + const_iterator begin = phi.begin(); + const_iterator end = phi.end(); + + for ( const_iterator idx = begin;idx!=end;++idx) + { + size_t i = idx.i(); + size_t j = idx.j(); + size_t k = idx.k(); + real_type c = phi( i,j,k ); + S0( i,j,k ) = c / ( sqrt( c * c + delta ) );//---TODO: Verify this formula!!! + } + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_COMPUTE_SIGN_FUNCTION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_coord2idx.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_coord2idx.h new file mode 100644 index 000000000..0b269ae05 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_coord2idx.h @@ -0,0 +1,65 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_COORD_TO_IDX_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_COORD_TO_IDX_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * Coordinate 2 Voxel Indices. + * This method computes the indices (i,j,k) of + * the voxel containing the specified point. + * + * The voxel index of the voxel containing the points is equal to index + * of the grid node of the containing voxel with lowest coordinates + * (ie. lower-left-back grid node). + * + * @param grid The grid. + * @param point The point. + * @param i Upon return this parameter contains the index of the voxel along the I-axe. + * @param j Upon return this parameter contains the index of the voxel along the J-axe. + * @param k Upon return this parameter contains the index of the voxel along the K-axe. + */ + template + inline void coord2idx( grid_type const & grid, vector3_type const & point, size_t & i, size_t & j, size_t & k ) + { + using std::floor; + + typedef typename vector3_type::value_type real_type; + + real_type const & dx = grid.dx(); + real_type const & dy = grid.dy(); + real_type const & dz = grid.dz(); + real_type const & mx = grid.min_coord( 0 ); + real_type const & my = grid.min_coord( 1 ); + real_type const & mz = grid.min_coord( 2 ); + + real_type xval = ( point( 0 ) - mx ) / dx; + i = static_cast( floor( xval ) ); + if ( ( xval - i ) > .5 ) + ++i; + real_type yval = ( point( 1 ) - my ) / dy; + j = static_cast( floor( yval ) ); + if ( ( yval - j ) > .5 ) + ++j; + real_type zval = ( point( 2 ) - mz ) / dz; + k = static_cast( floor( zval ) ); + if ( ( zval - k ) > .5 ) + ++k; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_COORD_TO_IDX_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_crop.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_crop.h new file mode 100644 index 000000000..8ffaba834 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_crop.h @@ -0,0 +1,116 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_CROP_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_CROP_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +#include +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * Crop grid to bounding box that do not include treshold. + * Can be used to automatically crop emptyness from scanned data. + * @param M Original grid to be cropped. + * @param m Destination grid. + * @param treshold Maximum value that needs to be cropped. + * @return Upon return the destination grid m contains the cropped grid. + */ + template < typename grid_type > + inline void crop(grid_type const & M, grid_type & m, typename grid_type::value_type const & treshold) + { + using std::min; + using std::max; + + typedef typename grid_type::math_types math_types; + typedef typename math_types::vector3_type vector3_type; + + typedef OpenTissue::math::Vector3 index_vector; + + index_vector min_idx( M.I(), M.J(), M.K() ); + index_vector max_idx( 0, 0, 0 ); + + for(size_t k=0; k treshold ) + { + min_idx = min( min_idx, index_vector(i,j,k) ); + max_idx = max( max_idx, index_vector(i,j,k) ); + } + } + index_vector new_dim = (max_idx - min_idx) + index_vector(1); + + vector3_type min_coord,max_coord; + idx2coord(M, min_idx(0), min_idx(1), min_idx(2),min_coord); + idx2coord(M, max_idx(0), max_idx(1), max_idx(2),max_coord); + + m.create( min_coord, max_coord, new_dim(0), new_dim(1), new_dim(2) ); + + size_t i_offset = min_idx(0); + size_t j_offset = min_idx(1); + size_t k_offset = min_idx(2); + for(size_t k=0; k + inline void crop( + grid_type const & M + , grid_type & m + , size_t min_i + , size_t min_j + , size_t min_k + , size_t max_i + , size_t max_j + , size_t max_k + ) + { + typedef typename grid_type::math_types math_types; + typedef typename math_types::vector3_type vector3_type; + typedef typename grid_type::index_iterator index_iterator; + + vector3_type min_coord,max_coord; + idx2coord(M, min_i, min_j, min_k, min_coord); + idx2coord(M, max_i, max_j, max_k, max_coord); + m.create( min_coord, max_coord, max_i-min_i+1, max_j-min_j+1, max_k-min_k+1 ); + + for(size_t k=0; k + +#include + +namespace OpenTissue +{ +namespace grid +{ + + /** + * Calculate curvature flow. + * + * WARNING: Unexpected behavior if input and output level sets are the same. + * + * + * @param phi Input level set. + * @param mu Mean Curvature coefficient. + * @param dt Time-step to use in update. + * @param psi Output levelset. Note if input is a signed distance map + * then output may not be a signed distance map, thus + * you may need to redistance output levelset. + */ + template< + typename grid_type + , typename real_type + > + inline void curvature_flow( + grid_type const & phi + , real_type const & mu + , real_type const & dt + , grid_type & psi + ) + { + using std::min; + using std::max; + using std::sqrt; + + typedef typename grid_type::value_type value_type; + typedef typename grid_type::iterator iterator; + typedef typename grid_type::index_iterator index_iterator; + typedef typename grid_type::const_index_iterator const_index_iterator; + + assert(mu>0 || !"curvature_flow(): curvature coefficient must be positive"); + assert(dt>0 || !"curvature_flow(): time-step must be positive"); + + size_t I = phi.I(); + size_t J = phi.J(); + size_t K = phi.K(); + real_type dx = static_cast ( phi.dx() ); + real_type dy = static_cast ( phi.dy() ); + real_type dz = static_cast ( phi.dz() ); + real_type m_inv_dx2 = static_cast ( 1.0 / (dx*dx) ); + real_type m_inv_dy2 = static_cast ( 1.0 / (dy*dy) ); + real_type m_inv_dz2 = static_cast ( 1.0 / (dz*dz) ); + real_type m_inv_4dxy = static_cast ( 0.25 / (dx*dy) ); + real_type m_inv_4dxz = static_cast ( 0.25 / (dx*dz) ); + real_type m_inv_4dyz = static_cast ( 0.25 / (dy*dz) ); + + real_type min_delta = static_cast (min(dx,min(dy,dz) )); + + real_type kappa_limit = static_cast ( 1.0 / ( max(dx, max(dy,dz) ) ) ); + + value_type zero = static_cast(0.0); + grid_type F = phi; //--- speed function + + if(&psi != &phi) + psi = phi; + + iterator fbegin = F.begin(); + index_iterator sbegin = psi.begin(); + index_iterator send = psi.end(); + + real_type threshold = static_cast(10e-15); //--- small number used to avoid division by zero!!! + + real_type time = static_cast(0.0); + while (time < dt) + { + value_type max_F = zero; //--- maximum speed, used to setup CFL condition + iterator f = fbegin; + index_iterator s = sbegin; + for(;s!=send; ++s,++f) + { + size_t i = s.i(); + size_t j = s.j(); + size_t k = s.k(); + + size_t im1 = ( i ) ? i - 1 : 0; + size_t jm1 = ( j ) ? j - 1 : 0; + size_t km1 = ( k ) ? k - 1 : 0; + size_t ip1 = min( i + 1, I - 1 ); + size_t jp1 = min( j + 1, J - 1 ); + size_t kp1 = min( k + 1, K - 1 ); + + size_t idx_000 = ( k * J + j ) * I + i; + size_t idx_m00 = ( k * J + j ) * I + im1; + size_t idx_p00 = ( k * J + j ) * I + ip1; + size_t idx_0m0 = ( k * J + jm1 ) * I + i; + size_t idx_0p0 = ( k * J + jp1 ) * I + i; + size_t idx_00m = ( km1 * J + j ) * I + i; + size_t idx_00p = ( kp1 * J + j ) * I + i; + size_t idx_pp0 = ( k * J + jp1 ) * I + ip1; + size_t idx_mp0 = ( k * J + jp1 ) * I + im1; + size_t idx_pm0 = ( k * J + jm1 ) * I + ip1; + size_t idx_mm0 = ( k * J + jm1 ) * I + im1; + size_t idx_p0p = ( kp1 * J + j ) * I + ip1; + size_t idx_m0p = ( kp1 * J + j ) * I + im1; + size_t idx_p0m = ( km1 * J + j ) * I + ip1; + size_t idx_m0m = ( km1 * J + j ) * I + im1; + size_t idx_0pp = ( kp1 * J + jp1 ) * I + i; + size_t idx_0pm = ( km1 * J + jp1 ) * I + i; + size_t idx_0mp = ( kp1 * J + jm1 ) * I + i; + size_t idx_0mm = ( km1 * J + jm1 ) * I + i; + + real_type d000 = 2.0 * psi( idx_000 ); + real_type dp00 = psi( idx_p00 ); + real_type dm00 = psi( idx_m00 ); + real_type d0p0 = psi( idx_0p0 ); + real_type d0m0 = psi( idx_0m0 ); + real_type d00p = psi( idx_00p ); + real_type d00m = psi( idx_00m ); + real_type dpp0 = psi( idx_pp0 ); + real_type dmp0 = psi( idx_mp0 ); + real_type dpm0 = psi( idx_pm0 ); + real_type dmm0 = psi( idx_mm0 ); + real_type dp0p = psi( idx_p0p ); + real_type dm0p = psi( idx_m0p ); + real_type dp0m = psi( idx_p0m ); + real_type dm0m = psi( idx_m0m ); + real_type d0pp = psi( idx_0pp ); + real_type d0pm = psi( idx_0pm ); + real_type d0mp = psi( idx_0mp ); + real_type d0mm = psi( idx_0mm ); + //---- Hessian matrix is defines as + // + // | d/(dx*dx) d(/(dx*dy) d/(dx*dz) | + // H = | d/(dy*dx) d(/(dy*dy) d/(dy*dz) | phi + // | d/(dz*dx) d(/(dz*dy) d/(dz*dz) | + // + // + //--- Following is a central diff approximation + real_type psi_x = (dp00 - dm00) * m_inv_dx2; + real_type psi_y = (d0p0 - d0m0) * m_inv_dy2; + real_type psi_z = (d00p - d00m) * m_inv_dz2; + real_type psi_xx = ( dp00 + dm00 - d000 ) * m_inv_dx2; + real_type psi_yy = ( d0p0 + d0m0 - d000 ) * m_inv_dy2; + real_type psi_zz = ( d00p + d00m - d000 ) * m_inv_dz2; + real_type psi_xy = ( dpp0 - dmp0 - dpm0 + dmm0 ) * m_inv_4dxy; + real_type psi_xz = ( dp0p - dm0p - dp0m + dm0m ) * m_inv_4dxz; + real_type psi_yz = ( d0pp - d0pm - d0mp + d0mm ) * m_inv_4dyz; + real_type norm_grad_psi = sqrt( (psi_x*psi_x) + (psi_y*psi_y) + (psi_z*psi_z) ); + real_type kappa = + ( + (psi_x*psi_x)*psi_yy + + (psi_x*psi_x)*psi_zz + + (psi_y*psi_y)*psi_xx + + (psi_y*psi_y)*psi_zz + + (psi_z*psi_z)*psi_xx + + (psi_z*psi_z)*psi_yy + - 2*(psi_x*psi_y)*psi_xy + - 2*(psi_x*psi_z)*psi_xz + - 2*(psi_y*psi_z)*psi_yz + ) + / + (norm_grad_psi*norm_grad_psi*norm_grad_psi + threshold ); + + kappa = max( kappa, -kappa_limit ); + kappa = min( kappa, kappa_limit ); + + *f = static_cast(mu * kappa); + max_F = max(max_F, std::fabs(*f)); + } + real_type time_left = max( dt - time, 0.0 ); + if(time_left <= 0) + return; + + value_type time_step = static_cast( min( time_left, min_delta / (max_F) ) ); + + std::cout << "\tcurvature_flow() : timestep = " << time_step << std::endl; + + for(s=sbegin, f=fbegin;s!=send;++s,++f) + (*s) = (*s) + time_step * (*f); + time += time_step; + } + } + +} // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_CURVATURE_FLOW_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_dilation.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_dilation.h new file mode 100644 index 000000000..8ce3ad10f --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_dilation.h @@ -0,0 +1,57 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_DILATION_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_DILATION_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * Dilate level set. + * + * @param phi Input level set. + * @param radius Radius of spherical structural element. + * @param dt Time-step to use in update. + * @param psi Output levelset. Note if input is a signed distance map then output may not be a signed distance map, thus you may need to redistance output levelset. + */ + template< + typename grid_type_in + , typename real_type + , typename grid_type_out + > + inline void dilation( + grid_type_in const & phi + , real_type const & radius + , real_type const & dt + , grid_type_out & psi + ) + { + typedef typename grid_type_in::value_type input_type; + typedef typename grid_type_out::value_type output_type; + typedef typename grid_type_in::const_index_iterator input_iterator; + + assert(radius>0 || !"dilation(): radius must be positive"); + assert(dt>0 || !"dilation(): time-step must be positive"); + + input_iterator input_begin = phi.begin(); + input_iterator input_end = phi.end(); + input_iterator input; + input_type unused = phi.unused(); + for(input=input_begin;input!=input_end;++input) + if(*input!=unused) + psi(input.get_index()) = *input - dt*radius; + } + + } // namespace grid + +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_DILATION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_div_grad.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_div_grad.h new file mode 100644 index 000000000..b4e671c67 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_div_grad.h @@ -0,0 +1,167 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_DIV_GRAD_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_DIV_GRAD_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include + +namespace OpenTissue +{ + namespace grid + { + + /** + * Div-Grad. + * Computes the divergence of the gradient of a specified field. Ie. the flux of the gradient field! + * + * @param phi A the grid. + * @param M A grid containing the values of the divergence of the gradient field of phi. + */ + template + inline void div_grad( + grid_type const & phi + , grid_type & M + ) + { + using std::min; + + typedef OpenTissue::math::Vector3< typename grid_type::value_type> vector3_type; + + typedef typename grid_type::iterator iterator; + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename grid_type::value_type value_type; + typedef typename grid_type::math_types math_types; + typedef typename math_types::real_type real_type; + + size_t const & I = phi.I(); + size_t const & J = phi.J(); + size_t const & K = phi.K(); + M.create(phi.min_coord(),phi.max_coord(),I,J,K); + static value_type unused = phi.unused(); + + vector3_type /*g000,*/gp00,gm00,g0p0,g0m0,g00p,g00m; + vector3_type gppp,gppm,gpmp,gpmm,gmpp,gmpm,gmmp,gmmm; + vector3_type gpp0, gpm0, gmp0, gmm0, gp0p, gp0m, gm0p, gm0m, g0pp, g0pm, g0mp, g0mm; + + static vector3_type np00 = unit(vector3_type( 1, 0, 0)); + static vector3_type nm00 = unit(vector3_type(-1, 0, 0)); + static vector3_type n0p0 = unit(vector3_type( 0, 1, 0)); + static vector3_type n0m0 = unit(vector3_type( 0,-1, 0)); + static vector3_type n00p = unit(vector3_type( 0, 0, 1)); + static vector3_type n00m = unit(vector3_type( 0, 0,-1)); + static vector3_type nppp = unit(vector3_type( 1, 1, 1)); + static vector3_type nppm = unit(vector3_type( 1, 1,-1)); + static vector3_type npmp = unit(vector3_type( 1,-1, 1)); + static vector3_type npmm = unit(vector3_type( 1,-1,-1)); + static vector3_type nmpp = unit(vector3_type(-1, 1, 1)); + static vector3_type nmpm = unit(vector3_type(-1, 1,-1)); + static vector3_type nmmp = unit(vector3_type(-1,-1, 1)); + static vector3_type nmmm = unit(vector3_type(-1,-1,-1)); + static vector3_type npp0 = unit(vector3_type( 1, 1, 0)); + static vector3_type npm0 = unit(vector3_type( 1,-1, 0)); + static vector3_type nmp0 = unit(vector3_type(-1, 1, 0)); + static vector3_type nmm0 = unit(vector3_type(-1,-1, 0)); + static vector3_type np0p = unit(vector3_type( 1, 0, 1)); + static vector3_type np0m = unit(vector3_type( 1, 0,-1)); + static vector3_type nm0p = unit(vector3_type(-1, 0, 1)); + static vector3_type nm0m = unit(vector3_type(-1, 0,-1)); + static vector3_type n0pp = unit(vector3_type( 0, 1, 1)); + static vector3_type n0pm = unit(vector3_type( 0, 1,-1)); + static vector3_type n0mp = unit(vector3_type( 0,-1, 1)); + static vector3_type n0mm = unit(vector3_type( 0,-1,-1)); + + + iterator m = M.begin(); + const_index_iterator pbegin = phi.begin(); + const_index_iterator pend = phi.end(); + const_index_iterator p = pbegin; + for(;p!=pend;++p,++m) + { + if((*p)==unused) + { + *m = value_type(); //--- should default to zero!!! + continue; + } + + size_t i = p.i(); + size_t j = p.j(); + size_t k = p.k(); + size_t im1 = ( i ) ? i - 1 : 0; + size_t jm1 = ( j ) ? j - 1 : 0; + size_t km1 = ( k ) ? k - 1 : 0; + size_t ip1 = min( i + 1u, I - 1u ); + size_t jp1 = min( j + 1u, J - 1u ); + size_t kp1 = min( k + 1u, K - 1u ); + + gradient(phi, ip1, j, k, gp00); + gradient(phi, im1, j, k, gm00); + gradient(phi, i, jp1, k, g0p0); + gradient(phi, i, jm1, k, g0m0); + gradient(phi, i, j, kp1, g00p); + gradient(phi, i, j, km1, g00m); + gradient(phi, ip1, jp1, k, gpp0); + gradient(phi, ip1, jm1, k, gpm0); + gradient(phi, im1, jp1, k, gmp0); + gradient(phi, im1, jm1, k, gmm0); + gradient(phi, ip1, j, kp1, gp0p); + gradient(phi, ip1, j, km1, gp0m); + gradient(phi, im1, j, kp1, gm0p); + gradient(phi, im1, j, km1, gm0m); + gradient(phi, i, jp1, kp1, g0pp); + gradient(phi, i, jp1, km1, g0pm); + gradient(phi, i, jm1, kp1, g0mp); + gradient(phi, i, jm1, km1, g0mm); + gradient(phi, ip1, jp1, kp1, gppp); + gradient(phi, ip1, jp1, km1, gppm); + gradient(phi, ip1, jm1, kp1, gpmp); + gradient(phi, ip1, jm1, km1, gpmm); + gradient(phi, im1, jp1, kp1, gmpp); + gradient(phi, im1, jp1, km1, gmpm); + gradient(phi, ip1, jm1, kp1, gmmp); + gradient(phi, im1, jm1, km1, gmmm); + + real_type flux = real_type(); //--- should default to zero!!! + flux += gp00 * np00; + flux += gm00 * nm00; + flux += g0p0 * n0p0; + flux += g0m0 * n0m0; + flux += g00p * n00p; + flux += g00m * n00m; + flux += gpp0 * npp0; + flux += gpm0 * npm0; + flux += gmp0 * nmp0; + flux += gmm0 * nmm0; + flux += gp0p * np0p; + flux += gp0m * np0m; + flux += gm0p * nm0p; + flux += gm0m * nm0m; + flux += g0pp * n0pp; + flux += g0pm * n0pm; + flux += g0mp * n0mp; + flux += g0mm * n0mm; + flux += gppp * nppp; + flux += gppm * nppm; + flux += gpmp * npmp; + flux += gpmm * npmm; + flux += gmpp * nmpp; + flux += gmpm * nmpm; + flux += gmmp * nmmp; + flux += gmmm * nmmm; + + (*m) = static_cast(flux); + } + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_DIV_GRAD_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_enclosing_indices.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_enclosing_indices.h new file mode 100644 index 000000000..f94073ded --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_enclosing_indices.h @@ -0,0 +1,79 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_ENCLOSING_INDICES_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_ENCLOSING_INDICES_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * Get Enclosing Indices. + * Finds enclosing node indices for a given point. + * Assumes that the point is strictly inside the volume. + * + * @param grid The grid. + * @param point Vector with coordinates of the point. + * @param i0 Upon return, this parameter contains the i-index of the lower-left-back node. + * @param j0 Upon return, this parameter contains the j-index of the lower-left-back node. + * @param k0 Upon return, this parameter contains the k-index of the lower-left-back node. + * @param i1 Upon return, this parameter contains the i-index of the upper-right-front node. + * @param j1 Upon return, this parameter contains the j-index of the upper-right-front node. + * @param k1 Upon return, this parameter contains the k-index of the upper-right-front node. + */ + template + inline void enclosing_indices( + grid_type const & grid + , vector3_type const & point + , size_t& i0 + , size_t& j0 + , size_t& k0 + , size_t& i1 + , size_t& j1 + , size_t& k1 + ) + { + using std::max; + using std::min; + using std::floor; + + typedef typename vector3_type::value_type real_type; + + real_type const & dx = grid.dx(); + real_type const & dy = grid.dy(); + real_type const & dz = grid.dz(); + size_t const & I = grid.I(); + size_t const & J = grid.J(); + size_t const & K = grid.K(); + + vector3_type range = grid.max_coord() - grid.min_coord(); + vector3_type diff = point - grid.min_coord(); + + diff = min( max(diff, vector3_type( real_type() ) ) , range); + + diff(0) /= dx; + diff(1) /= dy; + diff(2) /= dz; + + i0 = static_cast( floor( diff(0) ) ); + j0 = static_cast( floor( diff(1) ) ); + k0 = static_cast( floor( diff(2) ) ); + i1 = ( i0 + 1 ) % I; + j1 = ( j0 + 1 ) % J; + k1 = ( k0 + 1 ) % K; + + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_ENCLOSING_INDICES_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_erosion.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_erosion.h new file mode 100644 index 000000000..ce270382c --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_erosion.h @@ -0,0 +1,56 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_EROSION_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_EROSION_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * Erode level set. + * + * @param phi Input level set. + * @param radius Radius of spherical structural element. + * @param dt Time-step to use in update. + * @param psi Output levelset. Note if input is a signed distance map then output may not be a signed distance map, thus you may need to redistance output levelset. + */ + template< + typename grid_type_in + , typename real_type + , typename grid_type_out + > + inline void erosion( + grid_type_in const & phi + , real_type const & radius + , real_type const & dt + , grid_type_out & psi + ) + { + typedef typename grid_type_in::value_type input_type; + typedef typename grid_type_out::value_type output_type; + typedef typename grid_type_in::const_index_iterator input_iterator; + + assert(radius>0 || !"erosion(): radius must be positive"); + assert(dt>0 || !"erosion(): time-step must be positive"); + + input_iterator input_begin = phi.begin(); + input_iterator input_end = phi.end(); + input_iterator input; + input_type unused = phi.unused(); + for(input=input_begin;input!=input_end;++input) + if(*input!=unused) + psi(input.get_index()) = *input + dt*radius; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_EROSION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_extrapolation.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_extrapolation.h new file mode 100644 index 000000000..65d3c9210 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_extrapolation.h @@ -0,0 +1,136 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_EXTRAPOLATION_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_EXTRAPOLATION_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +#include +#include +#include + +namespace OpenTissue +{ + namespace grid + { + + /** + * + * @param max_iterations The maximum number of iterations allowed to do extrapolation. + * @param stead_threshold The threshold value used to test for steady state. + */ + template < typename grid_type > + inline void extrapolation( + grid_type & S + , grid_type const & phi + , size_t max_iterations = 10 + // , double steady_threshold = 0.05 + ) + { + using std::min; + using std::max; + using std::fabs; + + typedef OpenTissue::math::Vector3< typename grid_type::value_type> vector3_type; + + typedef typename grid_type::iterator iterator; + typedef typename grid_type::index_iterator index_iterator; + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename grid_type::value_type value_type; + typedef typename grid_type::math_types math_types; + typedef typename math_types::real_type real_type; + + Gradient gradient; + + size_t I = S.I(); + size_t J = S.J(); + size_t K = S.K(); + + real_type dx = S.dx(); + real_type dy = S.dy(); + real_type dz = S.dz(); + + value_type zero = static_cast(0.0); + + grid_type F = S; //--- speed function + grid_type sign_phi; //--- sign function + compute_sign_function(phi,sign_phi); + + iterator fbegin = F.begin(); + index_iterator sbegin = S.begin(); + index_iterator send = S.end(); + + size_t iteration = 0; + bool steady_state = false; + + while (!steady_state) + { + value_type max_F = zero; //--- maximum speed, used to setup CFL condition + const_index_iterator p = phi.begin(); + iterator s_phi = sign_phi.begin(); + iterator f = fbegin; + index_iterator s = sbegin; + for(;s!=send; ++s,++f,++s_phi,++p) + { + size_t i = s.i(); + size_t j = s.j(); + size_t k = s.k(); + + vector3_type n = gradient(p); + + size_t im1 = ( i ) ? i - 1 : 0; + size_t jm1 = ( j ) ? j - 1 : 0; + size_t km1 = ( k ) ? k - 1 : 0; + size_t ip1 = min( i + 1u, I - 1u ); + size_t jp1 = min( j + 1u, J - 1u ); + size_t kp1 = min( k + 1u, K - 1u ); + size_t idx = ( k * J + j ) * I + i; + size_t idx_im1 = ( k * J + j ) * I + im1; + size_t idx_ip1 = ( k * J + j ) * I + ip1; + size_t idx_jm1 = ( k * J + jm1 ) * I + i; + size_t idx_jp1 = ( k * J + jp1 ) * I + i; + size_t idx_km1 = ( km1 * J + j ) * I + i; + size_t idx_kp1 = ( kp1 * J + j ) * I + i; + value_type s_idx = S(idx); + value_type s_idx_im1 = S(idx_im1); + value_type s_idx_ip1 = S(idx_ip1); + value_type s_idx_jm1 = S(idx_jm1); + value_type s_idx_jp1 = S(idx_jp1); + value_type s_idx_km1 = S(idx_km1); + value_type s_idx_kp1 = S(idx_kp1); + value_type Spx = (s_idx_ip1 - s_idx )/ dx; + value_type Spy = (s_idx_jp1 - s_idx )/ dy; + value_type Spz = (s_idx_kp1 - s_idx )/ dz; + value_type Smx = (s_idx - s_idx_im1)/ dx; + value_type Smy = (s_idx - s_idx_jm1)/ dy; + value_type Smz = (s_idx - s_idx_km1)/ dz; + + *f = max( (*s_phi)*n(0) , zero)*Smx + + min( (*s_phi)*n(0) , zero)*Spx + + max( (*s_phi)*n(1) , zero)*Smy + + min( (*s_phi)*n(1) , zero)*Spy + + max( (*s_phi)*n(2) , zero)*Smz + + min( (*s_phi)*n(2) , zero)*Spz; + + max_F = max(max_F, fabs(*f)); + } + value_type time_step = static_cast( 1.0 / (max_F + 1.0) ); + for(s=sbegin, f=fbegin;s!=send;++s,++f) + (*s) = (*s) - time_step * (*f); + ++iteration; + if(iteration> max_iterations) + steady_state = true; + } + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_EXTRAPOLATION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_fast_blur.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_fast_blur.h new file mode 100644 index 000000000..9acdec8de --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_fast_blur.h @@ -0,0 +1,292 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_FAST_BLUR_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_FAST_BLUR_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace grid + { + // The idea is to represent the blurred image as f(x,s) in terms of position + // x and scale s. Gaussian blurring is accomplished by using the input image + // I(x,s0) as the initial image (of scale s0 > 0) for the partial differential + // equation + // s*df/ds = s^2*Laplacian(f) + // where the Laplacian operator is + // Laplacian = (d/dx)^2, dimension 1 + // Laplacian = (d/dx)^2+(d/dy)^2, dimension 2 + // Laplacian = (d/dx)^2+(d/dy)^2+(d/dz)^2, dimension 3 + // + // The term s*df/ds is approximated by + // s*df(x,s)/ds = (f(x,b*s)-f(x,s))/ln(b) + // for b > 1, but close to 1, where ln(b) is the natural logarithm of b. If + // you take the limit of the right-hand side as b approaches 1, you get the + // left-hand side. + // + // The term s^2*((d/dx)^2)f is approximated by + // s^2*((d/dx)^2)f = (f(x+h*s,s)-2*f(x,s)+f(x-h*s,s))/h^2 + // for h > 0, but close to zero. + // + // Equating the approximations for the left-hand side and the right-hand side + // of the partial differential equation leads to the numerical method used in + // this code. + // + // For iterative application of these functions, the caller is responsible + // for constructing a geometric sequence of scales, + // s0, s1 = s0*b, s2 = s1*b = s0*b^2, ... + // where the base b satisfies 1 < b < exp(0.5*d) where d is the dimension of + // the image. The upper bound on b guarantees stability of the finite + // difference method used to approximate the partial differential equation. + // The method assumes a pixel size of h = 1. + // + // + // Sample usage: + // + // const int iterations = ; + // double scale = 1.0, log_base = 0.125, base = exp(0.125); + // grid_type tmp(); // Same dimensions as image! + // for (int i = 0; i < iterations; ++i, scale *= base) + // { + // fast_blur(image, tmp, scale, log_base); + // ; + // } + + //---------------------------------------------------------------------------- + + namespace detail + { + template + inline void fast_blur ( grid_type & image, grid_type & tmp, double si, double sj, double sk, double log_base) + { + using std::min; + using std::ceil; + using std::floor; + + typedef typename grid_type::value_type value_type; + typedef typename grid_type::iterator iterator; + typedef typename grid_type::index_iterator index_iterator; + typedef typename grid_type::math_types math_types; + typedef typename math_types::real_type real_type; + + size_t I = image.I(); + size_t J = image.J(); + size_t K = image.K(); + + index_iterator s = image.begin(); + index_iterator s_end = image.end(); + iterator t = tmp.begin(); + for( ; s != s_end; ++s, ++t) + { + size_t i = s.i(); + size_t j = s.j(); + size_t k = s.k(); + + double dip = i + si; + double dim = i - si; + int ip = static_cast( floor(dip) ); + int im = static_cast( ceil(dim) ); + + double djp = j + sj; + double djm = j - sj; + int jp = static_cast( floor(djp) ); + int jm = static_cast( ceil(djm) ); + + double dkp = k + sk; + double dkm = k - sk; + int kp = static_cast( floor(dkp) ); + int km = static_cast( ceil(dkm) ); + + im = ( i ) ? im : 0; + jm = ( j ) ? jm : 0; + km = ( k ) ? km : 0; + ip = min( ip, I - 1 ); + jp = min( jp, J - 1 ); + kp = min( kp, K - 1 ); + + //size_t idx = ( k * J + j ) * I + i; + size_t idx_im = ( k * J + j ) * I + im; + size_t idx_ip = ( k * J + j ) * I + ip; + size_t idx_jm = ( k * J + jm ) * I + i; + size_t idx_jp = ( k * J + jp ) * I + i; + size_t idx_km = ( km * J + j ) * I + i; + size_t idx_kp = ( kp * J + j ) * I + i; + + real_type v000 = *s; + real_type vm00 = image(idx_im); + real_type vp00 = image(idx_ip); + real_type v0m0 = image(idx_jm); + real_type v0p0 = image(idx_jp); + real_type v00m = image(idx_km); + real_type v00p = image(idx_kp); + + // i portion of second central difference + double isum = -2.0*v000 + vp00 + vm00; + if ( ip < I-1 ) // not on boundary, interpolate linearly + { + real_type vpp00 = image( (k*J+j)*I+(ip+1) ); + isum += (dip-ip)*( vpp00 - vp00 ); + } + if ( im > 0 ) // not on boundary, interpolate linearly + { + real_type vmm00 = image( (k*J+j)*I+(im-1) ); + isum += (dim-im)*( vm00 - vmm00 ); + } + + // j portion of second central difference + double jsum = -2.0*v000 + v0p0 + v0m0; + if ( jp < J-1 ) // not on boundary, interpolate linearly + { + real_type v0pp0 = image( (k*J+(jp+1))*I+i ); + jsum += (djp-jp)*( v0pp0 - v0p0 ); + } + if ( jm > 0 ) // not on boundary, interpolate linearly + { + real_type v0mm0 = image( (k*J+(jm-1))*I+i ); + jsum += (djm-jm)*( v0m0 - v0mm0 ); + } + + // k portion of second central difference + double ksum = -2.0*v000 + v00p + v00m; + if ( kp < K-1 ) // use boundary value + { + real_type v00pp = image( ((kp+1)*J+j)*I+i ); + ksum += (dkp-kp)*( v00pp - v00p ); + } + if ( km > 0 ) // use boundary value + { + real_type v00mm = image( ((km-1)*J+j)*I+i ); + ksum += (dkm-km)*( v00m - v00mm ); + } + + *t = static_cast(v000 + log_base*(isum+jsum+ksum)); + } + + image = tmp; + } + + // for (k = 0; k < K; ++k) + // { + // double dkp = k + sk; + // double dkm = k - sk; + // int kp = static_cast( floor(dkp) ); + // int km = static_cast( ceil(dkm) ); + // + // for (j = 0; j < J; ++j) + // { + // double djp = j + sj; + // double djm = j - sj; + // int jp = static_cast( floor(djp) ); + // int jm = static_cast( ceil(djm) ); + // + // for (i = 0; i < I; ++i) + // { + // double dip = i + si; + // double dim = i - si; + // int ip = static_cast( floor(dip) ); + // int im = static_cast( ceil(dim) ); + // + // // i portion of second central difference + // double isum = -2.0*image(i,j,k); + // if ( ip >= I-1 ) // use boundary value + // { + // isum += image(I-1,j,k); + // } + // else // linearly interpolate + // { + // isum += image(ip,j,k)+(dip-ip)*(image(ip+1,j,k)-image(ip,j,k)); + // } + // + // if ( im <= 0 ) // use boundary value + // { + // isum += image(0,j,k); + // } + // else // linearly interpolate + // { + // isum += image(im,j,k)+(dim-im)*(image(im,j,k)-image(im-1,j,k)); + // } + // + // // j portion of second central difference + // double jsum = -2.0*image(i,j,k); + // if ( jp >= J-1 ) // use boundary value + // { + // jsum += image(i,J-1,k); + // } + // else // linearly interpolate + // { + // jsum += image(i,jp,k)+(djp-jp)*(image(i,jp+1,k)-image(i,jp,k)); + // } + // + // if ( jm <= 0 ) // use boundary value + // { + // jsum += image(i,0,k); + // } + // else // linearly interpolate + // { + // jsum += image(i,jm,k)+(djm-jm)*(image(i,jm,k)-image(i,jm-1,k)); + // } + // + // // k portion of second central difference + // double ksum = -2.0*image(i,j,k); + // if ( kp >= K-1 ) // use boundary value + // { + // ksum += image(i,j,K-1); + // } + // else // linearly interpolate + // { + // ksum += image(i,j,kp)+(dkp-kp)*(image(i,j,kp+1)-image(i,j,kp)); + // } + // + // if ( km <= 0 ) // use boundary value + // { + // ksum += image(i,j,0); + // } + // else // linearly interpolate + // { + // ksum += image(i,j,km)+(dkm-km)*(image(i,j,km)-image(i,j,km-1)); + // } + // + // tmp(i,j,k) = static_cast(image(i,j,k) + log_base*(isum+jsum+ksum)); + // } + // } + // } + + // for (k = 0; k < K; ++k) + // { + // for (j = 0; j < J; ++j) + // { + // for (i = 0; i < I; ++i) + // image(i,j,k) = tmp(i,j,k); + // } + // } + + } // namespace detail + + template + inline void fast_blur ( grid_type & image, double sx, double sy, double sz, double log_base, size_t iterations) + { + using std::exp; + + double base = exp(log_base); + grid_type tmp(image); + for( size_t i=0; i + inline void fast_blur ( grid_type & image, double s, double log_base, size_t iterations) + { + fast_blur(image, s, s, s, log_base, iterations); + } + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_FAST_BLUR_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_gradient.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_gradient.h new file mode 100644 index 000000000..10c2140d3 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_gradient.h @@ -0,0 +1,171 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_GRADIENT_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_GRADIENT_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace grid + { + + template + inline void gradient( + grid_type const & grid + , size_t i + , size_t j + , size_t k + , vector3_type & gradient + ) + { + using std::min; + + typedef typename grid_type::value_type value_type; + typedef typename vector3_type::value_type real_type; + + static value_type const unused = grid.unused(); + + size_t const & I = grid.I(); + size_t const & J = grid.J(); + size_t const & K = grid.K(); + + size_t im1 = 0, jm1 = 0, km1 = 0; + if ( i>0 ) + im1 = i - 1; + if ( j>0 ) + jm1 = j - 1; + if ( k>0 ) + km1 = k - 1; + + size_t ip1 = min( i + 1u, I - 1u ); + size_t jp1 = min( j + 1u, J - 1u ); + size_t kp1 = min( k + 1u, K - 1u ); + + size_t idx = ( k * J + j ) * I + i; + size_t idx_im1 = ( k * J + j ) * I + im1; + size_t idx_ip1 = ( k * J + j ) * I + ip1; + size_t idx_jm1 = ( k * J + jm1 ) * I + i; + size_t idx_jp1 = ( k * J + jp1 ) * I + i; + size_t idx_km1 = ( km1 * J + j ) * I + i; + size_t idx_kp1 = ( kp1 * J + j ) * I + i; + + //--- Return Unused-vector if any of the values in the map is unused. + gradient = vector3_type(unused,unused,unused); + + value_type s_idx = grid( idx ); + if ( s_idx == unused ) + return; + + value_type s_im1 = grid( idx_im1 ); + if ( s_im1 == unused ) + return; + + value_type s_ip1 = grid( idx_ip1 ); + if ( s_ip1 == unused ) + return; + + value_type s_jm1 = grid( idx_jm1 ); + if ( s_jm1 == unused ) + return; + + value_type s_jp1 = grid( idx_jp1 ); + if ( s_jp1 == unused ) + return; + + value_type s_km1 = grid( idx_km1 ); + if ( s_km1 == unused ) + return; + + value_type s_kp1 = grid( idx_kp1 ); + if ( s_kp1 == unused ) + return; + + real_type r_idx = static_cast( s_idx ); + real_type r_im1 = static_cast( s_im1 ); + real_type r_ip1 = static_cast( s_ip1 ); + real_type r_jm1 = static_cast( s_jm1 ); + real_type r_jp1 = static_cast( s_jp1 ); + real_type r_km1 = static_cast( s_km1 ); + real_type r_kp1 = static_cast( s_kp1 ); + + if ( i == 0 ) + { + gradient( 0 ) = ( r_ip1 - r_idx ); + gradient( 0 ) /= grid.dx(); + } + else if ( i == ( grid.I() - 1 ) ) + { + gradient( 0 ) = ( r_idx - r_im1 ); + gradient( 0 ) /= grid.dx(); + } + else + { + gradient( 0 ) = ( r_ip1 - r_im1 ); + gradient( 0 ) /= ( 2 * grid.dx() ); + } + if ( j == 0 ) + { + gradient( 1 ) = ( r_jp1 - r_idx ); + gradient( 1 ) /= grid.dy(); + } + else if ( j == ( grid.J() - 1 ) ) + { + gradient( 1 ) = ( r_idx - r_jm1 ); + gradient( 1 ) /= grid.dy(); + } + else + { + gradient( 1 ) = ( r_jp1 - r_jm1 ); + gradient( 1 ) /= ( 2 * grid.dy() ); + } + + if ( k == 0 ) + { + gradient( 2 ) = ( r_kp1 - r_idx ); + gradient( 2 ) /= grid.dz(); + } + else if ( k == ( grid.K() - 1 ) ) + { + gradient( 2 ) = ( r_idx - r_km1 ); + gradient( 2 ) /= grid.dz(); + } + else + { + gradient( 2 ) = ( r_kp1 - r_km1 ); + gradient( 2 ) /= ( 2 * grid.dz() ); + } + } + + template + inline void gradient (grid_iterator const & iter, vector3_type & gradient ) + { + typedef typename grid_iterator::grid_type grid_type; + + size_t i = iter.i(); + size_t j = iter.j(); + size_t k = iter.k(); + grid_type const & grid = iter.get_grid(); + gradient(grid, i, j, k, gradient); + } + + template + inline typename grid_iterator::math_types::vector3_type gradient (grid_iterator const & iter ) + { + typedef typename grid_iterator::math_types::vector3_type vector3_type; + + vector3_type gradient; + + gradient(iter, gradient); + return gradient; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_GRADIENT_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_gradient_at_point.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_gradient_at_point.h new file mode 100644 index 000000000..990f8b8ce --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_gradient_at_point.h @@ -0,0 +1,70 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_GRADIENT_AT_POINT_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_GRADIENT_AT_POINT_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include + +namespace OpenTissue +{ + namespace grid + { + + template + inline vector3_type gradient_at_point (grid_type const & grid, vector3_type const & point ) + { + typedef typename grid_type::value_type value_type; + + const static value_type infty = grid.infinity(); + const static value_type unused = grid.unused(); + + size_t i0, j0, k0, i1, j1, k1; + enclosing_indices(grid, point, i0, j0, k0, i1, j1, k1 ); + + vector3_type g000,g001,g010,g011,g100,g101,g110,g111; + gradient( grid, i0, j0, k0, g000 ); + gradient( grid, i1, j0, k0, g001 ); + gradient( grid, i0, j1, k0, g010 ); + gradient( grid, i1, j1, k0, g011 ); + gradient( grid, i0, j0, k1, g100 ); + gradient( grid, i1, j0, k1, g101 ); + gradient( grid, i0, j1, k1, g110 ); + gradient( grid, i1, j1, k1, g111 ); + + if ( g000( 0 ) == unused ) + return vector3_type( infty, infty, infty ); + if ( g001( 0 ) == unused ) + return vector3_type( infty, infty, infty ); + if ( g010( 0 ) == unused ) + return vector3_type( infty, infty, infty ); + if ( g011( 0 ) == unused ) + return vector3_type( infty, infty, infty ); + if ( g100( 0 ) == unused ) + return vector3_type( infty, infty, infty ); + if ( g101( 0 ) == unused ) + return vector3_type( infty, infty, infty ); + if ( g110( 0 ) == unused ) + return vector3_type( infty, infty, infty ); + if ( g111( 0 ) == unused ) + return vector3_type( infty, infty, infty ); + + typename vector3_type::value_type s = ( point(0) - ( i0*grid.dx() + grid.min_coord(0) ) ) / grid.dx(); + typename vector3_type::value_type t = ( point(1) - ( j0*grid.dy() + grid.min_coord(1) ) ) / grid.dy(); + typename vector3_type::value_type u = ( point(2) - ( k0*grid.dz() + grid.min_coord(2) ) ) / grid.dz(); + + return OpenTissue::math::trillinear(g000, g001, g010, g011, g100, g101, g110, g111, s, t, u ) ; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_GRADIENT_AT_POINT_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_gradient_magnitude.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_gradient_magnitude.h new file mode 100644 index 000000000..ce1633c6b --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_gradient_magnitude.h @@ -0,0 +1,69 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRADIENT_MAGNITUDE_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRADIENT_MAGNITUDE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +#include + +namespace OpenTissue +{ + namespace grid + { + + template + inline void gradient_magnitude( + grid_type const & grid + , size_t i + , size_t j + , size_t k + , real_type & grad_mag + ) + { + using std::sqrt; + + typedef OpenTissue::math::Vector3 vector3_type; + vector3_type g; + gradient(map,i,j,k,g); + grad_mag = sqrt(g*g); + } + + template + inline void gradient_magnitude (grid_iterator const & iter, real_type & grad_mag ) + { + typedef typename grid_iterator::grid_type grid_type; + + size_t i = iter.i(); + size_t j = iter.j(); + size_t k = iter.k(); + + grid_type const & grid = iter.get_grid(); + + gradient_magnitude(grid, i, j, k, grad_mag); + } + + template + inline typename grid_iterator::math_types::real_type gradient_magnitude( grid_iterator const & iter ) + { + typedef typename grid_iterator::math_types::real_type real_type; + + real_type grad_mag; + + gradient_magnitude(iter, grad_mag); + + return grad_mag; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRADIENT_MAGNITUDE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_hessian.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_hessian.h new file mode 100644 index 000000000..a5ecd0f4d --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_hessian.h @@ -0,0 +1,156 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_HESSIAN_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_HESSIAN_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace grid + { + + template + inline void hessian( + grid_type const & grid + , size_t i + , size_t j + , size_t k + , matrix3x3_type & H + ) + { + using std::min; + + typedef typename matrix3x3_type::value_type real_type; + + static grid_type * old_grid = 0; + static real_type m_inv_dx2 = 0; ///< Pre-computed value of 1./grid.dx()*grid.dx() + static real_type m_inv_dy2 = 0; ///< Pre-computed value of 1./grid.dy()*grid.dy() + static real_type m_inv_dz2 = 0; ///< Pre-computed value of 1./grid.dz()*grid.dz() + static real_type m_inv_4dxy = 0; ///< Pre-computed value of 1./4*grid.dx()*grid.dy() + static real_type m_inv_4dxz = 0; ///< Pre-computed value of 1./4*grid.dx()*grid.dz() + static real_type m_inv_4dyz = 0; ///< Pre-computed value of 1./4*grid.dy()*grid.dz() + + if(old_grid!=&grid) + { + old_grid = const_cast(&grid); + m_inv_dx2 = 1.0 / grid.dx() * grid.dx(); + m_inv_dy2 = 1.0 / grid.dy() * grid.dy(); + m_inv_dz2 = 1.0 / grid.dz() * grid.dz(); + m_inv_4dxy = 0.25 / grid.dx() * grid.dy(); + m_inv_4dxz = 0.25 / grid.dx() * grid.dz(); + m_inv_4dyz = 0.25 / grid.dy() * grid.dz(); + } + + size_t I = grid.I(); + size_t J = grid.J(); + size_t K = grid.K(); + + size_t im1 = 0, jm1 = 0, km1 = 0; + if ( i ) + im1 = i - 1; + if ( j ) + jm1 = j - 1; + if ( k ) + km1 = k - 1; + size_t ip1 = min( i + 1u, I - 1u ); + size_t jp1 = min( j + 1u, J - 1u ); + size_t kp1 = min( k + 1u, K - 1u ); + + size_t idx_000 = ( k * J + j ) * I + i; + size_t idx_m00 = ( k * J + j ) * I + im1; + size_t idx_p00 = ( k * J + j ) * I + ip1; + size_t idx_0m0 = ( k * J + jm1 ) * I + i; + size_t idx_0p0 = ( k * J + jp1 ) * I + i; + size_t idx_00m = ( km1 * J + j ) * I + i; + size_t idx_00p = ( kp1 * J + j ) * I + i; + + real_type d000 = 2.0 * grid( idx_000 ); + real_type dp00 = grid( idx_p00 ); + real_type dm00 = grid( idx_m00 ); + real_type d0p0 = grid( idx_0p0 ); + real_type d0m0 = grid( idx_0m0 ); + real_type d00p = grid( idx_00p ); + real_type d00m = grid( idx_00m ); + + size_t idx_pp0 = ( k * J + jp1 ) * I + ip1; + size_t idx_mp0 = ( k * J + jp1 ) * I + im1; + size_t idx_pm0 = ( k * J + jm1 ) * I + ip1; + size_t idx_mm0 = ( k * J + jm1 ) * I + im1; + size_t idx_p0p = ( kp1 * J + j ) * I + ip1; + size_t idx_m0p = ( kp1 * J + j ) * I + im1; + + real_type dpp0 = grid( idx_pp0 ); + real_type dmp0 = grid( idx_mp0 ); + real_type dpm0 = grid( idx_pm0 ); + real_type dmm0 = grid( idx_mm0 ); + real_type dp0p = grid( idx_p0p ); + real_type dm0p = grid( idx_m0p ); + + size_t idx_p0m = ( km1 * J + j ) * I + ip1; + size_t idx_m0m = ( km1 * J + j ) * I + im1; + size_t idx_0pp = ( kp1 * J + jp1 ) * I + i; + size_t idx_0pm = ( km1 * J + jp1 ) * I + i; + size_t idx_0mp = ( kp1 * J + jm1 ) * I + i; + size_t idx_0mm = ( km1 * J + jm1 ) * I + i; + + real_type dp0m = grid( idx_p0m ); + real_type dm0m = grid( idx_m0m ); + real_type d0pp = grid( idx_0pp ); + real_type d0pm = grid( idx_0pm ); + real_type d0mp = grid( idx_0mp ); + real_type d0mm = grid( idx_0mm ); + //---- Hessian matrix is defines as + // + // | d/(dx*dx) d(/(dx*dy) d/(dx*dz) | + // H = | d/(dy*dx) d(/(dy*dy) d/(dy*dz) | phi + // | d/(dz*dx) d(/(dz*dy) d/(dz*dz) | + // + // + //--- Following is a central diff approximation + H( 0, 0 ) = ( dp00 + dm00 - d000 ) * m_inv_dx2; + H( 1, 1 ) = ( d0p0 + d0m0 - d000 ) * m_inv_dy2; + H( 2, 2 ) = ( d00p + d00m - d000 ) * m_inv_dz2; + H( 1, 0 ) = H( 0, 1 ) = ( dpp0 - dmp0 - dpm0 + dmm0 ) * m_inv_4dxy; + H( 0, 2 ) = H( 2, 0 ) = ( dp0p - dm0p - dp0m + dm0m ) * m_inv_4dxz; + H( 1, 2 ) = H( 2, 1 ) = ( d0pp - d0pm - d0mp + d0mm ) * m_inv_4dyz; + } + + template + inline void hessian (grid_iterator const & iter, matrix3x3_type & H ) + { + typedef typename grid_iterator::grid_type grid_type; + + size_t i = iter.i(); + size_t j = iter.j(); + size_t k = iter.k(); + + grid_type const & grid = iter.get_grid(); + + hessian(grid, i, j, k, H); + } + + + template + inline OpenTissue::math::Matrix3x3< typename grid_iterator::math_types::real_type > + hessian (grid_iterator const & iter ) + { + typedef typename grid_iterator::math_types::real_type real_type; + typedef OpenTissue::math::Matrix3x3< real_type > matrix3x3_type; + + matrix3x3_type H; + + hessian(iter, H); + + return H; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_HESSIAN_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_idx2coord.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_idx2coord.h new file mode 100644 index 000000000..d36a636b2 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_idx2coord.h @@ -0,0 +1,92 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_IDX_TO_COORD_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_IDX_TO_COORD_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * Index To Coordinate Conversion. + * This method computes the coordinates of the voxel with indices (i,j,k) + * + * @param map The grid. + * @param i The index of the voxel along the I-axe. + * @param j The index of the voxel along the J-axe. + * @param k The index of the voxel along the K-axe. + * @param point Upon return this parameter contains the coordinates. + */ + template + inline void idx2coord( grid_type const & grid, size_t i, size_t j, size_t k, vector3_type & point ) + { + typedef typename vector3_type::value_type real_type; + assert(i + inline void idx2coord (grid_iterator const & iter, vector3_type & point ) + { + typedef typename grid_iterator::grid_type grid_type; + + size_t i = iter.i(); + size_t j = iter.j(); + size_t k = iter.k(); + + grid_type const & grid = iter.get_grid(); + + idx2coord(grid, i, j, k, point); + } + + /** + * Index To Coordinate Conversion. + * This method computes the coordinates of the voxel with indices (i,j,k) + * + * @param iter An iterator to the voxel. + * @return Holds the coordinates. + */ + template + inline typename grid_iterator::math_types::vector3_type idx2coord (grid_iterator const & iter ) + { + typedef typename grid_iterator::math_types::vector3_type vector3_type; + + vector3_type point; + + idx2coord(iter, point); + + return point; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_IDX_TO_COORD_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_ignore_region.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_ignore_region.h new file mode 100644 index 000000000..886f25618 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_ignore_region.h @@ -0,0 +1,74 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_IGNORE_REGION_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_IGNORE_REGION_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace grid + { + + /** + * Tag used to specify inside region of a level set (phi<0) + */ + struct inside_region_tag {}; + + /** + * Tag used to specify outside region of a level set (phi>0) + */ + struct outside_region_tag {}; + + + /** + * Mask out inside region. + * This function masks out the inside region of level set + * function by assigning the unused value to all phi<0. + * + * @param phi The level set grid. + */ + template + inline void ignore_region(grid_type & phi, inside_region_tag const & /*region*/) + { + typedef typename grid_type::value_type value_type; + typedef typename grid_type::index_iterator iterator; + + value_type unused = phi.unused(); + iterator end = phi.end(); + iterator p = phi.begin(); + for(;p!=end;++p) + if(*p < 0 ) + *p = unused; + } + + /** + * Mask out outside region. + * This function masks out the inside region of level set + * function by assigning the unused value to all phi>0. + * + * @param phi The level set grid. + */ + template + inline void ignore_region(grid_type & phi, outside_region_tag const & /*region*/) + { + typedef typename grid_type::value_type value_type; + typedef typename grid_type::index_iterator iterator; + + value_type unused = phi.unused(); + iterator end = phi.end(); + iterator p = phi.begin(); + for(;p!=end;++p) + if(*p > 0 ) + *p = unused; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_IGNORE_REGION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_is_point_inside.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_is_point_inside.h new file mode 100644 index 000000000..b5af27619 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_is_point_inside.h @@ -0,0 +1,43 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_IS_POINT_INSIDE_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_IS_POINT_INSIDE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * Point Inclusion Test. + * + * Checks if a given points is strictly inside the grid. + * Notice if point lies on boundary of grid, then + * it is not consider to be inside the grid. + * + * @param point Vector with coordinates for a point. + * @return True if point is strictly inside. False otherwise. + */ + template + inline bool is_point_inside( grid_type const & grid, vector3_type const & point ) + { + for ( size_t i = 0u;i < 3u;++i ) + { + if ( point( i ) <= grid.min_coord( i ) ) + return false; + if ( point( i ) >= grid.max_coord( i ) ) + return false; + } + return true; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_IS_POINT_INSIDE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_iterators.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_iterators.h new file mode 100644 index 000000000..2be1178d1 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_iterators.h @@ -0,0 +1,346 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_ITERATORS_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_ITERATORS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace grid + { + namespace detail + { + + //! basic iterator for walking through the WHOLE container + template + class Iterator + : public std::iterator< std::random_access_iterator_tag, typename grid_type_::value_type > + { + public: + + typedef grid_type_ grid_type; + typedef typename grid_type::math_types math_types; + + private: + + typedef OpenTissue::grid::detail::Iterator self_type; + + protected: + + typedef typename grid_type::index_vector index_vector; + grid_type * m_grid; + pointer_type m_pos; + + public: + + + grid_type & get_grid() { return *m_grid; } + grid_type const & get_grid() const { return *m_grid; } + + pointer_type const & get_pointer() const { return m_pos; } + pointer_type & get_pointer() { return m_pos; } + + public: + + Iterator() + : m_grid( 0 ) + , m_pos( 0 ) + {} + + Iterator( grid_type * grid, pointer_type pos ) + : m_grid( grid ) + , m_pos( pos ) + {} + + reference_type operator*() + { + return * m_pos; + } + + self_type operator++( int ) + { + self_type tmp = *this; + m_pos++; + return tmp; + } + + self_type &operator++() + { + m_pos++; + return *this; + } + + void operator+=( size_t m ) + { + m_pos += m; + } + + self_type operator+ ( size_t m ) + { + self_type tmp = *this; + tmp.m_pos += m; + return tmp; + } + + self_type operator- ( size_t m ) + { + self_type tmp = *this; + tmp.m_pos -= m; + return tmp; + } + + self_type operator--( int ) + { + self_type tmp = *this; + m_pos--; + return tmp; + } + + self_type &operator--() + { + m_pos--; + return *this; + } + + void operator-=( size_t m ) { m_pos -= m; } + int operator- ( self_type const & other ) const { return m_pos -other.m_pos; } + bool operator< ( self_type const & other ) const { return m_pos < other.m_pos; } + bool operator<=( self_type const & other ) const { return m_pos <= other.m_pos; } + bool operator> ( self_type const & other ) const { return m_pos > other.m_pos; } + bool operator>=( self_type const & other ) const { return m_pos >= other.m_pos; } + + // TODO: henrikd 2005-06-27 - confirm these! + index_vector compute_index() const + { + int offset = m_pos - m_grid->data(); + index_vector res; + res(2) = offset / ( m_grid->J() * m_grid->I() ); + offset = offset % ( m_grid->J() * m_grid->I() ); + res(1) = offset / m_grid->I(); + res(0) = offset % m_grid->I(); + return res; + } + + void operator+=( index_vector const & idx ) + { + m_pos += idx(0) + m_grid->I() * idx(1) + m_grid->I() * m_grid->J() * idx(2); + } + + self_type const & operator=( index_vector const & idx ) + { + m_pos = m_grid->data() + idx(0) + m_grid->I() * idx(1) + m_grid->I() * m_grid->J() * idx(2); + return *this; + } + + bool operator!=( self_type const & other ) + { + return m_pos != other.m_pos; + } + + bool operator==( self_type const & other ) + { + return m_pos == other.m_pos; + } + }; + + + //! iterator that keeps track of i,j,k position + /*! for walking through the WHOLE container */ + template + class IndexIterator + : public OpenTissue::grid::detail::Iterator + { + public: + + typedef typename grid_type::math_types math_types; + + protected: + + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::index_vector3_type index_vector; + + private: + + typedef OpenTissue::grid::detail::Iterator base_type; + typedef OpenTissue::grid::detail::IndexIterator self_type; + + // TODO: Use index_vector directly when vector is implemented as it should be! + size_t m_i; + size_t m_j; + size_t m_k; + + static size_t const m_out_of_bounds = 0u - 1u; + + void update_indices() + { + index_vector v = this->compute_index(); + m_i = v(0); + m_j = v(1); + m_k = v(2); + } + + public: + + IndexIterator() + : base_type() + , m_i(0) + , m_j(0) + , m_k(0) + {} + + IndexIterator( base_type const& other ) + : base_type( other ) + { + operator=( other ); + } + + IndexIterator( grid_type * grid, pointer_type pos ) + : base_type( grid, pos ) + { + // optionally move ijk if m_pos is not at begining of m_grid + if ( this->m_pos != this->m_grid->data() ) + { + base_type::operator=( base_type( this->m_grid, this->m_pos ).compute_index() ); + } + } + + public: + + size_t const& i() const { return m_i; } + size_t const& j() const { return m_j; } + size_t const& k() const { return m_k; } + + index_vector get_index() const + { + return index_vector( m_i, m_j, m_k ); + } + + vector3_type get_coord() const + { + return vector3_type( + m_i * this->m_grid->dx() + this->m_grid->min_coord( 0 ) + , m_j * this->m_grid->dy() + this->m_grid->min_coord( 1 ) + , m_k * this->m_grid->dz() + this->m_grid->min_coord( 2 ) + ); + } + + public: + + self_type operator++( int ) + { + self_type tmp = *this; + ++( *this ); + return tmp; + } + + self_type &operator++() + { + ++this->m_pos; + ++m_i; + if ( m_i >= this->m_grid->I() ) + { + m_i = 0u; + ++m_j; + if ( m_j >= this->m_grid->J() ) + { + m_j = 0u; + ++m_k; + } + } + return *this; + } + + self_type operator--( int ) + { + self_type tmp = *this; + --( *this ); + return tmp; + } + + self_type &operator--() + { + --this->m_pos; + --m_i; + if ( m_i == m_out_of_bounds ) + { + m_i = this->m_grid->I() - 1; + --m_j; + if ( m_j == m_out_of_bounds ) + { + m_j = this->m_grid->J() - 1; + --m_k; + } + } + return *this; + } + + // jump to new location + self_type const & operator=( index_vector const& idx ) + { + m_i = idx(0); + m_j = idx(1); + m_k = idx(2); + this->m_pos = &( *this->m_grid ) ( idx ); + return *this; + } + + void operator+=( size_t m ) + { + this->m_pos += m; + update_indices(); + } + + void operator-=( size_t m ) + { + this->m_pos -= m; + update_indices(); + } + + self_type operator+ ( size_t m ) + { + self_type tmp = *this; + tmp.m_pos += m; + tmp.update_indices(); + return tmp; + } + + self_type operator- ( size_t m ) + { + self_type tmp = *this; + tmp.m_pos -= m; + tmp.update_indices(); + return tmp; + } + + self_type const & operator= ( base_type const & other ) + { + this->m_grid = const_cast( &( other.get_grid() ) ); + this->m_pos = other.get_pointer(); + if ( this->m_pos == this->m_grid->data() ) + { + m_i = 0u; + m_j = 0u; + m_k = 0u; + } + else + { + update_indices(); + } + return *this; + } + + }; + + } // namespace detail + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_ITERATORS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_junctions.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_junctions.h new file mode 100644 index 000000000..42af31836 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_junctions.h @@ -0,0 +1,532 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_JUNCTIONS_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_JUNCTIONS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include // Needed for BasicMathTypes + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +namespace OpenTissue +{ + namespace grid + { + + namespace detail + { + + template + class JunctionCandidate + { + public: + + typedef math_types_ math_types; + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::real_type real_type; + + public: + + vector3_type m_center; //--- Estimated center of the largest enclosing sphere. + real_type m_radius; //--- Upper estimate of the radius of the largest enclosed sphere located at the junction point. + size_t m_cnt_boundary; //--- number of boundary that the largest enclosed sphere touches. + + public: + + JunctionCandidate(vector3_type const & center, real_type const & radius) + : m_center(center) + , m_radius(radius) + , m_cnt_boundary() + { + assert(m_radius>0 || !"JunctionCandidate(): Non-positive radius"); + } + }; + + + + template + class JunctionCollisionPolicy + { + public: + + typedef typename junction_candiate_type::math_types math_types; + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::real_type real_type; + typedef face_type data_type; + typedef junction_candiate_type query_type; + typedef bool result_container; + typedef OpenTissue::spatial_hashing::PrimeNumberHashFunction hash_function; + + typedef OpenTissue::spatial_hashing::Grid< vector3_type, OpenTissue::math::Vector3, data_type, hash_function> hash_grid; + + public: + + vector3_type min_coord(face_type const & face) const + { + vector3_type coord; + mesh::compute_face_minimum_coord(face,coord); + return coord; + } + + vector3_type max_coord(face_type const & face) const + { + vector3_type coord; + mesh::compute_face_maximum_coord(face,coord); + return coord; + } + + vector3_type min_coord(junction_candiate_type const & candidate) const + { + return vector3_type( + candidate.m_center(0) - candidate.m_radius + , candidate.m_center(1) - candidate.m_radius + , candidate.m_center(2) - candidate.m_radius + ); + } + + vector3_type max_coord(junction_candiate_type const & candidate) const + { + return vector3_type( + candidate.m_center(0) + candidate.m_radius + , candidate.m_center(1) + candidate.m_radius + , candidate.m_center(2) + candidate.m_radius + ); + } + + void reset(result_container & /*results*/) { } + + void report(data_type const & data, query_type const & query,result_container & /*results*/) + { + face_type * face = const_cast(&data); + junction_candiate_type * candidate = const_cast(&query); + + if(candidate->m_cnt_boundary >= 4) + return; + + + OpenTissue::geometry::Triangle triangle; + triangle.set( face ); + + typedef OpenTissue::math::BasicMathTypes< real_type, size_t> math_types; + OpenTissue::geometry::Sphere sphere(candidate->m_center,candidate->m_radius); + + if(OpenTissue::intersect::sphere_triangle(sphere,triangle)) + { + ++(candidate->m_cnt_boundary); + } + + } + }; + + }// namespace detail + + + + /** + * Detect MREP junctions. + * + * KE 2006-02-15: This is work in progress, please ignore it! + * + * @param aof A the aof grid. + * @param F A grid containing non-zero values for junctions. + */ + template + inline void junctions( + grid_type const & aof + , grid_type & phi + , mesh_type /*const*/ & mesh + , point_container & points + ) + { + using std::fabs; + using std::min; + using std::max; + using std::sqrt; + + typedef typename point_container::value_type vector3_type; + typedef typename vector3_type::value_type real_type; + typedef typename grid_type::value_type value_type; + typedef typename grid_type::const_iterator const_iterator; + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename grid_type::index_iterator index_iterator; + typedef typename mesh_type::face_type face_type; + + + //--- vorticity w = grad cross grad(phi) Is this any good???? + // + // d gz d gy + // wx = ------ - ------ + // dy dz + // + // d gx d gz + // wy = ------ - ------ + // dz dx + // + // d gy d gx + // wy = ------ - ------ + // dx dy + // + + + + + //{ + //------------------------------------------------------- + //--- Thining based detection, sucks!!! ---------------- + //------------------------------------------------------- + //size_t I = phi.I(); + //size_t J = phi.J(); + //size_t K = phi.K(); + //value_type s[27]; + //grid_type tmp = aof; + //{ + // index_iterator t = tmp.begin(); + // const_index_iterator a = aof.begin(); + // const_index_iterator end = aof.end(); + // for(;a!=end;++a,++t) + // { + // (*t) = 0; + // size_t i = a.i(); + // size_t j = a.j(); + // size_t k = a.k(); + // if( (*a) > 0) + // (*t) = detail::compute_aof_value(phi,i,j,k,0.1); + // if( (*t) < 1e-5) + // { + // (*t) = 0; + // continue; + // } + // if( phi(i,j,k) > 0) + // { + // (*t) = 0; + // continue; + // } + // } + //} + //{ + // phi = tmp; + // index_iterator t = phi.begin(); + // index_iterator a = tmp.begin(); + // index_iterator end = tmp.end(); + // for(;a!=end;++a,++t) + // { + // size_t i = a.i(); + // size_t j = a.j(); + // size_t k = a.k(); + // if( (*a) < 1e-5) + // continue; + // size_t im1 = ( i ) ? i - 1 : 0; + // size_t jm1 = ( j ) ? j - 1 : 0; + // size_t km1 = ( k ) ? k - 1 : 0; + // size_t ip1 = min( i + 1u, I - 1u ); + // size_t jp1 = min( j + 1u, J - 1u ); + // size_t kp1 = min( k + 1u, K - 1u ); + // s[0] = tmp( im1, jm1, km1); + // s[1] = tmp( im1, jm1, k); + // s[2] = tmp( im1, jm1, kp1); + // s[3] = tmp( im1, j, km1); + // s[4] = tmp( im1, j, k); + // s[5] = tmp( im1, j, kp1); + // s[6] = tmp( im1, jp1, km1); + // s[7] = tmp( im1, jp1, k); + // s[8] = tmp( im1, jp1, kp1); + // s[9] = tmp( i, jm1, km1); + // s[10] = tmp( i, jm1, k); + // s[11] = tmp( i, jm1, kp1); + // s[12] = tmp( i, j, km1); + // s[13] = tmp( i, j, k); + // s[14] = tmp( i, j, kp1); + // s[15] = tmp( i, jp1, km1); + // s[16] = tmp( i, jp1, k); + // s[17] = tmp( i, jp1, kp1); + // s[18] = tmp( ip1, jm1, km1); + // s[19] = tmp( ip1, jm1, k); + // s[20] = tmp( ip1, jm1, kp1); + // s[21] = tmp( ip1, j, km1); + // s[22] = tmp( ip1, j, k); + // s[23] = tmp( ip1, j, kp1); + // s[24] = tmp( ip1, jp1, km1); + // s[25] = tmp( ip1, jp1, k); + // s[26] = tmp( ip1, jp1, kp1); + // bool has_zero_neighbor = false; + // bool has_larger_neighbor = false; + // bool is_maxima = true; + // for(size_t i=0u;i<27u;++i) + // { + // if(s[i]<=0) + // has_zero_neighbor = true; + // if(s[i]>s[13]) + // has_larger_neighbor = true; + // if(i!=13 && s[13]< s[i]) + // is_maxima = false; + // } + // if(!is_maxima && has_larger_neighbor && has_zero_neighbor)//--- thining + // { + // (*t) = 0; + // continue; + // } + // vector3_type p; + // idx2coord(aof, i, j, k, p); + // points.push_back(p); + // } + //} + + + + + //------------------------------------------------------- + //--- Angle based detection, sucks!!! ------------------- + //------------------------------------------------------- + //vector3_type g[27]; + //vector3_type p[27]; + //value_type s[27]; + //vector3_type d[27]; + //vector3_type q[27]; + //size_t I = phi.I(); + //size_t J = phi.J(); + //size_t K = phi.K(); + // const_index_iterator a = aof.begin(); + // const_index_iterator end = aof.end(); + // for(;a!=end;++a,++t) + // { + // size_t i = a.i(); + // size_t j = a.j(); + // size_t k = a.k(); + // if( (*a) < 1e-5) || phi(i,j,k) > 0) + // { + // continue; + // } + //size_t im1 = ( i ) ? i - 1 : 0; + //size_t jm1 = ( j ) ? j - 1 : 0; + //size_t km1 = ( k ) ? k - 1 : 0; + //size_t ip1 = min( i + 1u, I - 1u ); + //size_t jp1 = min( j + 1u, J - 1u ); + //size_t kp1 = min( k + 1u, K - 1u ); + //grid_gradient(phi, im1, jm1, km1, g[0]); + //grid_gradient(phi, im1, jm1, k, g[1]); + //grid_gradient(phi, im1, jm1, kp1, g[2]); + //grid_gradient(phi, im1, j, km1, g[3]); + //grid_gradient(phi, im1, j, k, g[4]); + //grid_gradient(phi, im1, j, kp1, g[5]); + //grid_gradient(phi, im1, jp1, km1, g[6]); + //grid_gradient(phi, im1, jp1, k, g[7]); + //grid_gradient(phi, im1, jp1, kp1, g[8]); + //grid_gradient(phi, i, jm1, km1, g[9]); + //grid_gradient(phi, i, jm1, k, g[10]); + //grid_gradient(phi, i, jm1, kp1, g[11]); + //grid_gradient(phi, i, j, km1, g[12]); + //grid_gradient(phi, i, j, k, g[13]); + //grid_gradient(phi, i, j, kp1, g[14]); + //grid_gradient(phi, i, jp1, km1, g[15]); + //grid_gradient(phi, i, jp1, k, g[16]); + //grid_gradient(phi, i, jp1, kp1, g[17]); + //grid_gradient(phi, ip1, jm1, km1, g[18]); + //grid_gradient(phi, ip1, jm1, k, g[19]); + //grid_gradient(phi, ip1, jm1, kp1, g[20]); + //grid_gradient(phi, ip1, j, km1, g[21]); + //grid_gradient(phi, ip1, j, k, g[22]); + //grid_gradient(phi, ip1, j, kp1, g[23]); + //grid_gradient(phi, ip1, jp1, km1, g[24]); + //grid_gradient(phi, ip1, jp1, k, g[25]); + //grid_gradient(phi, ip1, jp1, kp1, g[26]); + //grid_idx2coord(phi, im1, jm1, km1, p[0]); + //grid_idx2coord(phi, im1, jm1, k, p[1]); + //grid_idx2coord(phi, im1, jm1, kp1, p[2]); + //grid_idx2coord(phi, im1, j, km1, p[3]); + //grid_idx2coord(phi, im1, j, k, p[4]); + //grid_idx2coord(phi, im1, j, kp1, p[5]); + //grid_idx2coord(phi, im1, jp1, km1, p[6]); + //grid_idx2coord(phi, im1, jp1, k, p[7]); + //grid_idx2coord(phi, im1, jp1, kp1, p[8]); + //grid_idx2coord(phi, i, jm1, km1, p[9]); + //grid_idx2coord(phi, i, jm1, k, p[10]); + //grid_idx2coord(phi, i, jm1, kp1, p[11]); + //grid_idx2coord(phi, i, j, km1, p[12]); + //grid_idx2coord(phi, i, j, k, p[13]); + //grid_idx2coord(phi, i, j, kp1, p[14]); + //grid_idx2coord(phi, i, jp1, km1, p[15]); + //grid_idx2coord(phi, i, jp1, k, p[16]); + //grid_idx2coord(phi, i, jp1, kp1, p[17]); + //grid_idx2coord(phi, ip1, jm1, km1, p[18]); + //grid_idx2coord(phi, ip1, jm1, k, p[19]); + //grid_idx2coord(phi, ip1, jm1, kp1, p[20]); + //grid_idx2coord(phi, ip1, j, km1, p[21]); + //grid_idx2coord(phi, ip1, j, k, p[22]); + //grid_idx2coord(phi, ip1, j, kp1, p[23]); + //grid_idx2coord(phi, ip1, jp1, km1, p[24]); + //grid_idx2coord(phi, ip1, jp1, k, p[25]); + //grid_idx2coord(phi, ip1, jp1, kp1, p[26]); + //s[0] = phi( im1, jm1, km1); + //s[1] = phi( im1, jm1, k); + //s[2] = phi( im1, jm1, kp1); + //s[3] = phi( im1, j, km1); + //s[4] = phi( im1, j, k); + //s[5] = phi( im1, j, kp1); + //s[6] = phi( im1, jp1, km1); + //s[7] = phi( im1, jp1, k); + //s[8] = phi( im1, jp1, kp1); + //s[9] = phi( i, jm1, km1); + //s[10] = phi( i, jm1, k); + //s[11] = phi( i, jm1, kp1); + //s[12] = phi( i, j, km1); + //s[13] = phi( i, j, k); + //s[14] = phi( i, j, kp1); + //s[15] = phi( i, jp1, km1); + //s[16] = phi( i, jp1, k); + //s[17] = phi( i, jp1, kp1); + //s[18] = phi( ip1, jm1, km1); + //s[19] = phi( ip1, jm1, k); + //s[20] = phi( ip1, jm1, kp1); + //s[21] = phi( ip1, j, km1); + //s[22] = phi( ip1, j, k); + //s[23] = phi( ip1, j, kp1); + //s[24] = phi( ip1, jp1, km1); + //s[25] = phi( ip1, jp1, k); + //s[26] = phi( ip1, jp1, kp1); + //for(size_t i=0u;i<27u;++i) + //{ + // q[i] = p[i] - g[i]*s[i]; + // d[i] = q[i] - p[i]; + //} + //bool is_junction = false; + //real_type min_cos_theta = 100000.0; + //real_type max_cos_theta = 0.0; + //for(size_t i=0u;i<27u;++i) + //{ + // if(i==13) + // continue; + // for(size_t j=i+1;j<27u;++j) + // { + // if(j==13) + // continue; + // real_type cos_theta = fabs( g[i] * g[j] ); + // cos_theta = max(0.0, min(cos_theta, 1.0)); + // min_cos_theta = min(cos_theta,min_cos_theta); + // max_cos_theta = max(cos_theta,max_cos_theta); + // } + //} + //std::cout << min_cos_theta << " " << max_cos_theta << std::endl; + //if(min_cos_theta >0.01 && min_cos_theta< 0.99) + // is_junction = true; + //if(is_junction) + //points.push_back(p[13]); + // } + //------------------------------------------------------- + //------------------------------------------------------- + //------------------------------------------------------- + + std::cout << "junctions(): junction candiates = " << points.size() << std::endl; + } + + + template + inline void junctions2( + grid_type const & aof + , grid_type const & phi + , mesh_type /*const*/ & mesh + , point_container & points + ) + { + //--- For each point in the clamped AOF take a sphere with radius + //--- equal to the distance value plus some small threshold (voxel size!), test + //--- the sphere for intersection with the mesh-faces + //--- + //--- If at least four intersection points are found, then the AOF point is a potential junction... no! Think of a pyramid + //--- + //--- At least four points is necessary but not a sufficient condition... + //--- + //--- For each intersection take a vector from the AOF point to the face intersection point, if all such vectors + //--- contrain the sphere then we have a junction point. That is the vectors must span the entire R^3 (to remove + //--- 3DOF of translation motion of the sphere along the mrep). + //--- + //--- Do we need sub-pixel precision of AOF points?...... + //--- + //--- + //--- + //--- culver : exact polyhedra + //--- foskey : theta-SMA + //--- hoffmann: voronoi-region/distance map + //--- + //--- + //--- + //--- + + using std::fabs; + using std::min; + using std::sqrt; + + typedef typename point_container::value_type vector3_type; + typedef typename vector3_type::value_type real_type; + typedef typename grid_type::const_iterator const_iterator; + typedef typename grid_type::const_index_iterator const_index_iterator; + + typedef typename grid_type::math_types math_types; + typedef typename mesh_type::face_type face_type; + typedef detail::JunctionCandidate candidate_type; + + typedef std::list candidate_container; + typedef detail::JunctionCollisionPolicy collision_policy; + typedef AABBDataQuery< typename collision_policy::hash_grid, collision_policy > query_algorithm; + + bool results = false; + query_algorithm query; + candidate_container candidates; + + { + real_type dx = phi.dx()*0.5; + real_type dy = phi.dy()*0.5; + real_type dz = phi.dz()*0.5; + real_type rho = sqrt(dx*dx+dy*dy+dz*dz)*.5; + + const_iterator p = phi.begin(); + const_index_iterator a = aof.begin(); + const_index_iterator end = aof.end(); + for(;a!=end;++a,++p) + { + if( (*a) < 1e-5 || (*p) >= 0) + continue; + vector3_type center; + idx2coord(a,center); + real_type radius = fabs( *p ); + candidate_type candidate(center,radius + rho); + candidates.push_back(candidate); + } + } + + query.init(candidates.begin(),candidates.end()); + query(mesh.face_begin(), mesh.face_end(), candidates.begin(), candidates.end(), results, typename query_algorithm::all_tag() ); + + size_t cnt = 0; + for(typename candidate_container::iterator c = candidates.begin(); c!=candidates.end();++c) + { + if(c->m_cnt_boundary >= 4) + { + points.push_back(c->m_center ); + ++cnt; + } + } + std::cout << "junctions2(): junction candiates = " << cnt << std::endl; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_JUNCTIONS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_local_minima.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_local_minima.h new file mode 100644 index 000000000..99b921011 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_local_minima.h @@ -0,0 +1,160 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_LOCAL_MINIMA_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_LOCAL_MINIMA_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +namespace OpenTissue +{ + namespace grid + { + namespace detail + { + /** + * Test if a specified node is a strict local minima. + * + * @param i + * @param j + * @param k + * @param phi + * + * @return True if node i,j,k is a local minima, otherwise false. + */ + template < typename grid_type > + inline bool is_local_minima( + size_t i + , size_t j + , size_t k + , grid_type const & phi + ) + { + using std::min; + static size_t idx[27]; + size_t I = phi.I(); + size_t J = phi.J(); + size_t K = phi.K(); + size_t im1 = ( i ) ? i - 1 : 0; + size_t jm1 = ( j ) ? j - 1 : 0; + size_t km1 = ( k ) ? k - 1 : 0; + size_t ip1 = min( i + 1u, I - 1u ); + size_t jp1 = min( j + 1u, J - 1u ); + size_t kp1 = min( k + 1u, K - 1u ); + idx[0] = ( kp1 * J + jp1 ) * I + im1; + idx[1] = ( k * J + jp1 ) * I + im1; + idx[2] = ( km1 * J + jp1 ) * I + im1; + idx[3] = ( kp1 * J + j ) * I + im1; + idx[4] = ( k * J + j ) * I + im1; + idx[5] = ( km1 * J + j ) * I + im1; + idx[6] = ( kp1 * J + jm1 ) * I + im1; + idx[7] = ( k * J + jm1 ) * I + im1; + idx[8] = ( km1 * J + jm1 ) * I + im1; + idx[9] = ( kp1 * J + jp1 ) * I + i; + idx[10] = ( k * J + jp1 ) * I + i; + idx[11] = ( km1 * J + jp1 ) * I + i; + idx[12] = ( kp1 * J + j ) * I + i; + idx[13] = ( k * J + j ) * I + i; + idx[14] = ( km1 * J + j ) * I + i; + idx[15] = ( kp1 * J + jm1 ) * I + i; + idx[16] = ( k * J + jm1 ) * I + i; + idx[17] = ( km1 * J + jm1 ) * I + i; + idx[18] = ( kp1 * J + jp1 ) * I + ip1; + idx[19] = ( k * J + jp1 ) * I + ip1; + idx[20] = ( km1 * J + jp1 ) * I + ip1; + idx[21] = ( kp1 * J + j ) * I + ip1; + idx[22] = ( k * J + j ) * I + ip1; + idx[23] = ( km1 * J + j ) * I + ip1; + idx[24] = ( kp1 * J + jm1 ) * I + ip1; + idx[25] = ( k * J + jm1 ) * I + ip1; + idx[26] = ( km1 * J + jm1 ) * I + ip1; + + for(size_t i=0;i<27u;++i) + if( phi(idx[13]) > phi(idx[i]) ) + return false; + return true; + } + + } // namespace detail + + + /** + * Extract local minima nodes. + * Note that local minima may exist at non-nodal locations. This function + * only considers nodal positions. Thus if you are looking for local minima + * at sub-pixel accuracy, then you need another approach. + * + * @param phi The map from where local minima nodes should be extracted from. + * @param points Upon return this container contains all the coordinates of all nodes that where local minima. + */ + template < typename grid_type,typename point_container > + inline void local_minima_as_points( + grid_type const & phi + , point_container & points + ) + { + typedef typename point_container::value_type vector3_type; + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename vector3_type::value_type real_type; + typedef typename grid_type::value_type value_type; + + const_index_iterator pend = phi.end(); + const_index_iterator p = phi.begin(); + for(;p!=pend;++p) + { + if(detail::is_local_minima(p.i(),p.j(),p.k(),phi)) + { + vector3_type point; + idx2coord(phi,p.i(),p.j(),p.k(),point); + points.push_back(point); + } + } + } + + /** + * Extract Local Minima as Points. + * + * @param phi + * @param mask A mask that can be used to mask-out nodes in phi, which is not + * of interest. Positive values correspond to nodes that are allowed + * to be classified as local minima. + * @param points + */ + template < typename grid_type,typename point_container > + inline void local_minima_as_points( + grid_type const & phi + , grid_type const & mask + , point_container & points + ) + { + typedef typename point_container::value_type vector3_type; + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename vector3_type::value_type real_type; + typedef typename grid_type::value_type value_type; + const_index_iterator end = phi.end(); + const_index_iterator p = phi.begin(); + const_index_iterator m = mask.begin(); + for(;p!=end;++p,++m) + { + if( (*m) <= 0 ) + continue; + if(detail::is_local_minima(p.i(),p.j(),p.k(),phi)) + { + vector3_type point; + idx2coord(phi,p.i(),p.j(),p.k(),point); + points.push_back(point); + } + } + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_LOCAL_MINIMA_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_mean_curvature.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_mean_curvature.h new file mode 100644 index 000000000..ec9e2cf51 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_mean_curvature.h @@ -0,0 +1,94 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_MEAN_CURVATURE_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_MEAN_CURVATURE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +#include +#include + +namespace OpenTissue +{ + namespace grid + { + + template + inline void mean_curvature( + grid_type const & grid + , size_t i + , size_t j + , size_t k + , real_type & K + ) + { + using std::min; + using std::max; + using std::pow; + + typedef OpenTissue::math::Vector3 vector3_type; + typedef OpenTissue::math::Matrix3x3 matrix3x3_type; + + real_type limit_K = boost::numeric_cast( 1. / min( grid.dx(), min( grid.dy(), grid.dz() ) ) ); + + vector3_type g; + matrix3x3_type H; + + gradient( grid, i, j, k, g ); + hessian( grid, i, j, k, H ); + real_type h = g * g; + + //--- Test whether the gradient was zero, if so we simply imagine it has norm one, a better + //--- solution would proberly be to pick a random node and compute the curvature information + //--- herein (this is suggest by Oscher and Fedkiw). + if ( h == 0 ) + h = 1; + //--- Compute Mean curvature, defined as: kappa = \nabla \cdot (\nabla \phi / \norm{\nabla \phi} ) + const static real_type exponent = boost::numeric_cast( 3. / 2. ); + K = ( 1.0 / pow( h, exponent ) ) * ( + g( 0 ) * g( 0 ) * ( H( 1, 1 ) + H( 2, 2 ) ) - 2. * g( 1 ) * g( 2 ) * H( 1, 2 ) + + g( 1 ) * g( 1 ) * ( H( 0, 0 ) + H( 2, 2 ) ) - 2. * g( 0 ) * g( 2 ) * H( 0, 2 ) + + g( 2 ) * g( 2 ) * ( H( 0, 0 ) + H( 1, 1 ) ) - 2. * g( 0 ) * g( 1 ) * H( 0, 1 ) + ); + //--- Clamp Curvature, it does not make sense if we compute + //--- a curvature value that can not be representated with the + //--- current map resolution. + K = min( K, limit_K ); + K = max( K, -limit_K ); + } + + template + inline void mean_curvature (grid_iterator const & iter, real_type & K) + { + typedef typename grid_iterator::grid_type grid_type; + + size_t i = iter.i(); + size_t j = iter.j(); + size_t k = iter.k(); + + grid_type const & grid = iter.get_grid(); + mean_curvature(grid, i, j, k, K); + } + + template + inline typename grid_iterator::math_types::real_type mean_curvature( grid_iterator const & iter ) + { + typedef typename grid_iterator::math_types::real_type real_type; + + real_type K; + mean_curvature(iter, K); + + return K; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_MEAN_CURVATURE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_metamorphosis.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_metamorphosis.h new file mode 100644 index 000000000..f64300830 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_metamorphosis.h @@ -0,0 +1,239 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_METAMORPHOSIS_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_METAMORPHOSIS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace grid + { + + /** + * Auxilliary function used by metamorphosis(). + * + * This function compute the normal velocity flow. + * + * + * + * @param phi The current surface + * @param gamma The inside-outside function of target surface + * @param F Upon return contains the normal velocity flow field. + */ + template< + typename grid_type + > + inline void metamorphosis_speed_function( + grid_type const & phi + , grid_type const & gamma + , grid_type & F + ) + { + using std::min; + using std::max; + using std::sqrt; + + typedef typename grid_type::value_type value_type; + typedef typename grid_type::iterator iterator; + typedef typename grid_type::index_iterator index_iterator; + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename grid_type::math_types math_types; + typedef typename math_types::real_type real_type; + + size_t I = phi.I(); + size_t J = phi.J(); + size_t K = phi.K(); + real_type dx = phi.dx(); + real_type dy = phi.dy(); + real_type dz = phi.dz(); + + const_index_iterator end = phi.end(); + const_index_iterator u = phi.begin(); + index_iterator f = F.begin(); + const_index_iterator g = gamma.begin(); + + for(;u!=end; ++u,++g,++f) + { + size_t i = u.i(); + size_t j = u.j(); + size_t k = u.k(); + + size_t im1 = ( i ) ? i - 1 : 0; + size_t jm1 = ( j ) ? j - 1 : 0; + size_t km1 = ( k ) ? k - 1 : 0; + size_t ip1 = min( i + 1u, I - 1u ); + size_t jp1 = min( j + 1u, J - 1u ); + size_t kp1 = min( k + 1u, K - 1u ); + size_t idx = ( k * J + j ) * I + i; + size_t idx_im1 = ( k * J + j ) * I + im1; + size_t idx_ip1 = ( k * J + j ) * I + ip1; + size_t idx_jm1 = ( k * J + jm1 ) * I + i; + size_t idx_jp1 = ( k * J + jp1 ) * I + i; + size_t idx_km1 = ( km1 * J + j ) * I + i; + size_t idx_kp1 = ( kp1 * J + j ) * I + i; + real_type u_idx = phi(idx); + real_type u_idx_im1 = phi(idx_im1); + real_type u_idx_ip1 = phi(idx_ip1); + real_type u_idx_jm1 = phi(idx_jm1); + real_type u_idx_jp1 = phi(idx_jp1); + real_type u_idx_km1 = phi(idx_km1); + real_type u_idx_kp1 = phi(idx_kp1); + + real_type Upx = (u_idx_ip1 - u_idx )/ dx; + real_type Upy = (u_idx_jp1 - u_idx )/ dy; + real_type Upz = (u_idx_kp1 - u_idx )/ dz; + real_type Umx = (u_idx - u_idx_im1)/ dx; + real_type Umy = (u_idx - u_idx_jm1)/ dy; + real_type Umz = (u_idx - u_idx_km1)/ dz; + + real_type Upx_min = min( Upx, 0.0); + real_type Upx_max = max( Upx, 0.0); + real_type Umx_min = min( Umx, 0.0); + real_type Umx_max = max( Umx, 0.0); + + real_type Upy_min = min( Upy, 0.0); + real_type Upy_max = max( Upy, 0.0); + real_type Umy_min = min( Umy, 0.0); + real_type Umy_max = max( Umy, 0.0); + + real_type Upz_min = min( Upz, 0.0); + real_type Upz_max = max( Upz, 0.0); + real_type Umz_min = min( Umz, 0.0); + real_type Umz_max = max( Umz, 0.0); + + if(-(*g)>=0) + *f = (*g)*sqrt( + Upx_min*Upx_min + + Umx_max*Umx_max + + Upy_min*Upy_min + + Umy_max*Umy_max + + Upz_min*Upz_min + + Umz_max*Umz_max + ); + else + *f = (*g)*sqrt( + Upx_max*Upx_max + + Umx_min*Umx_min + + Upy_max*Upy_max + + Umy_min*Umy_min + + Upz_max*Upz_max + + Umz_min*Umz_min + ); + } + //std::cout << "metamorphosis_speed_function(): done..." << std::endl; + } + + + /** + * Solid Shape Metamorphosis. + * + * Solve the PDE: + * + * \frac{\partial \phi(x) }{ \partial t} = | \nabla \phi(x)| \gamma(x) + * + * where \gamma usually is an inside-outside function of the target + * object. Usually the signed distance map of the target is used as + * \gamma. That is + * + * \gamma (x) = \phi_target (x) + * + * It could be defined more genrally as + * + * \gamma (x) = f ( \phi_target (x) ) + * + * where the function f is used to control the speed of the moving + * interface of phi. + * + * + * The process is controlled by the user by defining the initial overlapping + * regions of source and target. + * + * + * The final metamorphosis strategy is + * + * \frac{\partial \phi(x) }{ \partial t} = | \nabla \phi(x)| \gamma( T(x,alpha) ) + * + * Given the coordinate transform: + * + * y = T(x,alpha) + * + * where x is a point in source and y is corresponding point in + * target, and 0 \leq alpha \leq 1 is a parameterization such that + * + * T(x,0) = x + * T(x,1) = y + * + * The transform T is for instance useful for alignment of the objects. + * + * + * The flow is implemented using a upwind (Godonov) scheme. Currently + * implementation do not support the T-transform. The user do have the + * option of applying an f-function outside the function, since only + * the gamma-field is surplied to this function. + * + * + * @param phi Input/output level set. + * @param gamma The inside/outside function of the target object. + * @param dt The time-step size. + */ + template< + typename grid_type + > + inline void metamorphosis( + grid_type & phi + , grid_type const & gamma + , typename grid_type::math_types::real_type const & dt + ) + { + typedef typename grid_type::value_type value_type; + typedef typename grid_type::iterator iterator; + typedef typename grid_type::index_iterator index_iterator; + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename grid_type::math_types math_types; + typedef typename math_types::real_type real_type; + + using std::min; + using std::max; + using std::fabs; + + real_type dx = phi.dx(); + real_type dy = phi.dy(); + real_type dz = phi.dz(); + + grid_type F = phi; + + real_type min_delta = min ( dx, min( dy, dz)); + assert(min_delta>0 || !"metamorphosis(): minimum spacing was non-positive!"); + real_type max_gamma = max ( fabs( max(gamma)), fabs( min(gamma) ) ); + assert(max_gamma>0 || !"metamorphosis(): maximum absolute speed was non-positive!"); + real_type time = static_cast(0.0); + + while (time < dt) + { + metamorphosis_speed_function(phi,gamma,F); + + real_type time_step = min( dt, min_delta / (3.0*max_gamma) ); + assert(time_step>0 || !"metamorphosis(): time_step was non-positive!"); + std::cout << "\ttime step = " << time_step << std::endl; + + index_iterator end = phi.end(); + index_iterator u = phi.begin(); + index_iterator f = F.begin(); + for(;u!=end; ++u,++f) + (*u) = (*u) + time_step *(*f); + + time += time_step; + } + std::cout << "metamorphosis(): done..." << std::endl; + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_METAMORPHOSIS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_opening.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_opening.h new file mode 100644 index 000000000..720ff90e8 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_opening.h @@ -0,0 +1,48 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_OPENING_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_OPENING_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +namespace OpenTissue +{ + namespace grid + { + + /** + * Opening Operation. + * + * @param phi Input level set. + * @param radius Radius of spherical structural element. + * @param dt Time-step to use in update. + * @param psi Output levelset. Note if input is a signed distance map then output may not be a signed distance map, thus you may need to redistance output levelset. + */ + template< + typename grid_type_in + , typename real_type + , typename grid_type_out + > + inline void opening( + grid_type_in const & phi + , real_type const & radius + , real_type const & dt + , grid_type_out & psi + ) + { + dilation(phi,radius,dt,psi); + erosion(psi,radius,dt,psi); + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_OPENING_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_poisson_solver.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_poisson_solver.h new file mode 100644 index 000000000..6cc9edae4 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_poisson_solver.h @@ -0,0 +1,177 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_POISSON_SOLVER_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_POISSON_SOLVER_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +#include +#include + +namespace OpenTissue +{ + namespace grid + { + + /** + * Gauss-Seidel Poisson Solver with Pure Von Neuman Boundary Conditions. + * + * Theory: Given the PDE + * + * \nabla^2 \phi = W + * + * Solve for phi. Writing out we have + * + * \frac{\partial^2}{\partial x^2} phi + \frac{\partial^2}{\partial y^2} phi + \frac{\partial^2}{\partial z^2} phi = W + * + * Using central diff approximation leads to + * + * a_2 ( phi_{i+1,j,k} + phi_{i-1,j,k} ) + a_1( phi_{i,j+1,k} + phi_{i,j-1,k} ) + a_0( phi_{i,j,k+1} + phi_{i,j,k-1} ) - a_3 W + * phi_{i,j,k} = -------------------------------------------------------------------------------------------------------------------------------- + * 2 a_4 + * + * where + * + * a_0 = (dx*dx*dy*dy) + * a_1 = (dx*dx*dz*dz) + * a_2 = (dy*dy*dz*dz) + * a_3 = (dx*dx*dy*dy*dz*dz) + * a_4 = a_0 + a_1 + a_2 + * + * In case dx=dy=dz this simplifies to + * + * phi_{i+1,j,k} + phi_{i-1,j,k} + phi_{i,j+1,k} + phi_{i,j-1,k} + phi_{i,j,k+1} + phi_{i,j,k-1} - dx*dx W + * phi_{i,j,k} = ---------------------------------------------------------------------------------------------------------- + * 8 + * + * The solver uses pure Neumann bondary conditions. i.e.: + * + * \nabla phi = 0 + * + * on any boundary. This means that values outside boundary are copied from + * nearest boundary voxel => claming out-of-bound indices onto boundary. + * + * @param phi Contains initial guess for solution, and upon + * return contains the solution. + * @param b The right hand side of the poisson equation. + * @param max_iterations The maximum number of iterations allowed. Default is 30 iterations. + */ + template < typename grid_type > + inline void poisson_solver( + grid_type & phi + , grid_type const & W + , size_t max_iterations = 10 + ) + { + using std::min; + + typedef typename grid_type::value_type value_type; + typedef typename grid_type::const_iterator const_iterator; + typedef typename grid_type::index_iterator index_iterator; + typedef typename grid_type::math_types math_types; + typedef typename math_types::real_type real_type; + + size_t I = phi.I(); + size_t J = phi.J(); + size_t K = phi.K(); + real_type dx = phi.dx(); + real_type dy = phi.dy(); + real_type dz = phi.dz(); + + if(dx!=dy || dx!=dz || dy!=dz) + { + //std::cout << "poisson_solver(): non-uniform grid" << std::endl; + + real_type a0 = dx*dx*dy*dy; + real_type a1 = dx*dx*dz*dz; + real_type a2 = dy*dy*dz*dz; + real_type a3 = dx*dx*dy*dy*dz*dz; + real_type a4 = 1.0/(2*(a0 + a1 + a2)); + + for(size_t iteration=0;iteration + +#include + +#include +#include +#include + +namespace OpenTissue +{ + namespace grid + { + + namespace detail + { + class FullRedistance + { + protected: + + template < typename grid_type > + typename grid_type::value_type compute_speed( + grid_type const & phi + , grid_type const & S0 + , grid_type & speed + ) + { + using std::min; + using std::max; + using std::sqrt; + using std::fabs; + + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename grid_type::const_iterator const_iterator; + typedef typename grid_type::iterator iterator; + typedef typename grid_type::math_types math_types; + typedef typename math_types::value_type real_type; + + assert(phi.I()==S0.I() || !"compute_speed(): incompatible grid dimensions"); + assert(phi.J()==S0.J() || !"compute_speed(): incompatible grid dimensions"); + assert(phi.K()==S0.K() || !"compute_speed(): incompatible grid dimensions"); + assert(phi.min_coord()==S0.min_coord() || !"compute_speed(): incompatible grid side lengths"); + assert(phi.max_coord()==S0.max_coord() || !"compute_speed(): incompatible grid side lengths"); + + speed.create(phi.min_coord(),phi.max_coord(),phi.I(),phi.J(),phi.K()); + + real_type inv_dx = static_cast ( 1.0 / phi.dx()); + real_type inv_dy = static_cast ( 1.0 / phi.dy()); + real_type inv_dz = static_cast ( 1.0 / phi.dz()); + + real_type inv_dx2 = inv_dx*inv_dx; + real_type inv_dy2 = inv_dx*inv_dy; + real_type inv_dz2 = inv_dx*inv_dz; + + real_type cfl_condition = real_type(0.0); + real_type zero = static_cast(0.0); + + iterator f = speed.begin(); + const_index_iterator p = phi.begin(); + const_index_iterator end = phi.end(); + const_iterator s0 = S0.begin(); + + size_t I = phi.I(); + size_t J = phi.J(); + size_t K = phi.K(); + + for ( ; p != end; ++p, ++f,++s0) + { + size_t i = p.i(); + size_t j = p.j(); + size_t k = p.k(); + size_t im1 = ( i ) ? i - 1 : 0; + size_t jm1 = ( j ) ? j - 1 : 0; + size_t km1 = ( k ) ? k - 1 : 0; + size_t ip1 = min( i + 1, I - 1 ); + size_t jp1 = min( j + 1, J - 1 ); + size_t kp1 = min( k + 1, K - 1 ); + size_t idx_000 = ( k * J + j ) * I + i; + size_t idx_m00 = ( k * J + j ) * I + im1; + size_t idx_p00 = ( k * J + j ) * I + ip1; + size_t idx_0m0 = ( k * J + jm1 ) * I + i; + size_t idx_0p0 = ( k * J + jp1 ) * I + i; + size_t idx_00m = ( km1 * J + j ) * I + i; + size_t idx_00p = ( kp1 * J + j ) * I + i; + real_type d000 = phi( idx_000 ); + real_type dp00 = phi( idx_p00 ); + real_type dm00 = phi( idx_m00 ); + real_type d0p0 = phi( idx_0p0 ); + real_type d0m0 = phi( idx_0m0 ); + real_type d00p = phi( idx_00p ); + real_type d00m = phi( idx_00m ); + assert( is_finite( d000 ) || !"compute_speed(): NaN encountered"); + assert( is_finite( dp00 ) || !"compute_speed(): NaN encountered"); + assert( is_finite( dm00 ) || !"compute_speed(): NaN encountered"); + assert( is_finite( d0p0 ) || !"compute_speed(): NaN encountered"); + assert( is_finite( d0m0 ) || !"compute_speed(): NaN encountered"); + assert( is_finite( d00p ) || !"compute_speed(): NaN encountered"); + assert( is_finite( d00m ) || !"compute_speed(): NaN encountered"); + real_type dxp = (dp00 - d000)*inv_dx; + real_type dxm = (d000 - dm00)*inv_dx; + real_type dyp = (d0p0 - d000)*inv_dy; + real_type dym = (d000 - d0m0)*inv_dy; + real_type dzp = (d00p - d000)*inv_dz; + real_type dzm = (d000 - d00m)*inv_dz; + assert( is_finite( dxp ) || !"compute_speed(): NaN encountered"); + assert( is_finite( dxm ) || !"compute_speed(): NaN encountered"); + assert( is_finite( dyp ) || !"compute_speed(): NaN encountered"); + assert( is_finite( dym ) || !"compute_speed(): NaN encountered"); + assert( is_finite( dzp ) || !"compute_speed(): NaN encountered"); + assert( is_finite( dzm ) || !"compute_speed(): NaN encountered"); + real_type dxp_max = max( dxp, zero ); + real_type dxm_max = max( dxm, zero ); + real_type dyp_max = max( dyp, zero ); + real_type dym_max = max( dym, zero ); + real_type dzp_max = max( dzp, zero ); + real_type dzm_max = max( dzm, zero ); + real_type dxp_min = min( dxp, zero ); + real_type dxm_min = min( dxm, zero ); + real_type dyp_min = min( dyp, zero ); + real_type dym_min = min( dym, zero ); + real_type dzp_min = min( dzp, zero ); + real_type dzm_min = min( dzm, zero ); + real_type dxp_max_2 = dxp_max * dxp_max; + real_type dxm_max_2 = dxm_max * dxm_max; + real_type dyp_max_2 = dyp_max * dyp_max; + real_type dym_max_2 = dym_max * dym_max; + real_type dzp_max_2 = dzp_max * dzp_max; + real_type dzm_max_2 = dzm_max * dzm_max; + real_type dxp_min_2 = dxp_min * dxp_min; + real_type dxm_min_2 = dxm_min * dxm_min; + real_type dyp_min_2 = dyp_min * dyp_min; + real_type dym_min_2 = dym_min * dym_min; + real_type dzp_min_2 = dzp_min * dzp_min; + real_type dzm_min_2 = dzm_min * dzm_min; + // yellowbook p58 + real_type phi_x_2_p = max( dxm_max_2, dxp_min_2 ); + real_type phi_x_2_m = max( dxm_min_2, dxp_max_2 ); + real_type phi_y_2_p = max( dym_max_2, dyp_min_2 ); + real_type phi_y_2_m = max( dym_min_2, dyp_max_2 ); + real_type phi_z_2_p = max( dzm_max_2, dzp_min_2 ); + real_type phi_z_2_m = max( dzm_min_2, dzp_max_2 ); + // peng99 eq40 + real_type norm_grad_phi_p = sqrt( phi_x_2_p + phi_y_2_p + phi_z_2_p ); + real_type norm_grad_phi_m = sqrt( phi_x_2_m + phi_y_2_m + phi_z_2_m ); + // Godunov scheme (yellowbook p58 eq6.3 and 6.4) + if ( (*s0) > 0 ) + { + (*f) = (*s0) * ( norm_grad_phi_p - 1.0 ); + } + else if ( (*s0) < 0 ) + { + (*f) = (*s0) * ( norm_grad_phi_m - 1.0 ); + } + else + { + (*f) = 0; + } + real_type phi_x = fabs((dp00 - dm00)*inv_dx*.5); + real_type phi_y = fabs((d0p0 - d0m0)*inv_dy*.5); + real_type phi_z = fabs((d00p - d00m)*inv_dz*.5); + real_type norm_grad_phi = sqrt(phi_x*phi_x + phi_y*phi_y + phi_z*phi_z); + + real_type tmp = fabs( (*s0)*phi_x*inv_dx2/norm_grad_phi + (*s0)*phi_y*inv_dy2/norm_grad_phi + (*s0)*phi_z*inv_dz2/norm_grad_phi ); + cfl_condition = max( cfl_condition, tmp ); + } + return 1.0/cfl_condition; + } + + template < typename grid_type , typename real_type > + real_type update( + grid_type const & phi + , grid_type const & speed + , real_type const & time_step + , grid_type & psi + , real_type const & gamma = 10e30 + ) + { + typedef typename grid_type::const_index_iterator const_iterator; + typedef typename grid_type::iterator iterator; + + assert(phi.I()==psi.I() || !"update(): incompatible grid dimensions"); + assert(phi.J()==psi.J() || !"update(): incompatible grid dimensions"); + assert(phi.K()==psi.K() || !"update(): incompatible grid dimensions"); + assert(phi.min_coord()==psi.min_coord() || !"update(): incompatible grid side lengths"); + assert(phi.max_coord()==psi.max_coord() || !"update(): incompatible grid side lengths"); + assert(time_step>0 || !"update(): time-step must be positive"); + + real_type steady = real_type(0.0); + const_iterator i = phi.begin(); + const_iterator f = speed.begin(); + iterator o = psi.begin(); + const_iterator end = phi.end(); + for ( ; i!=end;++i, ++o, ++f) + { + real_type diff = time_step * (*f); + (*o) = (*i) - diff; + diff = std::fabs(diff); + if ( (*o) < gamma && steady < diff ) + steady = diff; + } + return steady; + } + + public: + + /** + * Signed Distance Map Reinitialization. + * + * Brute-force implementation of the reinitialization of the distance function. + * The following Hamilton-Jacobi equation is solved: + * d_tau + S(d) * ( |grad phi| - 1 ) = 0 + * d(x,0) = d_0(x) = phi(x,t) + * to steady state, with S(d) approximated by: + * s = d / ( sqrt( d^2 + |Dd|^2 * delta_x^2) ) + * + * Calculates gradient magnitude of phi using upwind-scheme. + * Performs PDE update using forward Euler time discretization. + * + * @param phi Input level set that should be redistanced into a signed distance grid. + * @param psi Output level set. That is the redistaned phi. + * @param max_iterations The maximum number of iterations allowed to do re-initialization. + * @param stead_threshold The threshold value used to test for steady state. + */ + template < typename grid_type > + void operator()( + grid_type const & phi + , grid_type & psi + , size_t max_iterations = 10 + , double steady_threshold = 0.05 + ) + { + using std::min; + using std::max; + + typedef typename grid_type::value_type real_type; + + assert(phi.I()==psi.I() || !"operator(): incompatible grid dimensions"); + assert(phi.J()==psi.J() || !"operator(): incompatible grid dimensions"); + assert(phi.K()==psi.K() || !"operator(): incompatible grid dimensions"); + assert(phi.min_coord()==psi.min_coord() || !"operator(): incompatible grid side lengths"); + assert(phi.max_coord()==psi.max_coord() || !"operator(): incompatible grid side lengths"); + + static grid_type S0; // TODO: Hmm, these temporaries take up a lot of space!!! + static grid_type speed; + + compute_sign_function(phi,S0); + + real_type alpha = static_cast ( 0.9 ); //--- CFL number + //real_type min_delta = static_cast ( (min( phi.dx(), min( phi.dy(), phi.dz() ) ) ) ); + real_type max_delta = static_cast ( (max( phi.dx(), max( phi.dy(), phi.dz() ) ) ) ); + + grid_type * phi_in = const_cast(&phi); + grid_type * phi_out = ψ + + real_type gamma = static_cast( max_delta * max_iterations ); + real_type steady = static_cast(0.0); + + for(size_t iterations=0;iterations 0 || !"resample(): j scale factor must be positive"); + assert(factor_k > 0 || !"resample(): k scale factor must be positive"); + + typedef typename grid_type::math_types math_types; + typedef typename math_types::vector3_type vector3_type; + typedef typename grid_type::index_iterator index_iterator; + + // TODO: Add gaussian smoothing with a standard deviation corresponding to factor_n. + //grid_type S = M; + //grid_gaussian_convolution(M,S,factor_i/2.0,factor_j/2.0,factor_k/2.0); + + m.create( + M.min_coord(), M.max_coord() + , static_cast( floor(M.I()/factor_i) ) + , static_cast( floor(M.J()/factor_j) ) + , static_cast( floor(M.K()/factor_k) ) + ); + + vector3_type point; + for( index_iterator iter = m.begin(); iter != m.end(); ++iter ) + { + idx2coord( iter,point ); + (*iter) = value_at_point( M, point ); + } + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_RESAMPLE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_second_derivative.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_second_derivative.h new file mode 100644 index 000000000..f12efab6a --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_second_derivative.h @@ -0,0 +1,86 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_SECOND_DERIVATVE_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_SECOND_DERIVATVE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include // Needed for vector3. +#include // Needed for matrix3x3. + +namespace OpenTissue +{ + namespace grid + { + + template + inline void second_derivative( + grid_type const & grid + , size_t i + , size_t j + , size_t k + , real_type & derivative + ) + { + using std::fabs; + + typedef OpenTissue::math::Vector3 vector3_type; + typedef OpenTissue::math::Matrix3x3 matrix3x3_type; + + static vector3_type g; + static matrix3x3_type H; + + + real_type const too_small = 10e-7; + + gradient(grid, i, j, k, g ); + + real_type tmp = g*g; + if(fabs(tmp) < too_small) + { + derivative = 0.0; + return; + } + + hessian(grid, i, j, k, H); + derivative = (1.0)/(tmp)* (g * (H *g)); + } + + template + inline void second_derivative (grid_iterator const & iter, real_type & derivative) + { + typedef typename grid_iterator::grid_type grid_type; + + size_t i = iter.i(); + size_t j = iter.j(); + size_t k = iter.k(); + + grid_type const & grid = iter.get_grid(); + second_derivative(grid, i, j, k, derivative); + } + + template + inline typename grid_iterator::math_types::real_type + second_derivative( grid_iterator const & iter ) + { + typedef typename grid_iterator::math_types::real_type real_type; + + real_type derivative; + + second_derivative(iter, derivative); + + return derivative; + } + + } // namespace grid + +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_SECOND_DERIVATVE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_split2slices.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_split2slices.h new file mode 100644 index 000000000..cc70222c7 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_split2slices.h @@ -0,0 +1,63 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_SPLIT2SLICES_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_SPLIT2SLICES_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace grid + { + + /** + * Split a 3D grid into multiple 2D grids along some major axis. + * + * @param grid The grid that should be split inot slices. + * @param slices Upon return this container holds grids corresponding to all the slices. + * @param axis The axis to split along. + */ + template + inline void split2slices(grid_type & grid, grid_container & slices, size_t axis = 2) + { + typename grid_type::math_types math_types; + typename math_types::vector3_type vector3_type; + + assert( axis==2 || !"split2slices(): Only splits along k-axis is currently supported"); + + slices.clear(); + + if (axis != 2) + return; + + vector3_type min_coord = grid.min_coord(); + vector3_type max_coord = grid.max_coord(); + min_coord(2) = 0; + max_coord(2) = 0; + for (size_t z = 0; z < grid.K(); ++z) + { + + grid_type slice; + slice.create(min_coord, max_coord, grid.I(), grid.J(), 2); + for (size_t y = 0; y < grid.J(); ++y) + for (size_t x = 0; x < grid.I(); ++x) + { + slice(x, y, 0) = grid(x, y, z); + slice(x, y, 1) = grid(x, y, z); // HACKING!!! + } + slices.push_back(slice); + } + } + + } // namespace grid + +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_SPLIT2SLICES_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_strict_local_minima.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_strict_local_minima.h new file mode 100644 index 000000000..2a7651e18 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_strict_local_minima.h @@ -0,0 +1,160 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_STRICT_GRID_LOCAL_MINIMA_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_STRICT_GRID_LOCAL_MINIMA_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +namespace OpenTissue +{ + namespace grid + { + + namespace detail + { + + /** + * Test if a specified node is a strict local minima. + * + * @param i + * @param j + * @param k + * @param phi + * + * @return True if node i,j,k is a local minima, otherwise false. + */ + template < typename grid_type > + inline bool is_strict_local_minima( + size_t i + , size_t j + , size_t k + , grid_type const & phi + ) + { + using std::min; + static size_t idx[27]; + size_t I = phi.I(); + size_t J = phi.J(); + size_t K = phi.K(); + size_t im1 = ( i ) ? i - 1 : 0; + size_t jm1 = ( j ) ? j - 1 : 0; + size_t km1 = ( k ) ? k - 1 : 0; + size_t ip1 = min( i + 1u, I - 1u ); + size_t jp1 = min( j + 1u, J - 1u ); + size_t kp1 = min( k + 1u, K - 1u ); + idx[0] = ( kp1 * J + jp1 ) * I + im1; + idx[1] = ( k * J + jp1 ) * I + im1; + idx[2] = ( km1 * J + jp1 ) * I + im1; + idx[3] = ( kp1 * J + j ) * I + im1; + idx[4] = ( k * J + j ) * I + im1; + idx[5] = ( km1 * J + j ) * I + im1; + idx[6] = ( kp1 * J + jm1 ) * I + im1; + idx[7] = ( k * J + jm1 ) * I + im1; + idx[8] = ( km1 * J + jm1 ) * I + im1; + idx[9] = ( kp1 * J + jp1 ) * I + i; + idx[10] = ( k * J + jp1 ) * I + i; + idx[11] = ( km1 * J + jp1 ) * I + i; + idx[12] = ( kp1 * J + j ) * I + i; + idx[13] = ( k * J + j ) * I + i; + idx[14] = ( km1 * J + j ) * I + i; + idx[15] = ( kp1 * J + jm1 ) * I + i; + idx[16] = ( k * J + jm1 ) * I + i; + idx[17] = ( km1 * J + jm1 ) * I + i; + idx[18] = ( kp1 * J + jp1 ) * I + ip1; + idx[19] = ( k * J + jp1 ) * I + ip1; + idx[20] = ( km1 * J + jp1 ) * I + ip1; + idx[21] = ( kp1 * J + j ) * I + ip1; + idx[22] = ( k * J + j ) * I + ip1; + idx[23] = ( km1 * J + j ) * I + ip1; + idx[24] = ( kp1 * J + jm1 ) * I + ip1; + idx[25] = ( k * J + jm1 ) * I + ip1; + idx[26] = ( km1 * J + jm1 ) * I + ip1; + for(size_t i=0;i<27u;++i) + if( phi(idx[13]) >= phi(idx[i]) ) + return false; + return true; + } + + } // namespace detail + + /** + * Extract local minima nodes. + * Note that local minima may exist at non-nodal locations. This function + * only considers nodal positions. Thus if you are looking for local minima + * at sub-pixel accuracy, then you need another approach. + * + * @param phi The map from where local minima nodes should be extracted from. + * @param points Upon return this container contains all the coordinates of all nodes that where local minima. + */ + template < typename grid_type,typename point_container > + inline void strict_local_minima_as_points( + grid_type const & phi + , point_container & points + ) + { + typedef typename point_container::value_type vector3_type; + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename vector3_type::value_type real_type; + typedef typename grid_type::value_type value_type; + + const_index_iterator pend = phi.end(); + const_index_iterator p = phi.begin(); + for(;p!=pend;++p) + { + if(detail::is_strict_local_minima(p.i(),p.j(),p.k(),phi)) + { + vector3_type point; + idx2coord(phi,p.i(),p.j(),p.k(),point); + points.push_back(point); + } + } + } + + /** + * Extract Strict Local Minima as Points. + * + * @param phi + * @param mask A mask that can be used to mask-out nodes in phi, which is not + * of interest. Positive values correspond to nodes that are allowed + * to be classified as local minima. + * @param points + */ + template < typename grid_type,typename point_container > + inline void strict_local_minima_as_points( + grid_type const & phi + , grid_type const & mask + , point_container & points + ) + { + typedef typename point_container::value_type vector3_type; + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename vector3_type::value_type real_type; + typedef typename grid_type::value_type value_type; + const_index_iterator end = phi.end(); + const_index_iterator p = phi.begin(); + const_index_iterator m = mask.begin(); + for(;p!=end;++p,++m) + { + if( (*m) <= 0 ) + continue; + if(detail::is_strict_local_minima(p.i(),p.j(),p.k(),phi)) + { + vector3_type point; + idx2coord(phi,p.i(),p.j(),p.k(),point); + points.push_back(point); + } + } + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_STRICT_GRID_LOCAL_MINIMA_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_translate.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_translate.h new file mode 100644 index 000000000..ef49febd3 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_translate.h @@ -0,0 +1,87 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_TRANSLATE_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_TRANSLATE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +#include // for abs() + +namespace OpenTissue +{ + namespace grid + { + /** + * Translate a grid by an integer vector and wrap around at border. + * + * Behavior is un-expected if input and output grid is the same.... + * TODO: make it work for zero-trans and negative trans. + * + * @param src Source grid to be translated. + * @param v translation vector. + * @param dst Upon return, contains the translated grid. + */ + template + inline void translate(grid_type const& src, vector3_int_type const& v, grid_type & dst) + { + assert( v(0)>0 && !"translate(): Only works for positive translations for now"); + assert( v(1)>0 && !"translate(): Only works for positive translations for now"); + assert( v(2)>0 && !"translate(): Only works for positive translations for now"); + + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename grid_type::math_types math_types; + typedef typename math_types::vector3_type vector3_type; + + // make sure translation is >0 + vector3_int_type t=v; + //if( t(0)<0 ) + // t(0) += src.I()*(1+abs(t(0)/src.I())); + //if( t(1)<0 ) + // t(1) += src.J()*(1+abs(t(1)/src.J())); + //if( t(2)<0 ) + // t(2) += src.K()*(1+abs(t(2)/src.K())); + + // Save last block of data + grid_type tmp; + tmp.create(vector3_type(0,0,0), vector3_type(1,1,1), t(0), t(1), t(2)); + size_t ti = src.I()-t(0); + size_t tj = src.J()-t(1); + size_t tk = src.K()-t(2); + size_t lk=0; + size_t lj=0; + size_t li=0; + for (size_t gk=tk; gk p( src.end() ), end( src.begin() ); + //for(p; p!=end; ++p) + //{ + // dst( (p.base().i()+t(0))%src.I(), (p.base().j()+t(1))%src.J(), (p.base().k()+t(2))%src.K() ) = *p; + //} + for(const_index_iterator p=src.end()-1; p!=src.begin()-1; --p) + { + dst( (p.i()+t(0))%src.I(), (p.j()+t(1))%src.J(), (p.k()+t(2))%src.K() ) = *p; + } + // copy over the stored values + for (size_t k=0; k + +#include //--- needed for gradient_func and value_func + +namespace OpenTissue +{ + namespace grid + { + + /** + * Uniform Point Sampling of Grid. + * + * @param phi The (signed distance grid) level set grid. + * @param points Upon return this container containts the random points. + * @param sub_Sample Optimal argument, allows one to control the density of points. + */ + template + inline void uniform_point_sampling( + grid_type const & phi + , vector3_container & points + , size_t sub_sample = 2u + ) + { + typedef typename vector3_container::value_type vector3_type; + typedef typename vector3_type::value_type real_type; + + points.clear(); + + real_type min_x = phi.min_coord(0); + real_type min_y = phi.min_coord(1); + real_type min_z = phi.min_coord(2); + + real_type max_x = phi.max_coord(0); + real_type max_y = phi.max_coord(1); + real_type max_z = phi.max_coord(2); + + real_type dx = phi.dx()*sub_sample; + real_type dy = phi.dy()*sub_sample; + real_type dz = phi.dz()*sub_sample; + + for ( real_type x = (min_x+dx); x < (max_x-dx); x += dx ) + for ( real_type y = (min_y+dy); y < (max_y-dy); y += dy ) + for ( real_type z = (min_z+dz); z < (max_z-dz); z += dz ) + { + points.push_back( vector3_type( x, y, z ) ); + } + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_UNIFORM_POINT_SAMPLING_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_upwind_gradient.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_upwind_gradient.h new file mode 100644 index 000000000..e7c97c5ed --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_upwind_gradient.h @@ -0,0 +1,107 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_UPWIND_GRADIENT_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_UPWIND_GRADIENT_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace grid + { + + /** + * Compute Upwind Gradient at Specified Node location. + * + * @param phi A the map in which we want to know the gradient. + * @param F A map containing the values of the speed function. + * @param i The i'th index of the node at which we want to compute the gradient. + * @param j The j'th index of the node at which we want to compute the gradient. + * @param k The k'th index of the node at which we want to compute the gradient. + * @param gradient Upon completion contains the upwind gradient + */ + template + inline void upwind_gradient( + grid_type const & phi + , grid_type const & F + , size_t i + , size_t j + , size_t k + , vector3_type & gradient + ) + { + using std::min; + + typedef typename grid_type::value_type value_type; + typedef typename grid_type::math_types math_types; + typedef typename math_types::real_type real_type; + + size_t I = phi.I(); + size_t J = phi.J(); + size_t K = phi.K(); + real_type inv_dx = 1.0/phi.dx(); + real_type inv_dy = 1.0/phi.dy(); + real_type inv_dz = 1.0/phi.dz(); + real_type inv_2dx = 0.5/phi.dx(); + real_type inv_2dy = 0.5/phi.dy(); + real_type inv_2dz = 0.5/phi.dz(); + + size_t im1 = ( i ) ? i - 1 : 0; + size_t jm1 = ( j ) ? j - 1 : 0; + size_t km1 = ( k ) ? k - 1 : 0; + size_t ip1 = min( i + 1u, I - 1u ); + size_t jp1 = min( j + 1u, J - 1u ); + size_t kp1 = min( k + 1u, K - 1u ); + size_t i000 = ( k * J + j ) * I + i; + size_t im00 = ( k * J + j ) * I + im1; + size_t ip00 = ( k * J + j ) * I + ip1; + size_t i0m0 = ( k * J + jm1 ) * I + i; + size_t i0p0 = ( k * J + jp1 ) * I + i; + size_t i00m = ( km1 * J + j ) * I + i; + size_t i00p = ( kp1 * J + j ) * I + i; + real_type f = F(i000); + real_type v000 = phi(i000); + real_type vp00 = phi(ip00); + real_type vm00 = phi(im00); + real_type v0p0 = phi(i0p0); + real_type v0m0 = phi(i0m0); + real_type v00p = phi(i00p); + real_type v00m = phi(i00m); + // phi^t+1 = phi_t + dt*F + // F > 0 use forward diffs + // F < 0 use backward diffs + real_type nx = 0; + real_type ny = 0; + real_type nz = 0; + if(f>0) + { + nx = (vp00 - v000 ) * inv_dx; + ny = (v0p0 - v000 ) * inv_dy; + nz = (v00p - v000 ) * inv_dz; + } + else if(f<0) + { + nx = (v000 - vm00) * inv_dx; + ny = (v000 - v0m0) * inv_dy; + nz = (v000 - v00m) * inv_dz; + } + else + { + nx = (vp00 - vm00) * inv_2dx; + ny = (v0p0 - v0m0) * inv_2dy; + nz = (v00p - v00m) * inv_2dz; + } + gradient = vector3_type(nx,ny,nz); + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_UPWIND_GRADIENT_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_upwind_gradient_field.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_upwind_gradient_field.h new file mode 100644 index 000000000..27157934c --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_upwind_gradient_field.h @@ -0,0 +1,115 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_UPWIND_GRADIENT_FIELD_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_UPWIND_GRADIENT_FIELD_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace grid + { + + /** + * Picks upwind direction of gradient according to the speed function (F). + */ + template < typename grid_type > + inline void upwind_gradient_field( + grid_type const & phi + , grid_type const & F + , grid_type & Nx + , grid_type & Ny + , grid_type & Nz + ) + { + using std::min; + + typedef OpenTissue::math::Vector3< typename grid_type::value_type> vector3_type; + + typedef typename grid_type::value_type value_type; + typedef typename grid_type::iterator iterator; + typedef typename grid_type::const_iterator const_iterator; + typedef typename grid_type::const_index_iterator const_index_iterator; + typedef typename grid_type::math_types math_types; + typedef typename math_types::real_type real_type; + + size_t I = phi.I(); + size_t J = phi.J(); + size_t K = phi.K(); + real_type inv_dx = 1.0/phi.dx(); + real_type inv_dy = 1.0/phi.dy(); + real_type inv_dz = 1.0/phi.dz(); + + real_type inv_2dx = 0.5/phi.dx(); + real_type inv_2dy = 0.5/phi.dy(); + real_type inv_2dz = 0.5/phi.dz(); + + iterator nx = Nx.begin(); + iterator ny = Ny.begin(); + iterator nz = Nz.begin(); + const_iterator f = F.begin(); + const_index_iterator pbegin = phi.begin(); + const_index_iterator pend = phi.end(); + const_index_iterator p = pbegin; + + for(;p!=pend; ++f,++p,++nx,++ny,++nz) + { + size_t i = p.i(); + size_t j = p.j(); + size_t k = p.k(); + + size_t im1 = ( i ) ? i - 1 : 0; + size_t jm1 = ( j ) ? j - 1 : 0; + size_t km1 = ( k ) ? k - 1 : 0; + size_t ip1 = min( i + 1u, I - 1u ); + size_t jp1 = min( j + 1u, J - 1u ); + size_t kp1 = min( k + 1u, K - 1u ); + size_t i000 = ( k * J + j ) * I + i; + size_t im00 = ( k * J + j ) * I + im1; + size_t ip00 = ( k * J + j ) * I + ip1; + size_t i0m0 = ( k * J + jm1 ) * I + i; + size_t i0p0 = ( k * J + jp1 ) * I + i; + size_t i00m = ( km1 * J + j ) * I + i; + size_t i00p = ( kp1 * J + j ) * I + i; + real_type v000 = phi(i000); + real_type vp00 = phi(ip00); + real_type vm00 = phi(im00); + real_type v0p0 = phi(i0p0); + real_type v0m0 = phi(i0m0); + real_type v00p = phi(i00p); + real_type v00m = phi(i00m); + // phi^t+1 = phi_t + dt*F + // F > 0 use forward diffs + // F < 0 use backward diffs + if((*f)>0) + { + *nx = (vp00 - v000 ) * inv_dx; + *ny = (v0p0 - v000 ) * inv_dy; + *nz = (v00p - v000 ) * inv_dz; + } + else if((*f)<0) + { + *nx = (v000 - vm00) * inv_dx; + *ny = (v000 - v0m0) * inv_dy; + *nz = (v000 - v00m) * inv_dz; + } + else + { + *nx = (vp00 - vm00) * inv_2dx; + *ny = (v0p0 - v0m0) * inv_2dy; + *nz = (v00p - v00m) * inv_2dz; + } + } + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_UPWIND_GRADIENT_FIELD_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_value_at_point.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_value_at_point.h new file mode 100644 index 000000000..97e88e12e --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_value_at_point.h @@ -0,0 +1,77 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_VALUE_AT_POINT_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_VALUE_AT_POINT_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * + * + * Warning this method do not test whether the point lies inside the grid. If the point is + * outside the behavior is undefined. + */ + template + inline typename grid_type::value_type value_at_point(grid_type const & grid, vector3_type const & point ) + { + typedef typename grid_type::value_type value_type; + typedef typename vector3_type::value_type real_type; + + //const static value_type infty = grid.infinity(); + const static value_type unused = grid.unused(); + const static value_type zero = value_type(); //--- by standard default constructed is zero!!! + + size_t i0, j0, k0, i1, j1, k1; + enclosing_indices( grid, point, i0, j0, k0, i1, j1, k1 ); + + value_type d000 = grid( i0, j0, k0 ); + value_type d001 = grid( i1, j0, k0 ); + value_type d010 = grid( i0, j1, k0 ); + value_type d011 = grid( i1, j1, k0 ); + value_type d100 = grid( i0, j0, k1 ); + value_type d101 = grid( i1, j0, k1 ); + value_type d110 = grid( i0, j1, k1 ); + value_type d111 = grid( i1, j1, k1 ); + + size_t cnt_unused = 0; + +#define MAGIC(val) \ + if ( (val) == unused) { \ + (val) = zero; \ + ++cnt_unused; \ + } + MAGIC( d000 ); + MAGIC( d001 ); + MAGIC( d010 ); + MAGIC( d011 ); + MAGIC( d100 ); + MAGIC( d101 ); + MAGIC( d110 ); + MAGIC( d111 ); +#undef MAGIC + + if ( cnt_unused == 8 ) + return unused; + + real_type s = ( point(0) - ( i0*grid.dx() + grid.min_coord(0) ) ) / grid.dx(); + real_type t = ( point(1) - ( j0*grid.dy() + grid.min_coord(1) ) ) / grid.dy(); + real_type u = ( point(2) - ( k0*grid.dz() + grid.min_coord(2) ) ) / grid.dz(); + return OpenTissue::math::trillinear( d000, d001, d010, d011, d100, d101, d110, d111, s, t, u ); + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_VALUE_AT_POINT_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_voxel_plane_clip.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_voxel_plane_clip.h new file mode 100644 index 000000000..97daa9c21 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/grid/util/grid_voxel_plane_clip.h @@ -0,0 +1,54 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_VOXEL_PLANE_CLIP_H +#define OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_VOXEL_PLANE_CLIP_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace grid + { + /** + * Clip a voxelgrid against a plane. + * @param voxels Input voxel grid. + * @param plane Plane to clip against. + * @param below Upon return, contains grid of all voxels below plane. + * @param above Upon return, contains grid of all voxels above plane. + */ + template < typename grid_type, typename plane_type > + inline void voxel_plane_clip( + grid_type const& voxels + , plane_type const& plane + , grid_type & below + , grid_type & above + ) + { + typedef typename grid_type::value_type value_type; + typedef typename grid_type::const_index_iterator const_index_iterator; + + const_index_iterator voxel; + for ( voxel=voxels.begin(); voxel!=voxels.end(); ++voxel ) + { + if ( plane.signed_distance( voxel.get_coord() ) >= 0 ) + { + above( voxel.get_index() ) = voxels( voxel.get_index() ); + below( voxel.get_index() ) = value_type(0); + } + else + { + below( voxel.get_index() ) = voxels( voxel.get_index() ); + above( voxel.get_index() ) = value_type(0); + } + } + } + + } // namespace grid +} // namespace OpenTissue + +// OPENTISSUE_CORE_CONTAINERS_GRID_UTIL_GRID_VOXEL_PLANE_CLIP_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_core_access.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_core_access.h new file mode 100644 index 000000000..ddb111a87 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_core_access.h @@ -0,0 +1,74 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_T4MESH_T4MESH_CORE_ACCESS_H +#define OPENTISSUE_CORE_CONTAINERS_T4MESH_T4MESH_CORE_ACCESS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace t4mesh + { + class t4mesh_core_access + { + public: + + template< typename feature_type, typename index_type> + static void set_index(feature_type & f, index_type idx) + { + f.set_index(idx); + } + + template< typename feature_type, typename mesh_type> + static void set_owner(feature_type & f, mesh_type * owner) + { + f.set_owner(owner); + } + + template< typename tetrahedron_type, typename index_type> + static void set_node0(tetrahedron_type & tetrahedron, index_type idx) + { + tetrahedron.set_node0(idx); + } + + template< typename tetrahedron_type, typename index_type> + static void set_node1(tetrahedron_type & tetrahedron, index_type idx) + { + tetrahedron.set_node1(idx); + } + + template< typename tetrahedron_type, typename index_type> + static void set_node2(tetrahedron_type & tetrahedron, index_type idx) + { + tetrahedron.set_node2(idx); + } + + template< typename tetrahedron_type, typename index_type> + static void set_node3(tetrahedron_type & tetrahedron, index_type idx) + { + tetrahedron.set_node3(idx); + } + + template< typename node_type, typename index_type> + static void tetrahedra_push_back(node_type & node, index_type idx) + { + node.tetrahedra_push_back(idx); + } + + template< typename node_type, typename index_type> + static void tetrahedra_remove(node_type & node, index_type idx) + { + node.tetrahedra_remove(idx); + } + + }; + + } // namespace trimesh +} // namespace OpenTissue + +//OPENTISSUE_CORE_CONTAINERS_T4MESH_T4MESH_CORE_ACCESS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_default_point_container.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_default_point_container.h new file mode 100644 index 000000000..6cb8cef66 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_default_point_container.h @@ -0,0 +1,70 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_T4MESH_T4MESH_DEFAULT_POINT_CONTAINER_H +#define OPENTISSUE_CORE_CONTAINERS_T4MESH_T4MESH_DEFAULT_POINT_CONTAINER_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace t4mesh + { + + /** + * Default Point Container. + * This utility class can be used to make the coordniates of the nodes in + * a tetrahedra mesh appear as a point container, i.e. as though the coordinates + * are stored as + * + * std::vector coordinates; + * + * and accesses as + * + * coordinates[node->idx()] + * + * instead of + * + * node->m_coord + * + * Many algoritms in OpenTissue have been implemented in such a way that they + * do not rely on nodes to have a m_coord member. Instead coordinates are passed + * as point containers. This utility make it convenient to use these algorithms + * on nodes where coordinates are stored in m_coord member. + * + */ + template + struct default_point_container + { + typedef M mesh_type; + typedef typename mesh_type::math_types math_types; + typedef typename math_types::vector3_type value_type; + + mesh_type * m_mesh; + + default_point_container(mesh_type * mesh) : m_mesh(mesh) {} + + value_type & operator[] (unsigned int const & idx) + { + return m_mesh->node(idx)->m_coord; + } + + value_type const & operator[] (unsigned int const & idx)const + { + return m_mesh->node(idx)->m_coord; + } + + void clear(){} + size_t size() const {return m_mesh->size_nodes();} + void resize(size_t){} + }; + + + } // namespace t4mesh +} // namespace OpenTissue + +//OPENTISSUE_CORE_CONTAINERS_T4MESH_T4MESH_DEFAULT_POINT_CONTAINER_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_default_traits.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_default_traits.h new file mode 100644 index 000000000..a47ffff00 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_default_traits.h @@ -0,0 +1,38 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_T4MESH_DEFAULT_TRAITS_H +#define OPENTISSUE_CORE_CONTAINERS_T4MESH_DEFAULT_TRAITS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace t4mesh + { + + template + class DefaultNodeTraits + { + public: + + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::real_type real_type; + + vector3_type m_coord; ///< Default Coordinate of tetramesh node. + }; + + class DefaultTetrahedronTraits { }; + + class DefaultT4EdgeTraits { }; + + class DefaultT4FaceTraits { }; + + } // namespace t4mesh +} // namespace OpenTissue + +//OPENTISSUE_CORE_CONTAINERS_T4MESH_DEFAULT_TRAITS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4boundary_faces.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4boundary_faces.h new file mode 100644 index 000000000..dee6b33d3 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4boundary_faces.h @@ -0,0 +1,153 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_T4MESH_T4BOUNDARY_FACES_H +#define OPENTISSUE_CORE_CONTAINERS_T4MESH_T4BOUNDARY_FACES_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +namespace OpenTissue +{ + namespace t4mesh + { + + /** + * t4mesh Face. + */ + template + class T4Face : public F + { + public: + + typedef M mesh_type; + typedef F face_traits; + typedef T4Face face_type; + typedef typename mesh_type::index_type index_type; + + protected: + + index_type m_idx0; ///< First node index. + index_type m_idx1; ///< Second node index. + index_type m_idx2; ///< Third node index. + + public: + + T4Face() + : m_idx0(-1) + , m_idx1(-1) + , m_idx2(-1) + {} + + T4Face(index_type const & index0, index_type const & index1, index_type const & index2) + : m_idx0(index0) + , m_idx1(index1) + , m_idx2(index2) + {} + + public: + + const index_type & idx0() const { return m_idx0; } + const index_type & idx1() const { return m_idx1; } + const index_type & idx2() const { return m_idx2; } + + }; + + /** + * Tetrahedra Mesh Boundary Faces. + * Note that if subsequent changes are made to the t4mesh are not reflected in + * the faces stored in this class. Meaning that a new face-queury + * must be initiated everytime the t4mesh changes its topology. + * + * Example of usage: + * + * typedef t4mesh<...> MyMeshType; + * MyMeshType mymesh; + * ... + * class MyFaceTraits : public DefaultFaceTraits + * { + * public: + * Color m_color; + * ... + * }; + * typedef T4BoundaryFaces MyBoundaryFaces; + * MyBoundaryFaces bounday(mymesh); + * for(MyBoundaryFaces::face_iterator face=boundary.begin();face!=boundary.end();++face) + * { + * face->m_color = Color::white; + * std::cout << face->idx0() << std::endl; + * } + */ + template + class T4BoundaryFaces + { + public: + + typedef M mesh_type; + typedef F face_traits; + typedef T4Face face_type; + typedef std::list face_container; + typedef typename face_container::iterator face_iterator; + typedef typename face_container::const_iterator const_face_iterator; + + protected: + + face_container m_faces; ///< Container of extrated boundary faces. + + public: + + face_iterator begin() { return m_faces.begin(); } + face_iterator end() { return m_faces.end(); } + const_face_iterator begin() const { return m_faces.begin(); } + const_face_iterator end() const { return m_faces.end(); } + + public: + + /** + * Default Constructor. + * Constructs an empty set of boundary faces. + */ + T4BoundaryFaces() + : m_faces() + {} + + /** + * Specialezed Constructor + * Traverses the tetrahedral mesh, and extracts all boundary faces. A boundary + * face is a face that only have one neighboring tetrahedron. A face inside + * the tetrahedral mesh will have exactly two neighboring tetrahedra. + * + * Face node indices are given in CCW order. + * + * @param mesh The tetrahedral mesh from which boundary + * faces are extracted. + */ + T4BoundaryFaces(mesh_type & mesh) + : m_faces() + { + typename mesh_type::tetrahedron_iterator tetrahedron; + for( tetrahedron = mesh.tetrahedron_begin(); tetrahedron != mesh.tetrahedron_end(); ++tetrahedron) + { + if(tetrahedron->jkm()==mesh.tetrahedron_end()) + m_faces.push_back(face_type(tetrahedron->j()->idx(),tetrahedron->k()->idx(),tetrahedron->m()->idx())); + if(tetrahedron->ijm()==mesh.tetrahedron_end()) + m_faces.push_back(face_type(tetrahedron->i()->idx(),tetrahedron->j()->idx(),tetrahedron->m()->idx())); + if(tetrahedron->kim()==mesh.tetrahedron_end()) + m_faces.push_back(face_type(tetrahedron->k()->idx(),tetrahedron->i()->idx(),tetrahedron->m()->idx())); + if(tetrahedron->ikj()==mesh.tetrahedron_end()) + m_faces.push_back(face_type(tetrahedron->i()->idx(),tetrahedron->k()->idx(),tetrahedron->j()->idx())); + } + } + + }; + + } // namespace t4mesh +} // namespace OpenTissue + +//OPENTISSUE_CORE_CONTAINERS_T4MESH_T4BOUNDARY_FACES_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4edges.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4edges.h new file mode 100644 index 000000000..caed6272a --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4edges.h @@ -0,0 +1,219 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_T4MESH_T4EDGES_H +#define OPENTISSUE_CORE_CONTAINERS_T4MESH_T4EDGES_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +namespace OpenTissue +{ + namespace t4mesh + { + + /** + * t4mesh Edge. + * An edge is uniquely defined by the indices of its two end nodes. + * The order of indices do not matter. + */ + template + class T4Edge : public E + { + public: + + typedef M mesh_type; + typedef E edge_traits; + typedef T4Edge edge_type; + typedef typename mesh_type::index_type index_type; + + protected: + + index_type m_idxA; ///< Index to first node. + index_type m_idxB; ///< Index to second node. + + public: + + T4Edge() + : m_idxA(-1) + , m_idxB(-1) + {} + + T4Edge(index_type const & indexA, index_type const & indexB) + : m_idxA(indexA) + , m_idxB(indexB) + {} + + public: + + index_type const & idxA() const { return m_idxA; } + index_type const & idxB() const { return m_idxB; } + + bool operator==(edge_type const & edge) const + { + if(m_idxA==edge.idxA() && m_idxB==edge.idxB()) + return true; + if(m_idxB==edge.idxA() && m_idxA==edge.idxB()) + return true; + return false; + } + + bool operator!=(edge_type const & edge)const{ return !((*this)==edge); } + + }; + + /** + * Tetrahedra Mesh Edges. + * Edges are not represented explicitly in a t4mesh, only nodes and tetrahedra + * are representated. Thus this class extracts all unique edges from a t4mesh, by + * traversing it and generating explicit edges. + * + * Note that if subsequent changes are made to the t4mesh, the edge changes are + * not reflected by the edges stored in this class. Meaning that a new edge-queury + * must be initiated everytime the t4mesh changes its topology. + * + */ + template + class T4Edges + { + public: + + typedef M mesh_type; + typedef E edge_traits; + typedef T4Edge edge_type; + typedef typename mesh_type::index_type index_type; + typedef std::list edge_container; + typedef typename edge_container::iterator edge_iterator; + typedef typename edge_container::const_iterator const_edge_iterator; + + protected: + + typedef enum{white,grey,black} color_type; + typedef std::vector color_container; + typedef typename mesh_type::node_type node_type; + typedef typename node_type::tetrahedron_circulator tetrahedron_type; + typedef std::list work_queue; + + protected: + + edge_container m_edges; + + public: + + edge_iterator begin() { return m_edges.begin(); } + edge_iterator end() { return m_edges.end(); } + const_edge_iterator begin() const { return m_edges.begin(); } + const_edge_iterator end() const { return m_edges.end(); } + + + protected: + + /** + * Internally used functor, needed for visiting neighboring + * nodes and extracting un-seen edges. + */ + struct VisitT4Edge + { + void visit( + index_type & idxA + , index_type & idxB + , work_queue & work + , color_container & colors + , edge_container & edges + , mesh_type & mesh + ) + { + if(idxA==idxB)//--- self-loop, ignore it + return; + + //std::cout << "\tvisting:" <swap(where,last); + } + + this->unlink(last); + //--- This might be a bit stupid, it would + //--- proberly be better to keep track of last unused + //--- entry and only resize vector when there is no + //--- more space + m_tetrahedra.pop_back(); + return where; + } + + protected: + + protected: + + + }; + + } // namespace detail + } // namespace t4mesh +} // namespace OpenTissue + +//OPENTISSUE_CORE_CONTAINERS_T4MESH_T4MESH_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4node.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4node.h new file mode 100644 index 000000000..7ca712985 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4node.h @@ -0,0 +1,138 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_T4MESH_T4NODE_H +#define OPENTISSUE_CORE_CONTAINERS_T4MESH_T4NODE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +namespace OpenTissue +{ + namespace t4mesh + { + + template< typename mesh_type_> + class T4Node : public mesh_type_::node_traits + { + public: + + typedef mesh_type_ mesh_type; + typedef typename mesh_type::node_type node_type; + typedef typename mesh_type::tetrahedron_type tetrahedron_type; + typedef typename mesh_type::index_type index_type; + typedef std::list tetrahedra_index_container; + typedef typename tetrahedra_index_container::iterator tetrahedra_idx_iterator; + + protected: + + index_type m_idx; ///< Global index of node + mesh_type * m_owner; ///< Pointer to mesh which the node belongs to. + tetrahedra_index_container m_tetrahedra; ///< Indices of tetrahedra this node is part of. + + private: + + friend class t4mesh_core_access; + + void set_index(index_type idx) { m_idx = idx; } + void set_owner(mesh_type * owner) { m_owner = owner; } + void tetrahedra_push_back(index_type idx) { m_tetrahedra.push_back(idx); } + void tetrahedra_remove(index_type idx) { m_tetrahedra.remove(idx); } + + public: + + class tetrahedron_circulator + { + private: + + node_type * m_node; ///< A pointer to the node + tetrahedra_idx_iterator m_it; ///< An ``local'' iterator to the tetrahedron index, indicating current tetrahedron of this iterator. + + public: + + tetrahedron_circulator() + : m_node( 0 ) + , m_it( 0 ) + {} + + tetrahedron_circulator( node_type * node, tetrahedra_idx_iterator pos) + : m_node( node ) + , m_it( pos ) + { + assert(m_node || !"tetrahedron_circulator() : node was null"); + assert(m_node->m_owner || !"tetrahedron_circulator(..) : owner was null"); + } + + bool operator== ( tetrahedron_circulator const & other ) const + { + return ( m_node == other.m_node && m_it == other.m_it ); + } + + bool operator!= ( tetrahedron_circulator const & other ) const + { + return !( *this == other); + } + + tetrahedron_type & operator*() + { + assert(m_node || !"tetrahedron_circulator::* : node was null"); + assert(m_node->m_owner || !"tetrahedron_circulator::* : owner was null"); + return *(m_node->m_owner->tetrahedron(*m_it)); + } + + tetrahedron_type * operator->() + { + assert(m_node || !"tetrahedron_circulator::-> : node was null"); + assert(m_node->m_owner || !"tetrahedron_circulator::-> : owner was null"); + return &(*(m_node->m_owner->tetrahedron(*m_it))); + } + + tetrahedron_circulator & operator++() + { + assert(m_node || !"tetrahedron_circulator::++ : node was null"); + assert(m_node->m_owner || !"tetrahedron_circulator::++ : owner was null"); + ++m_it; + return *this; + } + + }; + + tetrahedron_circulator begin() { return tetrahedron_circulator( this, m_tetrahedra.begin() ); } + tetrahedron_circulator end() { return tetrahedron_circulator( this, m_tetrahedra.end() ); } + + /** + * Get Number of Tetrahedra. + * + * @return The number of tetrahedra that this node is part of. If + * zero it means the node is an isolated node. + */ + size_t size_tetrahedra() const { return m_tetrahedra.size(); } + + bool isolated() const { return m_tetrahedra.empty(); } + + public: + + T4Node() + : m_idx( mesh_type::undefined() ) + , m_owner(0) + , m_tetrahedra() + {} + + public: + + index_type idx() const { return m_idx; } + mesh_type * owner() { return m_owner; } + mesh_type const * owner() const { return m_owner; } + + }; + + } // namespace t4mesh +} // namespace OpenTissue + +//OPENTISSUE_CORE_CONTAINERS_T4MESH_T4NODE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4tetrahedron.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4tetrahedron.h new file mode 100644 index 000000000..8f81b3d9e --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/t4mesh_t4tetrahedron.h @@ -0,0 +1,105 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_T4MESH_T4TETRAHEDRON_H +#define OPENTISSUE_CORE_CONTAINERS_T4MESH_T4TETRAHEDRON_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace t4mesh + { + + template< typename mesh_type_> + class T4Tetrahedron : public mesh_type_::tetrahedron_traits + { + public: + typedef mesh_type_ mesh_type; + typedef typename mesh_type::node_type node_type; + typedef typename mesh_type::tetrahedron_type tetrahedron_type; + typedef typename mesh_type::index_type index_type; + + index_type m_idx; ///< Global index of tetrahedron + mesh_type * m_owner; ///< Pointer to mesh which the node belongs to. + index_type m_nodes[4]; ///< Global index of node i,j,k and m + + private: + + friend class t4mesh_core_access; + + void set_index(index_type idx) { m_idx = idx; } + void set_owner(mesh_type * owner) { m_owner = owner; } + void set_node0(index_type idx) { m_nodes[0] = idx; } + void set_node1(index_type idx) { m_nodes[1] = idx; } + void set_node2(index_type idx) { m_nodes[2] = idx; } + void set_node3(index_type idx) { m_nodes[3] = idx; } + + public: +#ifdef TODO_ERWIN + T4Tetrahedron() + : m_idx( mesh_type::undefined() ) + , m_owner(0) + { + this->m_nodes.assign( mesh_type::undefined() ); + } +#endif + + public: + + node_type* node(index_type idx) + { + return &m_owner->m_nodes[m_nodes[idx]]; + } + + const node_type* node(index_type idx) const + { + return &m_owner->m_nodes[m_nodes[idx]]; + } + + node_type* i() + { + return node(0); + } + node_type* j() + { + return node(1); + } + node_type* k() + { + return node(2); + } + node_type* m() + { + return node(3); + } + + + index_type idx() const { return m_idx; } + + index_type node_idx(index_type const & local_idx) const + { + assert(0<=local_idx); + assert(local_idx<=3); + + return m_nodes[local_idx]; + } + + + mesh_type * owner() { return m_owner; } + mesh_type const * owner() const { return m_owner; } + + + + }; + + } // namespace t4mesh +} // namespace OpenTissue + +//OPENTISSUE_CORE_CONTAINERS_T4MESH_T4TETRAHEDRON_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_block_generator.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_block_generator.h new file mode 100644 index 000000000..f5c28ab39 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_block_generator.h @@ -0,0 +1,114 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_T4MESH_UTIL_T4MESH_BLOCK_GENERATOR_H +#define OPENTISSUE_CORE_CONTAINERS_T4MESH_UTIL_T4MESH_BLOCK_GENERATOR_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + + +namespace OpenTissue +{ + namespace t4mesh + { + + + /** + * t4mesh Block Generator. + * + * @param I The number of blocks along x axis. + * @param J The number of blocks along y axis. + * @param K The number of blocks along z axis. + * @param block_width The edgelength of the blocks along x-axis. + * @param block_height The edgelength of the blocks along x-axis. + * @param block_depth The edgelength of the blocks along x-axis. + * @param mesh A generic t4mesh, which upon return holds the generated mesh. + */ + template < typename real_type, typename t4mesh_type > + void generate_blocks( + unsigned int const & I + , unsigned int const & J + , unsigned int const & K + , real_type const & block_width + , real_type const & block_height + , real_type const & block_depth + , t4mesh_type & mesh + ) + { + + mesh.clear(); + + unsigned int numVertices = (I + 1) * (J + 1) * (K + 1); + for (unsigned int i=0; i + + +#include + +namespace OpenTissue +{ + + namespace t4mesh + { + + namespace mesh_coupling + { + + template + class collision_policy + { + public: + + typedef typename surface_mesh::vertex_type vertex_type; + typedef typename volume_mesh::tetrahedron_type tetrahedron_type; + + typedef double real_type; + typedef math::Vector3 point_type; + + typedef vertex_type* data_type; + typedef tetrahedron_type query_type; + + typedef OpenTissue::spatial_hashing::GridHashFunction hash_function; + typedef OpenTissue::spatial_hashing::Grid< point_type, math::Vector3, data_type, hash_function> hash_grid; + + class result_type + { + public: + vertex_type * m_data; + tetrahedron_type * m_query; + real_type m_w0; + real_type m_w1; + real_type m_w2; + real_type m_w3; + }; + + typedef std::list result_container; + + public: + + point_type position(data_type const & data) const + { + return data->m_coord; + } + + point_type min_coord(query_type const & query) const + { + using std::min; + + point_type & p0 = query.i()->m_coord; + point_type & p1 = query.j()->m_coord; + point_type & p2 = query.k()->m_coord; + point_type & p3 = query.m()->m_coord; + return min( p0, min( p1 , min( p2, p3) ) ); + } + + point_type max_coord(query_type const & query) const + { + using std::max; + + point_type & p0 = query.i()->m_coord; + point_type & p1 = query.j()->m_coord; + point_type & p2 = query.k()->m_coord; + point_type & p3 = query.m()->m_coord; + return max( p0, max( p1 , max( p2, p3) ) ); + } + + void reset(result_container & results) { results.clear(); }; + void report(data_type const & data, query_type const & query,result_container & results) + { + //--- First we do a quick rejection test. If the vertex is allready reported then simply ignore it!!! + if(data->m_tag == 1) + return; + + point_type & pi = query.i()->m_coord; + point_type & pj = query.j()->m_coord; + point_type & pk = query.k()->m_coord; + point_type & pm = query.m()->m_coord; + point_type & p = data->m_coord; + + real_type delta = 10e-5; + real_type lower = - delta; + real_type upper = 1.+ delta; + result_type result; + OpenTissue::geometry::barycentric_algebraic(pi,pj,pk,pm,p,result.m_w0,result.m_w1,result.m_w2,result.m_w3); + if( + (result.m_w0>lower)&&(result.m_w0lower)&&(result.m_w1lower)&&(result.m_w2lower)&&(result.m_w3m_tag = 1; + result.m_data = const_cast( data ); + result.m_query = const_cast( &query ); + results.push_back( result ); + return; + } + } + }; + + /** + * Bind Mesh Surface to t4mesh. + * + * + * @param M A surface mesh that should be bound to a volume mesh. + * @param V A T4Mesh it is implicitly assumed that node traits have a + * real_type and vector3_type (these are defined in the + * default node traits). + */ + template + void bind_surface (surface_mesh & M,volume_mesh & V,point_container & barycentric,index_container & bind_indices) + { + + //--- + //--- The main idea behind mesh coupling is to create a multiresolution for the + //--- animataion. The idea is to separate the visual geometry from the geometry + //--- used to compute the dynamics. This allows one to use highly detailed visual + //--- representation of objects, while using a computational low cost coarse volume + //--- mesh for the computing the dynamcis. + //--- + //--- A technique for doing this is callled mesh coupling or cartoon meshing. Below + //--- we describe how it is used together with tetrahedral meshes. It is however a + //--- general approach and can be used with other types of geometries, FFD lattices + //--- are probably anohter very common example on mesh coupling. + //--- + //--- When using Mesh Coulping the first step is to bind the vertices of the surface + //--- mesh to the tetrahedral elements of the volume mesh. + //--- + //--- Here we use a spatial hashing algorithm to find vertex tetrahedron pairs, where + //--- the vertex is embedded inside the tetrahedron. The actual test is done by first + //--- computing the barycentric coordinates of the vertex with respect to a tetrahedron + //--- in question. + //--- + //--- If each barycentric coordinate is greather than equal to zero and less than equal + //--- to one then the vertex is embedded in the tetrahedron. In pratice we need to apply + //--- treshold testing to counter numerical precision problems. It may therefor happen + //--- that vertices lying close to a face of a tetrahedron gets reported twice. Once of + //--- the tetrahedron embedding it and once for the neighboring tetetrahedron. The same + //--- happens if the vertex lies exactly on a face. + //--- + //--- Therefore, we firstdo a quick rejection test. If the vertex is allready reported + //--- then simply ignore it!!! + //--- + //--- Before rendering each frame, we must update the vertex positions to reflect the + //--- underlying deformation of the tetrahedral mesh. This is done using the barycentric + //--- coordinates, such that the new vertex position is given by + //--- + //--- c = w0 p0 + w1 p1 + w2 p2 + w3 p3 + //--- + //--- Where p0, p1 ,p2, and p3 are the nodal coordinates of the tetrahedron which + //--- the vertex was bounded to. + //--- + //--- If stiffness warping is used, the element rotation, Re, can be + //--- used to update the undeformed vertex normal, n0, into the deformed + //--- vertex normal, n, by, + //--- + //--- n = Re n0 + //--- + //--- Often a tetrahedra mesh is used with a conservative coverage of the surface mesh. + //--- That means one is guaranteed that all vertices of the surface mesh are embedded + //--- inside one unique tetrahedron. However, mesh coupling can be used in cases where + //--- one only have a partial coverage. The solution is to bind a vertex to the + //--- closest tetrahedron. Eventhough the vertex lies outside the tetrahedra mesh, the + //--- barycentric coordinates extend the deformation of the tetrahedra mesh beyond + //--- its surface. + //--- + typedef collision_policy policy; + typedef OpenTissue::spatial_hashing::PointDataQuery point_query_type; + typename policy::result_container results; + point_query_type point_query; + point_query.auto_init_settings(V.tetrahedron_begin(),V.tetrahedron_end()); + //--- perform query + mesh::clear_vertex_tags(M); + + std::vector vertex_ptr_container(M.size_vertices()); + unsigned int i = 0; + for(typename surface_mesh::vertex_iterator v= M.vertex_begin();v!=M.vertex_end();++v,++i) + { + vertex_ptr_container[i] = &(*v); + } + + point_query( + vertex_ptr_container.begin() + , vertex_ptr_container.end() + , V.tetrahedron_begin() + , V.tetrahedron_end() + , results + , typename point_query_type::all_tag() + ); + + unsigned int size = static_cast( results.size() ); + barycentric.resize(size); + bind_indices.resize(size); + typename policy::result_container::iterator Rbegin = results.begin(); + typename policy::result_container::iterator Rend = results.end(); + for(typename policy::result_container::iterator R = Rbegin;R!=Rend;++R) + { + unsigned int i = static_cast( R->m_data->get_handle().get_idx() ); + + barycentric[i](0) = R->m_w1; + barycentric[i](1) = R->m_w2; + barycentric[i](2) = R->m_w3; + bind_indices[i] = R->m_query->idx(); + } + }; + + /** + * Update Surface Mesh. + * + * + * @param M A surface mesh that is bound to a volume mesh. + * @param V The volume mesh that the surface is bound to. It + * is implicitly assumed that the node traits define + * a real_type and vector3_type ((these are defined in + * the default node traits). + * @param barycentric + * @param bind_info + */ + template + void update_surface ( + surface_mesh & M, + volume_mesh & V, + point_container const & barycentric, + index_container const & bind_info + ) + { + typename surface_mesh::vertex_iterator begin = M.vertex_begin(); + typename surface_mesh::vertex_iterator end = M.vertex_end(); + + typedef typename volume_mesh::tetrahedron_iterator tetrahedron_iterator; + typedef typename volume_mesh::node_type node_type; + typedef typename node_type::real_type real_type; + typedef typename node_type::vector3_type vector3_type; + + for( typename surface_mesh::vertex_iterator v = begin; v!=end; ++v) + { + unsigned int i = static_cast( v->get_handle().get_idx() ); + tetrahedron_iterator T = V.tetrahedron( bind_info[i] ); + + real_type w1 = barycentric[i](0); + real_type w2 = barycentric[i](1); + real_type w3 = barycentric[i](2); + real_type w0 = 1 - w1 - w2 - w3; + + vector3_type & p0 = T->i()->m_coord; + vector3_type & p1 = T->j()->m_coord; + vector3_type & p2 = T->k()->m_coord; + vector3_type & p3 = T->m()->m_coord; + v->m_coord = p0*w0 + p1*w1 + p2*w2 + p3*w3; + + } + }; + + } // namespace mesh_coupling + + } // namespace t4mesh + +} // namespace OpenTissue + +//OPENTISSUE_CORE_CONTAINERS_T4MESH_UTIL_T4MESH_MESH_COUPLING_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_remove_redundant_nodes.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_remove_redundant_nodes.h new file mode 100644 index 000000000..e69de29bb diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_tetgen_constrained_delaunay_tetrahedralization.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_tetgen_constrained_delaunay_tetrahedralization.h new file mode 100644 index 000000000..681840702 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_tetgen_constrained_delaunay_tetrahedralization.h @@ -0,0 +1,48 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_T4MESH_UTIL_T4MESH_TETGEN_CONSTRAINED_DELAUNAY_TRIANGULATION_H +#define OPENTISSUE_CORE_CONTAINERS_T4MESH_UTIL_T4MESH_TETGEN_CONSTRAINED_DELAUNAY_TRIANGULATION_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace t4mesh + { + namespace tetgen + { + + /** + * Constrained Delaunay Tetrahedralization Routine. + * - uses TetGen to create the resulting tetrahedal mesh + * + * @param polymesh A poly mesh, which holds a closed two-manifold. + * @param t4mesh A generic t4mesh, which upon return holds the generated tetrahedal mesh. + * + * @return if succesfull then the return value is true otherwise it is false. + */ + template + inline bool constrained_delaunay_tetrahedralization(const polymesh_type& polymesh, t4mesh_type & t4mesh) + { + OpenTissue::t4mesh::mesh_lofter_settings config; + + config.m_intermediate_file = "tmp"; + config.m_quality_ratio = 0.0; + config.m_maximum_volume = 0.0; + config.m_quiet_output = true; + + return OpenTissue::t4mesh::mesh_lofter(t4mesh, polymesh, config); + } + + } // namespace tetgen + } // namespace t4mesh +} // namespace OpenTissue + +//OPENTISSUE_CORE_CONTAINERS_T4MESH_UTIL_T4MESH_TETGEN_CONSTRAINED_DELAUNAY_TRIANGULATION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_tetgen_mesh_lofter.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_tetgen_mesh_lofter.h new file mode 100644 index 000000000..36bc1a5ae --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_tetgen_mesh_lofter.h @@ -0,0 +1,128 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_T4MESH_UTIL_T4MESH_TETGEN_MESH_LOFTER_H +#define OPENTISSUE_CORE_CONTAINERS_T4MESH_UTIL_T4MESH_TETGEN_MESH_LOFTER_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + + +namespace OpenTissue +{ + namespace t4mesh + { + + struct mesh_lofter_settings + { + mesh_lofter_settings() + : m_quality_ratio(2./*std::sqrt(2.)*/) + , m_maximum_volume(0.) + , m_intermediate_file("") + , m_quiet_output(false) + , m_verify_input(false) + {} + double m_quality_ratio; ///< quality t4mesh is issued if > 0. A minimum radius-edge ratio may be specifyed (default 2.0). + double m_maximum_volume; ///< max volume constraints on t4mesh if > 0. + std::string m_intermediate_file; ///< use intermediate files to/fro tetget if name specified. + bool m_quiet_output; ///< keep output spam as silent as possible, great for RELEASE. + bool m_verify_input; ///< DEBUG: detects plc intersections, i.e. verify "bad" input mesh. + }; + + + /** + * t4mesh_mesh_lofter. + * - uses tetgen to loft/extrude a closed two-manifold polymesh to a generic tetrahedal mesh + * + * @param polymesh A poly mesh, which holds a closed two-manifold. + * @param t4mesh A generic t4mesh, which upon return holds the generated tetrahedal mesh. + * @param settings The settings/configuration for TetGen, can be omitted to use the default settings. + * + */ + // TODO: Implement work-in-mem solution, thus avoid using intermediate files [MKC] + template + bool mesh_lofter(t4mesh_type& t4mesh, const polymesh_type& polymesh, const mesh_lofter_settings & settings = mesh_lofter_settings()) + { + // local functions are no go in C++, thus we need to wrap them in a local class, tsk! + class local_aux + { + public: + static bool error(const std::string& text) + { + using std::operator<<; + std::cerr << "ERROR [t4mesh_mesh_lofter]:\n- " << text << std::endl; + return false; + } + }; + + // convenient pointer to use with TetGen + char* tmp_file = settings.m_intermediate_file.size()>0?const_cast(settings.m_intermediate_file.c_str()):NULL; + + // "no tmp file" won't work check + if (!tmp_file) + return local_aux::error("current version only supports using intermediate files"); + + if (!is_manifold(polymesh)) + return local_aux::error("polymesh is not a two-manifold"); + + if (tmp_file) + if (!tetgen_write(settings.m_intermediate_file+".poly", polymesh)) + return local_aux::error("mesh_tetgen_write failed in writing the polymesh file: '"+settings.m_intermediate_file+".poly'"); + + std::stringstream tmp; + tmp << "p"; // piecewise linear complex (always) + if (settings.m_verify_input) + tmp << "d"; + else { + if (settings.m_quality_ratio > 0) + tmp << "q" << settings.m_quality_ratio; // quality + if (settings.m_maximum_volume > 0) + tmp << "a" << settings.m_maximum_volume; // max volume + if (settings.m_quiet_output) + tmp << "Q"; // keep quiet :) + } + std::string txt = tmp.str().c_str(); + tetgenbehavior config; + // MKC: setting vars in tetgenbehavior explicitly is too complicated, and requires a deeper knowledge of the system :( + if (!config.parse_commandline(const_cast(txt.c_str()))) // parsing a "cmd" line will set the correct vars and their dependecies. + return local_aux::error("TetGen parse_commandline failed: '"+txt+"'"); + + tetgenio in, out; + if (tmp_file) + if (!in.load_poly(tmp_file)) + return local_aux::error("TetGen load_poly failed: '"+settings.m_intermediate_file+"'"); + + // let TetGen do some magic :) + tetrahedralize(&config, &in, &out); + + if (out.numberoftetrahedra < 1) + return local_aux::error("TetGen tetrahedralize failed: no tetrahedra generated!"); + + if (tmp_file) { + out.save_elements(tmp_file); + out.save_nodes(tmp_file); + + if (!tetgen_read(settings.m_intermediate_file, t4mesh)) + return local_aux::error("t4mesh_tetgen_read failed in reading the data: '"+settings.m_intermediate_file+"'"); + } + + return true; + } + + } // namespace t4mesh +} // namespace OpenTissue + +//OPENTISSUE_CORE_CONTAINERS_T4MESH_UTIL_T4MESH_TETGEN_MESH_LOFTER_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_tetgen_quality_tetrahedralization.h b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_tetgen_quality_tetrahedralization.h new file mode 100644 index 000000000..5f05ab450 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/containers/t4mesh/util/t4mesh_tetgen_quality_tetrahedralization.h @@ -0,0 +1,48 @@ +#ifndef OPENTISSUE_CORE_CONTAINERS_T4MESH_UTIL_T4MESH_TETGEN_QUALITY_TETRAHEDRALIZATION_H +#define OPENTISSUE_CORE_CONTAINERS_T4MESH_UTIL_T4MESH_TETGEN_QUALITY_TETRAHEDRALIZATION_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace t4mesh + { + namespace tetgen + { + + /** + * Quality Tetrahedralization Routine. + * - uses TetGen to create the resulting tetrahedal mesh + * + * @param polymesh A poly mesh, which holds a closed two-manifold. + * @param t4mesh A generic t4mesh, which upon return holds the generated tetrahedal mesh. + * + * @return if succesfull then the return value is true otherwise it is false. + */ + template + inline bool quality_tetrahedralization(const polymesh_type& polymesh, t4mesh_type & t4mesh) + { + OpenTissue::t4mesh::mesh_lofter_settings config; + + config.m_intermediate_file = "tmp"; + config.m_quality_ratio = 2.0; + config.m_maximum_volume = 0.0; + config.m_quiet_output = true; + + return OpenTissue::t4mesh::mesh_lofter(t4mesh, polymesh, config); + } + + } // namespace tetgen + } // namespace t4mesh +} // namespace OpenTissue + +//OPENTISSUE_CORE_CONTAINERS_T4MESH_UTIL_T4MESH_TETGEN_QUALITY_TETRAHEDRALIZATION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_aabb.h b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_aabb.h new file mode 100644 index 000000000..77a042fe4 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_aabb.h @@ -0,0 +1,368 @@ +#ifndef OPENTISSUE_CORE_GEOMETRY_GEOMETRY_AABB_H +#define OPENTISSUE_CORE_GEOMETRY_GEOMETRY_AABB_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +#include +#include + + +namespace OpenTissue +{ + + namespace geometry + { + + /** + * Axed Aligned Bounding Box (AABB). + */ + template< typename math_types_ > + class AABB + : public VolumeShape< math_types_ > + , public OpenTissue::utility::ClassID< AABB > + { + public: + + typedef math_types_ math_types; + typedef typename math_types::value_traits value_traits; + typedef typename math_types::real_type real_type; + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::matrix3x3_type matrix3x3_type; + typedef typename math_types::quaternion_type quaternion_type; + + public: + + vector3_type m_min; ///< Coordinates of minimum corner. + vector3_type m_max; ///< Coordinates of maximum corner. + + public: + + size_t const class_id() const { return OpenTissue::utility::ClassID >::class_id(); } + + AABB() + { + m_min.clear(); + m_max.clear(); + } + + explicit AABB( + real_type const & xmin + , real_type const & ymin + , real_type const & zmin + , real_type const & xmax + , real_type const & ymax + , real_type const & zmax + ) + { + set(xmin,ymin,zmin,xmax,ymax,zmax); + } + + explicit AABB( vector3_type const & pmin_v, vector3_type const & pmax_v) { set(pmin_v,pmax_v); } + + virtual ~AABB() {} + + public: + + void compute_surface_points(std::vector & points) const + { + vector3_type dia; + dia = m_max - m_min; + + vector3_type p000 = m_min; + vector3_type p001 = m_min + vector3_type( dia[0], value_traits::zero(), value_traits::zero()); + vector3_type p010 = m_min + vector3_type(value_traits::zero(), dia[1], value_traits::zero()); + vector3_type p011 = m_min + vector3_type( dia[0], dia[1], value_traits::zero()); + vector3_type p100 = m_min + vector3_type(value_traits::zero(), value_traits::zero(), dia[2]); + vector3_type p101 = m_min + vector3_type( dia[0], value_traits::zero(), dia[2]); + vector3_type p110 = m_min + vector3_type(value_traits::zero(), dia[1], dia[2]); + vector3_type p111 = m_min + vector3_type( dia[0], dia[1], dia[2]); + + points.push_back(p000); + points.push_back(p001); + points.push_back(p010); + points.push_back(p011); + points.push_back(p100); + points.push_back(p101); + points.push_back(p110); + points.push_back(p111); + } + + vector3_type center() const { return vector3_type( (m_max+m_min)/value_traits::two() ); } + + public: + /** + * Assingment Method. + * Assigns the size of the specified AABB to this AABB. + * + * @param other_aabb Another AABB + */ + void set(AABB const & other_aabb) + { + this->m_min = other_aabb.m_min; + this->m_max = other_aabb.m_max; + } + + void set( vector3_type const & pmin_v, vector3_type const & pmax_v) + { + assert(pmin_v[0] <= pmax_v[0] || !"AABB.set(): minimum must be less than or equal to maximum"); + assert(pmin_v[1] <= pmax_v[1] || !"AABB.set(): minimum must be less than or equal to maximum"); + assert(pmin_v[2] <= pmax_v[2] || !"AABB.set(): minimum must be less than or equal to maximum"); + m_min = pmin_v; + m_max = pmax_v; + } + + /** + * Set AABB box. + * + * @param xmin + * @param ymin + * @param zmin + * @param xmax + * @param ymax + * @param zmax + */ + void set( + real_type const & xmin + , real_type const & ymin + , real_type const & zmin + , real_type const & xmax + , real_type const & ymax + , real_type const & zmax + ) + { + assert(xmin<=xmax || !"AABB.set(): minimum must be less than or equal to maximum"); + assert(ymin<=ymax || !"AABB.set(): minimum must be less than or equal to maximum"); + assert(zmin<=zmax || !"AABB.set(): minimum must be less than or equal to maximum"); + m_min = vector3_type(xmin,ymin,zmin); + m_max = vector3_type(xmax,ymax,zmax); + } + + /** + * Set AABB box. + * + * @param xmin + * @param ymin + * @param zmin + * @param width + * @param height + * @param depth + */ + void extent( + real_type const & xmin + , real_type const & ymin + , real_type const & zmin + , real_type const & width + , real_type const & height + , real_type const & depth + ) + { + assert(width>=0 || !"AABB.extent(): width must be non-negative"); + assert(height>=0 || !"AABB.extent(): height must be non-negative"); + assert(depth>=0 || !"AABB.extent(): depth must be non-negative"); + m_min = vector3_type(xmin,ymin,zmin); + m_max = vector3_type(xmin+width,ymin+height,zmin+depth); + } + + public: + + real_type const & x() const { return m_min[0]; } + real_type const & y() const { return m_min[1]; } + real_type const & z() const { return m_min[2]; } + real_type w() const { return m_max[0] - m_min[0]; } + real_type h() const { return m_max[1] - m_min[1]; } + real_type d() const { return m_max[2] - m_min[2]; } + + vector3_type & min() { return m_min; } + vector3_type & max() { return m_max; } + vector3_type const & min() const { return m_min; } + vector3_type const & max() const { return m_max; } + + /** + * Update Bounding Box + * This method updates the size of the AABB such that it encloses the + * given nodes. + * + * @param n0 + * @param n1 + * @param n2 + */ + void update(vector3_type const & n0,vector3_type const & n1, vector3_type const & n2) + { + using std::min; + using std::max; + + m_min[0] = min(n2[0],min(n1[0],n0[0])); + m_min[1] = min(n2[1],min(n1[1],n0[1])); + m_min[2] = min(n2[2],min(n1[2],n0[2])); + m_max[0] = max(n2[0],max(n1[0],n0[0])); + m_max[1] = max(n2[1],max(n1[1],n0[1])); + m_max[2] = max(n2[2],max(n1[2],n0[2])); + } + + /** + * Update Bounding Box + * This method updates the size of the AABB such that it encloses the + * given nodes. + * + * @param n0 + * @param n1 + * @param n2 + * @param n3 + */ + void update(vector3_type const & n0,vector3_type const & n1,vector3_type const & n2, vector3_type const & n3) + { + using std::min; + using std::max; + + m_min[0] = min(n3[0],min(n2[0],min(n1[0],n0[0]))); + m_min[1] = min(n3[1],min(n2[1],min(n1[1],n0[1]))); + m_min[2] = min(n3[2],min(n2[2],min(n1[2],n0[2]))); + m_max[0] = max(n3[0],max(n2[0],max(n1[0],n0[0]))); + m_max[1] = max(n3[1],max(n2[1],max(n1[1],n0[1]))); + m_max[2] = max(n3[2],max(n2[2],max(n1[2],n0[2]))); + } + + /** + * Update Box. + * This method updates the size of the AABB such that it encloses + * the given AABBs. + * + * @param A + * @param B + */ + void update(AABB const & A,AABB const & B) + { + using std::min; + using std::max; + + m_min[0] = min(A.m_min[0],B.m_min[0]); + m_min[1] = min(A.m_min[1],B.m_min[1]); + m_min[2] = min(A.m_min[2],B.m_min[2]); + m_max[0] = max(A.m_max[0],B.m_max[0]); + m_max[1] = max(A.m_max[1],B.m_max[1]); + m_max[2] = max(A.m_max[2],B.m_max[2]); + } + + /** + * Get Volume. + * + * @return The current value of the volume of the AABB. + */ + real_type volume() const + { + vector3_type vd = m_max-m_min; + return vd[0]*vd[1]*vd[2]; + } + + real_type area() const + { + vector3_type vd = m_max-m_min; + return 2*vd[0]*vd[1] + 2*vd[0]*vd[2] + 2*vd[2]*vd[1]; + } + + real_type diameter() const + { + vector3_type vd = m_max-m_min; + return length(vd); + } + + void translate(vector3_type const & T) + { + m_max += T; + m_min += T; + } + + void rotate(matrix3x3_type const & /*R*/) { } + + void scale(real_type const & s) + { + assert(s>=value_traits::zero() || !"AABB::scale: s must be non-negative"); + vector3_type c = center(); + m_min -= c; + m_max -= c; + m_min *= s; + m_max *= s; + m_min += c; + m_max += c; + } + + vector3_type get_support_point(vector3_type const & v) const + { + vector3_type dir = unit(v); + vector3_type ext = (m_max - m_min)/2.; + vector3_type c = center(); + vector3_type p(0,0,0); + + real_type sign = value_traits::zero(); + real_type tst = ext[0] * dir[0]; + if(tst>0) + sign = value_traits::one(); + else if(tst<0) + sign = -value_traits::one(); + else //(tst==0) + sign = value_traits::zero(); + p[0] += sign * ext[0]; + + tst = ext[1] * dir[1]; + if(tst>0) + sign = value_traits::one(); + else if(tst<0) + sign = -value_traits::one(); + else //(tst==0) + sign = value_traits::zero(); + p[1] += sign * ext[1]; + + tst = ext[2] * dir[2]; + if(tst>0) + sign = value_traits::one(); + else if(tst<0) + sign = -value_traits::one(); + else //(tst==0) + sign = value_traits::zero(); + p[2] += sign * ext[2]; + p += c; + return p; + } + + + + /** + * Compute Bounding Box. + * This method computes an axis aligned bounding + * box (AABB) that encloses the geometry. + * + * @param r The position of the model frame (i.e the coordinate frame the geometry lives in). + * @param R The orientation of the model frame (i.e the coordinate frame the geometry lives in). + * @param min_coord Upon return holds the minimum corner of the box. + * @param max_coord Upon return holds the maximum corner of the box. + * + */ + void compute_collision_aabb( + vector3_type const & r + , matrix3x3_type const & /*R*/ + , vector3_type & min_coord + , vector3_type & max_coord + ) const + { + min_coord = r + this->min(); + max_coord = r + this->max(); + } + + + }; + + } // namespace geometry + +} // namespace OpenTissue + +//OPENTISSUE_CORE_GEOMETRY_GEOMETRY_AABB_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_barycentric.h b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_barycentric.h new file mode 100644 index 000000000..b4717bba8 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_barycentric.h @@ -0,0 +1,394 @@ +#ifndef OPENTISSUE_CORE_GEOMETRY_BARYCENTRIC_H +#define OPENTISSUE_CORE_GEOMETRY_BARYCENTRIC_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include + +#include +#include + +namespace OpenTissue +{ + namespace geometry + { + + /* + * Compute Barycentric Coordinates. + * This method computes the barycentric coodinates for a point x3 of an edge + * given by the points x1 and x2. + * + * The barycentric coordinates w1 and w2 are defined such + * that x3' = w1*x1 + w2*x2, is the point on the line closest to x3. + * + * if 0 <= w1,w2 <= 1 then the point lies inside or on the perimeter of the triangle. + * + * @warning This method uses a geometric approach to compute the barycentric coordinates. + * + * @param x1 The first point of the edge. + * @param x2 The second point of the edge. + * @param x3 The point for which the barycentric coordinates should be computed. + * @param w1 Upon return this parameter contains the value of the first barycentric coordinate. + * @param w2 Upon return this parameter contains the value of the second barycentric coordinate. + */ + template + inline void barycentric_geometric( + V const & x1 + , V const & x2 + , V const & x3 + , typename V::value_type & w1 + , typename V::value_type & w2 + ) + { + using std::sqrt; + + typedef typename V::value_type T; + typedef typename V::value_traits value_traits; + + V const u = x2-x1; + T const uu = dot(u,u); + + assert( is_number(uu) || !"barycentric_geometric(): NaN encountered"); + assert( uu > value_traits::zero() || !"barycentric_geometric(): Degenerate edge encountered"); + + // Project x3 onto edge running from x1 to x2. + V const q = (dot(u, x3-x1)/ uu )*u + x1; + V const a = q - x2; + //V const b = q-x1; + T const aa = dot(a,a); + + assert( is_number(aa) || !"barycentric_geometric(): NaN encountered"); + + //T const bb = dot(b,b); + w1 = sqrt( aa / uu ); + w2 = value_traits::one() - w1; // sqrt( bb / uu ); + + assert( is_number(w1) || !"barycentric_geometric(): NaN encountered"); + assert( is_number(w2) || !"barycentric_geometric(): NaN encountered"); + + assert( w1 >= value_traits::zero() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w1 <= value_traits::one() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w2 >= value_traits::zero() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w2 <= value_traits::one() || !"barycentric_geometric(): Illegal coordinate encountered"); + } + + /* + * Compute Barycentric Coordinates. + * This method computes the barycentric coodinates for a point x4 of a triangle + * given by the points x1,x2, and x3 (in counter clockwise order). + * + * The barycentric coordinates w1,w2, and w3 are defined such + * that x4' = w1*x1 + w2*x2 + w3*x3, is the point in plane of the + * triangle closest to x4. + * + * if 0 <= w1,w2,w3 <= 1 then the point lies inside or on the perimeter of the triangle. + * + * @warning This method uses a geometric approach to compute the barycentric coordinates. + * + * @param x1 The first point of the triangle. + * @param x2 The second point of the triangle. + * @param x3 The third point of the triangle. + * @param x4 The point for which the barycentric coordinates should be computed. + * @param w1 Upon return this parameter contains the value of the first barycentric coordinate. + * @param w2 Upon return this parameter contains the value of the second barycentric coordinate. + * @param w3 Upon return this parameter contains the value of the third barycentric coordinate. + */ + template + inline void barycentric_geometric( + V const & x1 + , V const & x2 + , V const & x3 + , V const & x4 + , typename V::value_type & w1 + , typename V::value_type & w2 + , typename V::value_type & w3 + ) + { + using std::sqrt; + + typedef typename V::value_type T; + typedef typename V::value_traits value_traits; + + V const n = cross( x1-x3, x2-x3); + T const nn = dot(n,n); + + assert( is_number(nn) || !"barycentric_geometric(): NaN encountered"); + assert( nn > value_traits::zero() || !"barycentric_geometric(): Degenerate triangle encountered"); + + V const q = x4 - ( dot(n, x4 - x1)*n / nn ); + V const a = cross( x2-q, x3-q); + V const b = cross( x1-q, x3-q); + //V const c = cross( x1-q, x2-q); + + T const aa = dot(a,a); + T const bb = dot(b,b); + //T const cc = dot(c,c); + + assert( is_number(aa) || !"barycentric_geometric(): NaN encountered"); + assert( is_number(bb) || !"barycentric_geometric(): NaN encountered"); + //assert( is_number(cc) || !"barycentric_geometric(): NaN encountered"); + + w1 = sqrt( aa / nn ); + w2 = sqrt( bb / nn ); + w3 = value_traits::one() - w1 - w2; // sqrt( cc / nn ); + + assert( is_number(w1) || !"barycentric_geometric(): NaN encountered"); + assert( is_number(w2) || !"barycentric_geometric(): NaN encountered"); + assert( is_number(w3) || !"barycentric_geometric(): NaN encountered"); + + assert( w1 >= value_traits::zero() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w1 <= value_traits::one() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w2 >= value_traits::zero() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w2 <= value_traits::one() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w3 >= value_traits::zero() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w3 <= value_traits::one() || !"barycentric_geometric(): Illegal coordinate encountered"); + } + + /* + * Compute Barycentric Coordinates. + * This method computes the barycentric coodinates for a point p of a tetrahedron + * given by the points x1,x2,x3, x4 (in right-hand order). + * + * @warning This method uses a geometric approach to compute the barycentric coordinates. + * + * @param x1 The first point of the triangle. + * @param x2 The second point of the triangle. + * @param x3 The third point of the triangle. + * @param x4 The fourth point of the triangle. + * @param p The point for which the barycentric coordinates should be computed. + * @param w1 Upon return this parameter contains the value of the first barycentric coordinate. + * @param w2 Upon return this parameter contains the value of the second barycentric coordinate. + * @param w3 Upon return this parameter contains the value of the third barycentric coordinate. + * @param w4 Upon return this parameter contains the value of the fourth barycentric coordinate. + */ + template + inline void barycentric_geometric( + V const & x1 + , V const & x2 + , V const & x3 + , V const & x4 + , V const & p + , typename V::value_type & w1 + , typename V::value_type & w2 + , typename V::value_type & w3 + , typename V::value_type & w4 + ) + { + using std::fabs; + + typedef typename V::value_type T; + typedef typename V::value_traits value_traits; + + T const V6 = fabs( dot( (x1-x4), cross( x2-x4, x3-x4 ) ) ); + + assert( is_number(V6) || !"barycentric_geometric(): NaN encountered"); + assert( V6 > value_traits::zero() || !"barycentric_geometric(): Degenerate tetrahedron encountered"); + + w1 = fabs( dot( (x2-p), cross( (x3-p), (x4-p) ) ) ) / V6; + w2 = fabs( dot( (x1-p), cross( (x3-p), (x4-p) ) ) ) / V6; + w3 = fabs( dot( (x1-p), cross( (x2-p), (x4-p) ) ) ) / V6; + //w4 = fabs( dot( (x1-p), cross( (x2-p), (x3-p) ) ) ); + w4 = value_traits::one() - w1 -w2 - w3; + + assert( is_number(w1) || !"barycentric_geometric(): NaN encountered"); + assert( is_number(w2) || !"barycentric_geometric(): NaN encountered"); + assert( is_number(w3) || !"barycentric_geometric(): NaN encountered"); + assert( is_number(w4) || !"barycentric_geometric(): NaN encountered"); + + assert( w1 >= value_traits::zero() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w1 <= value_traits::one() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w2 >= value_traits::zero() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w2 <= value_traits::one() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w3 >= value_traits::zero() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w3 <= value_traits::one() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w4 >= value_traits::zero() || !"barycentric_geometric(): Illegal coordinate encountered"); + assert( w4 <= value_traits::one() || !"barycentric_geometric(): Illegal coordinate encountered"); + } + + /* + * Compute Barycentric Coordinates. + * This method computes the barycentric coodinates for a point p of a triangle + * given by the points x1,x2, and x3 (in counter clockwise order). + * + * The barycentric coordinates w1, w2, and w3 are defined such + * that p' = w1*x1 + w2*x2 + w3*x3, is the point in plane of the + * triangle closest to p. + * + * if 0 <= w1,w2,w3 <= 1 then the point lies inside or on the perimeter of the triangle. + * + * @warning This method uses a algebraic approach to compute the barycentric coordinates. + * + * @param x1 The first point of the triangle. + * @param x2 The second point of the triangle. + * @param x3 The third point of the triangle. + * @param p The point for which the barycentric coordinates should be computed. + * @param w1 Upon return this parameter contains the value of the first barycentric coordinate. + * @param w2 Upon return this parameter contains the value of the second barycentric coordinate. + * @param w3 Upon return this parameter contains the value of the third barycentric coordinate. + */ + template + inline void barycentric_algebraic( + V const & x1 + , V const & x2 + , V const & x3 + , V const & p + , typename V::value_type & w1 + , typename V::value_type & w2 + , typename V::value_type & w3 + ) + { + typedef typename V::value_type T; + typedef math::ValueTraits value_traits; + + //--- We compute barycentric coordinates, w1,w2,w3, of p, that is we solve the linear system + //--- + //--- |(x1-x3).(x1-x3) (x1-x3).(x2-x3)| |w1| |(x1-x3).(p-x3)| + //--- |(x1-x3).(x2-x3) (x2-x3).(x2-x3)| |w2| = |(x2-x3).(p-x3)| + //--- w1 + w2 + w3 = 1 + //--- + V x13 = x1-x3; + V x23 = x2-x3; + V x43 = p-x3; + + //--- + //--- We introduce the shorthand notation + //--- + //--- |a11 a12| |w1| |b1| + //--- |a12 a22| |w2| = |b2| + //--- + //--- Isolating w2 from the second equation yields + //--- + //--- w2 = (b2 - a12 w1)/a22 + //--- = b2/a22 - (a12/a22)w1 + //--- + //--- And substituting into the first gives + //--- + //--- a11 w1 + a12 ((b2 - a12 w1)/a22) = b1 + //--- a11 w1 + (a12/a22)*b2 - a12*(a12/a22)*w1 = b1 + //--- (a11 - a12*(a12/a22))*w1 = b1 - (a12/a22)*b2 + //--- + //--- Letting f = a12/a22 (to minimize computations by collecting common subterms) and + //--- + //--- m = (a11 - a12*f) + //--- n = b1 - f*b2 + //--- + //--- we have + //--- + //--- w1 = n/m + //--- w2 = b2/a22 - f*w1 + //--- w3 = 1 - w1 - w2 + //--- + //--- Since we always have a11>0 and a22>0 a solution will always exist + //--- regardless of the value of a12,b1 and b2 + //--- + + T a11 = dot(x13 , x13); + T a12 = dot(x13 , x23); + T a22 = dot(x23 , x23); + T b1 = dot(x13 , x43); + T b2 = dot(x23 , x43); + + assert( is_number(a11) || !"barycentric_algebraic(): NaN encountered"); + assert( is_number(a12) || !"barycentric_algebraic(): NaN encountered"); + assert( is_number(a22) || !"barycentric_algebraic(): NaN encountered"); + assert( is_number(b1) || !"barycentric_algebraic(): NaN encountered"); + assert( is_number(b2) || !"barycentric_algebraic(): NaN encountered"); + + assert( a22 < value_traits::zero() || a22>value_traits::zero() || !"barycentric_algebraic(): Degenerate triangle encountered"); + + T f = a12/a22; + T m = (a11 - a12*f); + T n = b1-(b2*f); + + assert( is_number(f) || !"barycentric_algebraic(): NaN encountered"); + assert( is_number(m) || !"barycentric_algebraic(): NaN encountered"); + assert( is_number(n) || !"barycentric_algebraic(): NaN encountered"); + + assert( m < value_traits::zero() || m > value_traits::zero() || !"barycentric_algebraic(): potential division by zero"); + + w1 = n/m; + w2 = b2/a22 - f*w1; + w3 = value_traits::one() - w1 - w2; + + assert( is_number(w1) || !"barycentric_algebraic(): NaN encountered"); + assert( is_number(w2) || !"barycentric_algebraic(): NaN encountered"); + assert( is_number(w3) || !"barycentric_algebraic(): NaN encountered"); + } + + /* + * Compute Barycentric Coordinates. + * This method computes the barycentric coodinates for a point p of a tetrahedron + * given by the points x1,x2,x3, p (in right-hand order). + * + * @warning This method uses a algebraic approach to compute the barycentric coordinates. + * + * @param x1 The first point of the triangle. + * @param x2 The second point of the triangle. + * @param x3 The third point of the triangle. + * @param x4 The fourth point of the triangle. + * @param p The point for which the barycentric coordinates should be computed. + * @param w1 Upon return this parameter contains the value of the first barycentric coordinate. + * @param w2 Upon return this parameter contains the value of the second barycentric coordinate. + * @param w3 Upon return this parameter contains the value of the third barycentric coordinate. + * @param w4 Upon return this parameter contains the value of the fourth barycentric coordinate. + */ + template + inline void barycentric_algebraic( + V const & x1 + , V const & x2 + , V const & x3 + , V const & x4 + , V const & p + , typename V::value_type & w1 + , typename V::value_type & w2 + , typename V::value_type & w3 + , typename V::value_type & w4 + ) + { + typedef typename V::value_type T; + typedef math::ValueTraits value_traits; + typedef math::Matrix3x3 M; // 2008-08-01 kenny: This is rather bad, it makes this function hardwired to our own matrix3x3 type! + + M P; + + P(0,0) = x1(0) - x4(0); + P(1,0) = x1(1) - x4(1); + P(2,0) = x1(2) - x4(2); + + P(0,1) = x2(0) - x4(0); + P(1,1) = x2(1) - x4(1); + P(2,1) = x2(2) - x4(2); + + P(0,2) = x3(0) - x4(0); + P(1,2) = x3(1) - x4(1); + P(2,2) = x3(2) - x4(2); + + P = math::inverse(P); // 2008-08-01 kenny: This is very specific for our OT library, it should probably be a policy? + + V w = P * V(p-x4); + + w1 = w(0); + w2 = w(1); + w3 = w(2); + w4 = value_traits::one() - w1 - w2 - w3; + + assert( is_number(w1) || !"barycentric_algebraic(): NaN encountered"); + assert( is_number(w2) || !"barycentric_algebraic(): NaN encountered"); + assert( is_number(w3) || !"barycentric_algebraic(): NaN encountered"); + assert( is_number(w4) || !"barycentric_algebraic(): NaN encountered"); + + } + + } // namespace geometry +} // namespace OpenTissue + +// OPENTISSUE_CORE_GEOMETRY_BARYCENTRIC_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_base_shape.h b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_base_shape.h new file mode 100644 index 000000000..8d1b08cc4 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_base_shape.h @@ -0,0 +1,64 @@ +#ifndef OPENTISSUE_CORE_GEOMETRY_GEOMETRY_BASE_SHAPE_H +#define OPENTISSUE_CORE_GEOMETRY_GEOMETRY_BASE_SHAPE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +#include + + +namespace OpenTissue +{ + + namespace geometry + { + + /** + * BaseShape Template Class + * If you implement a new geometry type then you should derive your geometry class from the BaseShape class + * + * @tparam math_types standard math types containing at least the real, vector3 and matrix3x3 types. + **/ + template< typename math_types > + class BaseShape + : public OpenTissue::collision::GeometryInterface< math_types > + { + public: + + virtual ~BaseShape() {} + + public: + + typedef typename math_types::real_type real_type; + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::matrix3x3_type matrix3x3_type; + + +// 2007-07-24 kenny: generic design problem with compute_surface_points +// 2007-09-11 micky: I agree, it's not possible to have a pure abstract interface and letting the user decide his choice of point container. +// 2007-09-11 micky: compute_surface_points and get_support_point should be removed from this interface. Both functions belong to some collision interface. +// 2007-09-18 kenny: I partial agree... get_support_point is used by GJK (exclusively as I recall?) However, compute_surface_points is used by some of the geometry utils. Fitting tools and such. + + // TODO 2007-02-06 kenny: Yikes what if end-user wants to use std::list or some homebrewed container type? It is a bit ugly that the container type is hardwired into the abstract base class:-( Could we make something like a pure virtual template class? + virtual void compute_surface_points( std::vector & points) const = 0; + virtual vector3_type get_support_point(vector3_type const & v) const = 0; + + virtual void translate(vector3_type const & T) = 0; + virtual void rotate(matrix3x3_type const & R) = 0; + virtual void scale(real_type const & s) = 0; + + }; + + } // namespace geometry + +} // namespace OpenTissue + +//OPENTISSUE_CORE_GEOMETRY_GEOMETRY_BASE_SHAPE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_compute_tetrahedron_aabb.h b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_compute_tetrahedron_aabb.h new file mode 100644 index 000000000..615213656 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_compute_tetrahedron_aabb.h @@ -0,0 +1,103 @@ +#ifndef OPENTISSUE_CORE_GEOMETRY_GEOMETRY_COMPUTE_TETRAHEDRON_AABB_H +#define OPENTISSUE_CORE_GEOMETRY_GEOMETRY_COMPUTE_TETRAHEDRON_AABB_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace geometry + { + /** + * Compute AABB of Tetrahedron. + * + * + * @param p0 The position of a corner of the tetrahedron. + * @param p1 The position of a corner of the tetrahedron. + * @param p2 The position of a corner of the tetrahedron. + * @param p3 The position of a corner of the tetrahedron. + * @param min_coord Upon return holds the minimum coordinate corner of the AABB. + * @param max_coord Upon return holds the maximum coordinate corner of the AABB. + */ + template + void compute_tetrahedron_aabb( + vector3_type const & p0 + , vector3_type const & p1 + , vector3_type const & p2 + , vector3_type const & p3 + , vector3_type & min_coord + , vector3_type & max_coord + ) + { + using std::min; + + min_coord = min(p0, p1); + min_coord = min(min_coord, p2); + min_coord = min(min_coord, p3); + + max_coord = max(p0, p1); + max_coord = max(max_coord, p2); + max_coord = max(max_coord, p3); + } + + /** + * Compute Tetrahedron AABB. + * + * This function computes a tight fitting axis aligned bounding box around the specified tetrahedron. + * + * @param tetrahedron The specified tetrahedron. + * @param min_coord Upon return holds the minimum coordinate corner of the AABB. + * @param max_coord Upon return holds the maximum coordinate corner of the AABB. + */ + template + void compute_tetrahedron_aabb( + tetrahedron_type const & tetrahedron + , vector3_type & min_coord + , vector3_type & max_coord + ) + { + compute_tetrahedron_aabb(tetrahedron.p0(),tetrahedron.p1(),tetrahedron.p2(),tetrahedron.p3(),min_coord,max_coord); + } + + /** + * Compute Tetrahedron AABB. + * + * This function computes a tight fitting axis aligned bounding box around the specified tetrahedron. + * + * @param tetrahedron The specified tetrahedron. + * @param aabb Upon return holds the computed AABB. + */ + template + void compute_tetrahedron_aabb(tetrahedron_type const & tetrahedron,aabb_type & aabb) + { + compute_tetrahedron_aabb(tetrahedron.p0(),tetrahedron.p1(),tetrahedron.p2(),tetrahedron.p3(),aabb.min(),aabb.max()); + } + + /** + * Compute Tetrahedron AABB. + * + * This function computes a tight fitting axis aligned bounding box around the specified tetrahedron. + * + * @param tetrahedron The specified tetrahedron. + * @return The computed AABB. + */ + template + geometry::AABB compute_tetrahedron_aabb(tetrahedron_type const & tetrahedron) + { + geometry::AABB aabb; + compute_tetrahedron_aabb(tetrahedron,aabb); + return aabb; + } + + } //End of namespace geometry +} //End of namespace OpenTissue + +// OPENTISSUE_CORE_GEOMETRY_GEOMETRY_COMPUTE_TETRAHEDRON_AABB_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_tetrahedron.h b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_tetrahedron.h new file mode 100644 index 000000000..fd00cd0c1 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_tetrahedron.h @@ -0,0 +1,272 @@ +#ifndef OPENTISSUE_CORE_GEOMETRY_GEOMETRY_TETRAHEDRON_H +#define OPENTISSUE_CORE_GEOMETRY_GEOMETRY_TETRAHEDRON_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + + +namespace OpenTissue +{ + + namespace geometry + { + + template< typename math_types_ > + class Tetrahedron + : public VolumeShape< math_types_ > + , public OpenTissue::utility::ClassID< OpenTissue::geometry::Tetrahedron > + { + public: + + typedef math_types_ math_types; + typedef typename math_types::value_traits value_traits; + typedef typename math_types::real_type real_type; + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::matrix3x3_type matrix3x3_type; + typedef typename math_types::quaternion_type quaternion_type; + + protected: + + vector3_type m_p0; ///< Tetrahedron vertex number 1 + vector3_type m_p1; ///< Tetrahedron vertex number 2 + vector3_type m_p2; ///< Tetrahedron vertex number 3 + vector3_type m_p3; ///< Tetrahedron vertex number 4 + + public: + + size_t const class_id() const { return OpenTissue::utility::ClassID >::class_id(); } + + Tetrahedron() + { + m_p0.clear(); + m_p1.clear(); + m_p2.clear(); + m_p3.clear(); + } + + virtual ~Tetrahedron() {} + + Tetrahedron(Tetrahedron const & tetrahedron_) { set(tetrahedron_); } + + Tetrahedron( vector3_type const & p0, vector3_type const & p1, vector3_type const & p2, vector3_type const & p3) + { + set(p0,p1,p2,p3); + } + + Tetrahedron const & operator=(Tetrahedron const & tetrahedron_) + { + set(tetrahedron_); + return *this; + } + + void set(Tetrahedron const & t) + { + m_p0 = t.m_p0; + m_p1 = t.m_p1; + m_p2 = t.m_p2; + m_p3 = t.m_p3; + } + + void set(vector3_type const & p0, vector3_type const & p1, vector3_type const & p2, vector3_type const & p3) + { + m_p0 = p0; + m_p1 = p1; + m_p2 = p2; + m_p3 = p3; + } + + real_type area() const + { + vector3_type u,v,uXv; + real_type A = value_traits::zero(); + + u = m_p1-m_p0; + v = m_p2-m_p0; + uXv = (u % v); + A += length(uXv); + + u = m_p0-m_p3; + v = m_p1-m_p3; + uXv = (u % v); + A += length(uXv); + + u = m_p1-m_p3; + v = m_p2-m_p3; + uXv = (u % v); + A += length(uXv); + + u = m_p2-m_p3; + v = m_p0-m_p3; + uXv = (u % v); + A += length(uXv); + + return A/value_traits::two(); + } + + void compute_surface_points(std::vector & points) const + { + points.push_back(m_p0); + points.push_back(m_p1); + points.push_back(m_p2); + points.push_back(m_p3); + } + + vector3_type & p0() { return m_p0; }; + vector3_type & p1() { return m_p1; }; + vector3_type & p2() { return m_p2; }; + vector3_type & p3() { return m_p3; }; + vector3_type const & p0() const { return m_p0; }; + vector3_type const & p1() const { return m_p1; }; + vector3_type const & p2() const { return m_p2; }; + vector3_type const & p3() const { return m_p3; }; + + vector3_type center() const { return vector3_type( (m_p0+m_p1+m_p2+m_p3)/4. ); }; + + /** + * Compute Barycentric Coordinates. + * + * @param p The point for which the barycentric coordinates should be computed. + * @param w1 Upon return this parameter contains the value of the first barycentric coordinate. + * @param w2 Upon return this parameter contains the value of the second barycentric coordinate. + * @param w3 Upon return this parameter contains the value of the third barycentric coordinate. + * @param w4 Upon return this parameter contains the value of the fourth barycentric coordinate. + */ + void barycentric(vector3_type const & p,real_type & w1,real_type & w2,real_type & w3,real_type & w4) const + { + OpenTissue::geometry::barycentric_algebraic(m_p0,m_p1,m_p2,m_p3,p,w1,w2,w3,w4); + } + + real_type diameter() const + { + using std::max; + using std::sqrt; + + vector3_type x30 = m_p3 - m_p0; + vector3_type x20 = m_p2 - m_p0; + vector3_type x10 = m_p1 - m_p0; + real_type d = max( max(x30*x30, x20*x20), x10*x10); + return sqrt(d); + } + + vector3_type get_support_point(vector3_type const & v) const + { + vector3_type p; + real_type tst = -FLT_MAX;//math::detail::lowest(); + real_type tmp = v * m_p0; + if (tmp>tst) + { + tst = tmp; + p = m_p0; + } + tmp = v * m_p1; + if (tmp>tst) + { + tst = tmp; + p = m_p1; + } + tmp = v *m_p2; + if (tmp>tst) + { + tst = tmp; + p = m_p2; + } + tmp = v * m_p3; + if (tmp>tst) + { + tst = tmp; + p = m_p3; + } + return p; + } + + void translate(vector3_type const & T) + { + m_p0 += T; + m_p1 += T; + m_p2 += T; + m_p3 += T; + } + + void rotate(matrix3x3_type const & R) + { + vector3_type c = center(); + m_p0 = (R*(m_p0 - c)) + c; + m_p1 = (R*(m_p1 - c)) + c; + m_p2 = (R*(m_p2 - c)) + c; + m_p3 = (R*(m_p3 - c)) + c; + } + + void scale(real_type const & s) + { + vector3_type c = center(); + m_p0 = s*(m_p0 - c) + c; + m_p1 = s*(m_p1 - c) + c; + m_p2 = s*(m_p2 - c) + c; + m_p3 = s*(m_p3 - c) + c; + } + + real_type volume() const + { + /* + ** From Zienkiewicz & Taylor p.637 + ** V = 1/6*det( 1 x0 y0 z0; 1 x1 y1 z1; 1 x2 y2 z2; 1 x3 y3 z3) + ** where x0 = n0->i; y0 = n0[1]; ... + ** Calculated by Mathematica ;-) + */ + return (m_p0[2]*m_p1[1]*m_p2[0] - m_p0[1]*m_p1[2]*m_p2[0] - m_p0[2]*m_p1[0]*m_p2[1] + + m_p0[0]*m_p1[2]*m_p2[1] + m_p0[1]*m_p1[0]*m_p2[2] - m_p0[0]*m_p1[1]*m_p2[2] + - m_p0[2]*m_p1[1]*m_p3[0] + m_p0[1]*m_p1[2]*m_p3[0] + m_p0[2]*m_p2[1]*m_p3[0] + - m_p1[2]*m_p2[1]*m_p3[0] - m_p0[1]*m_p2[2]*m_p3[0] + m_p1[1]*m_p2[2]*m_p3[0] + + m_p0[2]*m_p1[0]*m_p3[1] - m_p0[0]*m_p1[2]*m_p3[1] - m_p0[2]*m_p2[0]*m_p3[1] + + m_p1[2]*m_p2[0]*m_p3[1] + m_p0[0]*m_p2[2]*m_p3[1] - m_p1[0]*m_p2[2]*m_p3[1] + - m_p0[1]*m_p1[0]*m_p3[2] + m_p0[0]*m_p1[1]*m_p3[2] + m_p0[1]*m_p2[0]*m_p3[2] + - m_p1[1]*m_p2[0]*m_p3[2] - m_p0[0]*m_p2[1]*m_p3[2] + m_p1[0]*m_p2[1]*m_p3[2])/6.; + } + + + /** + * Compute Bounding Box. + * This method computes an axis aligned bounding + * box (AABB) that encloses the geometry. + * + * @param r The position of the model frame (i.e the coordinate frame the geometry lives in). + * @param R The orientation of the model frame (i.e the coordinate frame the geometry lives in). + * @param min_coord Upon return holds the minimum corner of the box. + * @param max_coord Upon return holds the maximum corner of the box. + * + */ + void compute_collision_aabb( + vector3_type const & r + , matrix3x3_type const & R + , vector3_type & min_coord + , vector3_type & max_coord + ) const + { + compute_tetrahedron_aabb( *this, min_coord, max_coord ); + } + + + }; + + } // namespace geometry + +} // namespace OpenTissue + +// OPENTISSUE_CORE_GEOMETRY_GEOMETRY_TETRAHEDRON_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_tetrahedron_slicer.h b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_tetrahedron_slicer.h new file mode 100644 index 000000000..cd7930dc0 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_tetrahedron_slicer.h @@ -0,0 +1,157 @@ +#ifndef OPENTISSUE_CORE_GEOMETRY_GEOMETRY_TETRAHEDRON_SLICER_H +#define OPENTISSUE_CORE_GEOMETRY_GEOMETRY_TETRAHEDRON_SLICER_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + + +namespace OpenTissue +{ + namespace geometry + { + + + template + class TetrahedronSlicer + { + public: + + typedef vector3_type_ vector3_type; + typedef typename vector3_type::value_type real_type; + + + vector3_type m_nodes[4]; ///< Nodes of tetrahedron + int m_edges[6][2]; ///< Edge topology of tetrahedron + ///< m_edges[i][0] index of the first node of the i'th edge + ///< m_edges[i][1] index of the second node of the i'th edge + + vector3_type m_intersections[4]; ///< Intersection points of slice and tetrahedron. + vector3_type m_triangle1[3]; ///< Vertices of first triangle in slice. + vector3_type m_triangle2[3]; ///< Vertices of second trinagle in slice. + + public: + + TetrahedronSlicer( + vector3_type const & p0 + , vector3_type const & p1 + , vector3_type const & p2 + , vector3_type const & p3 + ) + { + m_nodes[0] = p0; + m_nodes[1] = p1; + m_nodes[2] = p2; + m_nodes[3] = p3; + + // + // Node indices and edge labels: + // + // 3 + // + + // / \ + // / \ + // / . \ + // / \ + // / \ + // / . \ + // / \ + // / \ + // / . \ + // C F E + // / \ + // / . \ + // / \ + // / . \ + // / 2+ \ + // / . . \ + // / . B D. \ + // / . . \ + // +------------------A------------------+ + //0 1 + // + // + m_edges[ 0 ][ 0 ] = 0; // A + m_edges[ 0 ][ 1 ] = 1; + m_edges[ 1 ][ 0 ] = 0; // B + m_edges[ 1 ][ 1 ] = 2; + m_edges[ 2 ][ 0 ] = 0; // C + m_edges[ 2 ][ 1 ] = 3; + m_edges[ 3 ][ 0 ] = 1; // D + m_edges[ 3 ][ 1 ] = 2; + m_edges[ 4 ][ 0 ] = 1; // E + m_edges[ 4 ][ 1 ] = 3; + m_edges[ 5 ][ 0 ] = 2; // F + m_edges[ 5 ][ 1 ] = 3; + + }; + + protected: + + template + void compute_slice(plane_type const & plane) + { + //--- Loop over all edges and find those that cross the plane + int count = 0; + for(int i=0;i<6;++i) + { + vector3_type & a = m_nodes[m_edges[i][0]]; + vector3_type & b = m_nodes[m_edges[i][1]]; + real_type dst_a = plane.signed_distance(a); + real_type dst_b = plane.signed_distance(b); + if ( (dst_a*dst_b<0) || (dst_a==0 && dst_b>0) || (dst_b==0 && dst_a>0)) + { + //--- Compute coordinates of the plane intersections + vector3_type & a = m_nodes[m_edges[ i ][0]]; + vector3_type & b = m_nodes[m_edges[ i ][1]]; + m_intersections[count++] = plane.get_intersection(a,b); + } + } + assert(count==3 || count==4); //--no other possibilities? + + //--- Order plane intersections to a a CCW polygon (as seen from positive side of plane) + vector3_type & n = plane.n(); + vector3_type e10 = m_intersections[1] - m_intersections[0]; + vector3_type e21 = m_intersections[2] - m_intersections[1]; + if(n*(e10%e20)<0) + { + m_triangle1[0] = m_intersections[0]; + m_triangle1[1] = m_intersections[2]; + m_triangle1[2] = m_intersections[1]; + } + else + { + m_triangle1[0] = m_intersections[0]; + m_triangle1[1] = m_intersections[1]; + m_triangle1[2] = m_intersections[2]; + } + if ( count == 4 ) + { + for(int i=0;i<3;++i) + { + int next = (i+1)%3; + int prev = i; + vector3_type e = m_triangle1[next] - m_triangle1[prev]; + vector3_type h = m_intersections[4] - m_triangle1[prev]; + if(n*(h%e)>0) + { + m_triangle2[0] = m_triangle1[prev]; + m_triangle2[1] = m_intersections[4]; + m_triangle2[2] = m_triangle1[next]; + break; + } + } + } + }; + + }; + + } // namespace geometry +} // namespace OpenTissue + +//OPENTISSUE_CORE_GEOMETRY_GEOMETRY_TETRAHEDRON_SLICER_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_tetrahedron_z_slicer.h b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_tetrahedron_z_slicer.h new file mode 100644 index 000000000..3b9a07721 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/geometry/geometry_tetrahedron_z_slicer.h @@ -0,0 +1,165 @@ + #ifndef OPENTISSUE_CORE_GEOMETRY_GEOMETRY_TETRAHEDRON_Z_SLICER_H +#define OPENTISSUE_CORE_GEOMETRY_GEOMETRY_TETRAHEDRON_Z_SLICER_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace geometry + { + /** + * Specialized Tetrahedron Slicer. + * Cut a tetrahedron with a specified z-plane and returns the + * sliced tetrahedron. + * + * This code was originally implemented by Andreas Bæentzen, IMM (jab@imm.dtu.dk). + * + */ + template + class ZTetrahedronSlicer + { + public: + + typedef vector3_type_ vector3_type; + typedef typename vector3_type::value_type real_type; + + protected: + + vector3_type m_p[4]; ///< Nodes of tetrahedron. + + public: + + ZTetrahedronSlicer( + vector3_type const & p0 + , vector3_type const & p1 + , vector3_type const & p2 + , vector3_type const & p3 + ) + { + m_p[0] = p0; + m_p[1] = p1; + m_p[2] = p2; + m_p[3] = p3; + + //--- Primitive bubble sort - ensures that tetrahedron vertices + //--- are sorted according to z value. + while(swap(0,1)||swap(1,2)||swap(2,3)); + }; + + protected: + + /** + * Swap the order of two nodes if their z-value allows it. + * + * @param i A node index. + * @param j A node index. + * + * @return true if swapped otherwise false. + */ + bool swap(int i,int j) + { + if(m_p[j](2)< m_p[i](2)) + { + vector3_type tmp = m_p[j]; + m_p[j] = m_p[i]; + m_p[i] = tmp; + return true; + } + return false; + }; + + /** + * Computes intersection point of edge going from node i towards node j + * + * @param z The z value, indicating the z-plane aginst which the edge is tested. + * @param i The node index of the starting node (got lowest z-value). + * @param j The node index of the ending node (got highest z-value). + * @param p Upon return this argument holds the intersection point. + */ + void intersect(real_type const & z, int i, int j, vector3_type & p) const + { + p = m_p[i]; + vector3_type dir = m_p[j] - m_p[i]; + real_type s = (z-m_p[i](2))/(m_p[j](2)-m_p[i](2)); + p += s * dir; + }; + + public: + + /** + * Compute Intersection Slice of Tetrahedron and Z-plane. + * + * @param z The z-plane to slice against. + * @param slice Upon return holds the intersection points of the sliced tetrahedron. + * + * @return The number of vertices in the sliced tetrahedron. + */ + unsigned int intersect(real_type const & z, vector3_type slice[4]) const + { + if( z < m_p[0](2) || z > m_p[3](2) ) + return 0; + if( z m_p[0](2)) + // { + // if(z < m_p[3](2)) + // { + // intersect(z,0,3,slice[cnt++]); + // if(z + +#include + + +namespace OpenTissue +{ + namespace geometry + { + /** + * VolumeShape Template Class. + * If you implement a new volumetric geometry then you should derive your new class from the VolumeShape class . + * + * @tparam math_types standard math types containing at least the real, vector3 and matrix3x3 types. + **/ + template + class VolumeShape + : public BaseShape< math_types > + { + public: + + virtual ~VolumeShape() {} + + public: + + typedef typename math_types::real_type real_type; + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::matrix3x3_type matrix3x3_type; + + /** + * Get Center Position. + * + * @return The center point of the volume shape, in most cases this would be the mean point. + */ + virtual vector3_type center() const = 0; + + /** + * Get Volume. + * + * @return The actual volume of the shape. + */ + virtual real_type volume() const = 0; + + /** + * Get Area. + * + * @return The actual surface area of the volume. + */ + virtual real_type area() const = 0; + + /** + * Get Diameter. + * + * + * @return A measure of the diameter of the shape. Not all shapes have a well-defined + * diameter in such cases the measure would simply be a measure of the longst + * extent of the shape. + */ + virtual real_type diameter() const = 0; + + }; + + } // namespace geometry + +} // namespace OpenTissue + +//OPENTISSUE_CORE_GEOMETRY_GEOMETRY_VOLUME_SHAPE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_basic_types.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_basic_types.h new file mode 100644 index 000000000..c6286de98 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_basic_types.h @@ -0,0 +1,52 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_BASIC_TYPES_H +#define OPENTISSUE_CORE_MATH_MATH_BASIC_TYPES_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include +#include +#include +#include + +namespace OpenTissue +{ + namespace math + { + + // TODO add doxygen documentation explaining rationale behind this typebinder class. Why have we made it, and for what purpose? Also give example usages + template< typename real_type_ + , typename index_type_ + > + class BasicMathTypes + { + public: + + typedef real_type_ real_type; + typedef index_type_ index_type; + typedef Vector3 vector3_type; + typedef Quaternion quaternion_type; + typedef DualQuaternion dual_quaternion_type; + typedef Matrix3x3 matrix3x3_type; + typedef CoordSys coordsys_type; + + typedef Vector3 index_vector3_type; + typedef ValueTraits value_traits; + }; + + + // TODO add doxygen comments explaining what this type is useful for + typedef BasicMathTypes default_math_types; + + } // namespace math +} // namespace OpenTissue + +//OPENTISSUE_CORE_MATH_MATH_BASIC_TYPES_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_compute_contiguous_angle_interval.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_compute_contiguous_angle_interval.h new file mode 100644 index 000000000..311f8c255 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_compute_contiguous_angle_interval.h @@ -0,0 +1,139 @@ +#ifndef OPENTISSUE_CORE_MATH_COMPUTE_CONTIGUOUS_ANGLE_INTERVAL_H +#define OPENTISSUE_CORE_MATH_COMPUTE_CONTIGUOUS_ANGLE_INTERVAL_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +#include +#include +#include +#include + +namespace OpenTissue +{ + namespace math + { + + /** + * Compute Contiguous Angle Interval. + * This function tries to find a contiguous interval of angle values. As + * input it takes a sequence of angle values. From this sequence the + * function tries to compute the starting and ending angle (in terms + * of counter-clock-wise rotation order) of a contiguous interval. + * interval containing all the angles. + * + * One could interact with the function as follows + * + * std::vector angles; + * + * ... fill angles with values ... + * + * double min_val, max_val; + * compute_contiguous_angle_interval( angles.begin(), angles.end(), min_val, max_val ); + * + * The resulting minimum and maximum values yields the starting and ending points + * of the contiguous interval. The periodicity of rotation angles is used to adjust + * the end-points of the interval to always be positive values. + * + * The supplied angles can be given in any sort of interval as long as + * the width of the interval is no longer than 2 pi ( one full rotation). Typically + * one would supply values from either -pi..pi or 0..2pi intervals. + * + * @param begin An iterator to the first angle value + * @param end An iterator to one past the last angle value + * @param theta_min Upon return this value holds the starting value of the contiguous angle interval. + * @param theta_max Upon return this value holds the ending value of the contiguous angle interval. + */ + template + inline void compute_contiguous_angle_interval( + iterator_type const & begin + , iterator_type const & end + , typename iterator_type::value_type & theta_min + , typename iterator_type::value_type & theta_max ) + { + //using std::cos; + //using std::sin; + //using std::sqrt; + //using std::atan2; + + typedef typename iterator_type::value_type T; + typedef OpenTissue::math::ValueTraits value_traits; + + + T const two_pi = value_traits::pi()*value_traits::two(); + + // Determine the number of samples + size_t const N = std::distance( begin, end); + + // Make a copy of the samples and sort them in ascending order + std::vector storage; + storage.resize( N ); + std::copy( begin, end, storage.begin() ); + std::sort( storage.begin(), storage.end() ); + + // Find the two theta values with the largest gap inbetween. That + // is the largest angle difference as measured in a counter-clock-wise manner. + T max_delta_theta = value_traits::zero(); + size_t max_i = N; + + for(size_t i = 0u;i max_delta_theta) + { + max_i = i; + max_delta_theta = delta_theta; + theta_min = theta_j; + theta_max = theta_i; + } + } + + //// Compute the mean angle by converting angles to points on + //// the unit-circle and calculating the mean point. + //T mx = value_traits::zero(); + //T my = value_traits::zero(); + //for(iterator_type theta = begin; theta != end; ++ theta) + //{ + // T const c = cos( *theta ); + // T const s = sin( *theta ); + // mx += c; + // my += s; + //} + + //T norm = sqrt( mx*mx + my*my); + //assert( is_number(norm) || !"compute_contiguous_angle_interval(): NaN encountered"); + + //if(norm<= value_traits::zero()) + // throw std::logic_error("insufficient angle samples to determine a mean angle"); + + //mx /= norm; + //my /= norm; + + //assert( is_number(mx) || !"compute_contiguous_angle_interval(): NaN encountered"); + //assert( is_number(my) || !"compute_contiguous_angle_interval(): NaN encountered"); + + while(theta_min < value_traits::zero()) + theta_min += two_pi; + + while(theta_max < theta_min) + theta_max += two_pi; + + assert( theta_min <= theta_max || !"compute_contiguous_angle_interval(): invalid interval"); + } + + + } // namespace math +} // namespace OpenTissue + +//OPENTISSUE_CORE_MATH_COMPUTE_CONTIGUOUS_ANGLE_INTERVAL_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_constants.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_constants.h new file mode 100644 index 000000000..59b1363aa --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_constants.h @@ -0,0 +1,209 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_CONSTANTS_H +#define OPENTISSUE_CORE_MATH_MATH_CONSTANTS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +//#include +#include + + +namespace OpenTissue +{ + + namespace math + { + + // constant: 0 (zero) + namespace detail + { + + template + inline T zero(); + + template <> + inline float zero() { return 0.0f; } + + template <> + inline double zero() { return 0.0; } + + template <> + inline int zero() { return 0; } + + template <> + inline long unsigned int zero() { return 0; } + + template <> + inline unsigned int zero() { return 0u; } + + } // namespace detail + + + // constant: 1 (one) + namespace detail + { + + template + inline T one(); + + template <> + inline float one() { return 1.0f; } + + template <> + inline double one() { return 1.0; } + + template <> + inline int one() { return 1; } + + template <> + inline unsigned int one() { return 1u; } + + } // namespace detail + + + // constant: 2 (two) + namespace detail + { + + template + inline T two(); + + template <> + inline float two() { return 2.0f; } + + template <> + inline double two() { return 2.0; } + + template <> + inline int two() { return 2; } + + template <> + inline unsigned int two() { return 2u; } + + } // namespace detail + + + // constant: 3 (three) + namespace detail + { + + template + inline T three(); + + template <> + inline float three() { return 3.0f; } + + template <> + inline double three() { return 3.0; } + + template <> + inline int three() { return 3; } + + template <> + inline unsigned int three() { return 3u; } + + } // namespace detail + + + // constant: 4 (four) + namespace detail + { + + template + inline T four(); + + template <> + inline float four() { return 4.0f; } + + template <> + inline double four() { return 4.0; } + + template <> + inline int four() { return 4; } + + template <> + inline unsigned int four() { return 4u; } + + } // namespace detail + + + // constant: 8 (eight) + namespace detail + { + + template + inline T eight(); + + template <> + inline float eight() { return 8.0f; } + + template <> + inline double eight() { return 8.0; } + + template <> + inline int eight() { return 8; } + + template <> + inline unsigned int eight() { return 8u; } + + } // namespace detail + + + // constant: ½ (half) + namespace detail + { + + template + inline T half(); + + template <> + inline float half() { return 0.5f; } + + template <> + inline double half() { return 0.5; } + + } // namespace detail + + + // constant: pi and fractions of pi + namespace detail + { + + template + inline T pi() { return (T)(M_PI); } + + template + inline T pi_half() { return (T)(M_PI_2); } + + template + inline T pi_quarter() { return (T)(M_PI_4); } + + } // namespace detail + + + // constant: specials + namespace detail + { + + + // one degree in radians + template + inline T degree() { return (T)(0.017453292519943295769236907684886); } + + // one radian in degrees + template + inline T radian() { return (T)(57.295779513082320876798154814105); } + + } + + } // namespace math + +} // namespace OpenTissue + +// #define OPENTISSUE_CORE_MATH_MATH_CONSTANTS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_coordsys.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_coordsys.h new file mode 100644 index 000000000..ce71c34d1 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_coordsys.h @@ -0,0 +1,320 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_COORDSYS_H +#define OPENTISSUE_CORE_MATH_MATH_COORDSYS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include +#include + +#include + +namespace OpenTissue +{ + + namespace math + { + + /** + * A Coordinate System. + * + * Remeber, this represents a transform that brings you from a local frame + * into a global frame, that is: + * + * BF -> WCS + */ + template< + typename value_type_ + //, typename value_traits_ = ValueTraits + > + class CoordSys + { + protected: + + typedef typename OpenTissue::math::ValueTraits value_traits_ ; // TODO value_traits_ should be parameterized as a class template parameter. + + public: + + typedef value_traits_ value_traits; ///< Convenience typedef to make value traits accessible for all template functions using Vector3 types. + typedef value_type_ value_type; ///< Typedef is required for compliance with many other libraries and data containers! + + typedef Vector3 vector3_type; + typedef Quaternion quaternion_type; + typedef Matrix3x3 matrix3x3_type; + typedef Quaternion rotation_type; ///< typedef to make interface for different coordsystypes the same + + protected: + + vector3_type m_T; ///< The Position. + quaternion_type m_Q; ///< The orientation in Quaternion form. + + public: + + vector3_type & T() { return m_T; } + vector3_type const & T() const { return m_T; } + quaternion_type & Q() { return m_Q; } + quaternion_type const & Q() const { return m_Q; } + + public: + + CoordSys() + : m_T(value_traits::zero(),value_traits::zero(),value_traits::zero()) + , m_Q(value_traits::one(),value_traits::zero(),value_traits::zero(),value_traits::zero()) + {} + + explicit CoordSys(vector3_type const & T_val, quaternion_type const & Q_val) + { + m_T = T_val; + m_Q = unit(Q_val); + } + + explicit CoordSys(vector3_type const & T_val, matrix3x3_type const & R_val) + { + m_T = T_val; + m_Q = R_val; + } + + CoordSys & operator=(CoordSys const & C) + { + m_T = C.m_T; + m_Q = C.m_Q; + return *this; + } + + public: + + // TODO: Comparing floats with == or != is not safe NOTE T might not be a float type it could be anything? This suggest that we need some kind of metaprogramming technique to deal with this problem? + bool operator==(CoordSys const & C) const { return m_T == C.m_T && m_Q==C.m_Q; } + + public: + + void identity() + { + m_T.clear(); + m_Q.identity(); + } + + /** + * This method assumes that the point is in this coordinate system. + * In other words this method maps local points into non local + * points: + * + * BF -> WCS + * + * Let p be the point and let f designate the function of this + * method then we have + * + * [p]_WCS = f(p) + * + */ + void xform_point(vector3_type & p) const { p = m_Q.rotate(p) + m_T; } + + /** + * This method assumes that the vector is in this + * coordinate system. That is it maps the vector + * from BF into WCS. + */ + void xform_vector(vector3_type & v) const { v = m_Q.rotate(v); } + + /** + * Transform Matrix. + * + * @param O A reference to a rotation matrix, which should be transformed. + */ + void xform_matrix(matrix3x3_type & O) const + { + O = matrix3x3_type(m_Q) * O; + } + + /** + * Transform Coordinate System. + * This method transforms the specified coordinate + * system by this coordinate transform. + * + * That is: + * + * Tnew = Tx Rx Told + * Rnew = Rx Rold + * + * @param X The transform by which the current + * transform should be changed with.. + */ + CoordSys operator*(CoordSys const & X1) const + { + return CoordSys( m_Q.rotate(X1.T()) + m_T , unit( prod( m_Q , X1.Q()) ) ); + } + + public: + + bool is_equal(CoordSys const & C, value_type const & accuracy) const + { + return m_T.is_equal(C.m_T,accuracy) && m_Q.is_equal(C.m_Q,accuracy); + } + + }; // class CoordSys + + + + /** + * Coordinate Transformation Product. + * This function should be used to concatenate coordinate transformations. + * In terms of homogeneous coordinates L and R corresponds to the matrices. + * + * L = | R_l T_l | + * | 0 1 | + * + * R = | R_r T_r | + * | 0 1 | + * + * This function computes the equivalent of the product + * + * X = L R + * + * = | R_l T_l | | R_r T_r | + * | 0 1 | | 0 1 | + * + * = | R_l R_r R_l T_r + T_l | + * | 0 1 | + * + * @param L The left coordinate transformation + * @param R The right coordinate transformation + * + * @return The coordinate transformation corresponding to the product L*R. + */ + template + inline CoordSys prod(CoordSys const & L, CoordSys const & R) + { + return CoordSys( L.Q().rotate( R.T() ) + L.T() , unit( prod( L.Q() , R.Q()) ) ); + } + + /** + * Inverse Transform. + * + * If we have + * + * BF -> WCS + * + * Then we want to find + * + * WCS -> BF + * + */ + template + inline CoordSys inverse(CoordSys const & X) + { + //--- + //--- p' = R p + T + //--- + //--- => + //--- + //--- p = R^{-1} p' + R^{-1}(-T) + //--- + return CoordSys( conj(X.Q()).rotate(-X.T()) , conj(X.Q()) ); + } + + /** + * Model Update Transform. + * This method computes the necessary transform needed in + * order to transform coordinates from one local frame + * into another local frame. This utility is useful when + * one wants to do model updates instead of world updates. + * + * In mathematical terms we have two transforms: + * + * C1 : H -> G + * C2 : F -> G + * + * And we want to find + * + * C3 : H -> F + * + * This transform is computed and assigned to this coordinate + * system. + */ + template + inline CoordSys model_update(CoordSys const & A, CoordSys const & B) + { + return model_update(A.T(),A.Q(),B.T(),B.Q()); + } + + /** + * Model Update Transform. + * This method computes the necessary transform needed in + * order to transform coordinates from one local frame + * into another local frame. This utility is useful when + * one wants to do model updates instead of world updates. + * + * In mathematical terms we have two transforms: + * + * C1 : H -> G + * C2 : F -> G + * + * And we want to find + * + * C3 : H -> F + * + * This transform is computed and assigned to this coordinate + * system. + * + * Very important: Note that this method finds the transform A -> B. + */ + template + inline CoordSys model_update(Vector3 const & TA, Quaternion const & QA, Vector3 const & TB, Quaternion const & QB) + { + //--- + //--- p' = RA p + TA (*1) from A->WCS + //--- + //--- p = RB^T (p' - TB) (*2) from WCS-B + //--- + //--- Insert (*1) into (*2) A -> B + //--- + //--- p = RB^T ( RA p + TA - TB) + //--- = RB^T RA p + RB^T (TA - TB) + //--- So + //--- R = RB^T RA + //--- T = RB^T (TA - TB) + //--- + Quaternion q; + if(QA==QB) + { + q.identity(); + } + else + { + q = unit( prod( conj(QB), QA) ); + } + return CoordSys( conj(QB).rotate(TA - TB), q); + } + + template + inline std::ostream & operator<< (std::ostream & o, CoordSys const & C) + { + o << "[" << C.T() << "," << C.Q() << "]"; + return o; + } + + template + inline std::istream & operator>>(std::istream & i, CoordSys & C) + { + char dummy; + i >> dummy; + i >> C.T(); + i >> dummy; + i >> C.Q(); + i >> dummy; + return i; + } + + } // namespace math + +} // namespace OpenTissue + +//OPENTISSUE_CORE_MATH_MATH_COORDSYS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_covariance.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_covariance.h new file mode 100644 index 000000000..45a4dd2bd --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_covariance.h @@ -0,0 +1,84 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_COVARIANCE_H +#define OPENTISSUE_CORE_MATH_MATH_COVARIANCE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + + namespace math + { + + /** + * + * @param mean Resulting mean point. + * @param C Resulting covariance matrix. + */ + template< typename vector3_iterator, typename vector3_type, typename matrix3x3_type> + inline void + covariance( + vector3_iterator begin, vector3_iterator end + , vector3_type & mean, matrix3x3_type & C + ) + { + typedef typename vector3_type::value_traits value_traits; + + unsigned int N = 0; + + mean.clear(); + + for(vector3_iterator v = begin;v!=end;++v,++N) + mean += (*v); + mean /= N; + + C.clear(); + for(vector3_iterator v = begin;v!=end;++v,++N) + { + C(0,0) += ((*v)(0) - mean(0))*((*v)(0) - mean(0)); + C(1,1) += ((*v)(1) - mean(1))*((*v)(1) - mean(1)); + C(2,2) += ((*v)(2) - mean(2))*((*v)(2) - mean(2)); + C(0,1) += ((*v)(0) - mean(0))*((*v)(1) - mean(1)); + C(0,2) += ((*v)(0) - mean(0))*((*v)(2) - mean(2)); + C(1,2) += ((*v)(1) - mean(1))*((*v)(2) - mean(2)); + } + C(1,0) = C(0,1); + C(2,0) = C(0,2); + C(2,1) = C(1,2); + C /= N; + } + + /** + * + * + * @param mean Resulting mean point. + * @param C Resulting covariance matrix. + */ + template + inline void + covariance_union( + vector3_type const & mean1, matrix3x3_type const & C1 + , vector3_type const & mean2, matrix3x3_type const & C2 + , vector3_type & mean, matrix3x3_type & C + ) + { + typedef typename vector3_type::value_traits value_traits; + + mean = (mean1 + mean2)/value_traits::two(); + matrix3x3_type KK = outer_prod(mean,mean); + matrix3x3_type NN = outer_prod(mean1,mean1); + matrix3x3_type MM = outer_prod(mean2,mean2); + C = ((C1 + NN + C2 + MM)/value_traits::two() - KK) ; + } + + } // namespace math + +} // namespace OpenTissue + +// OPENTISSUE_CORE_MATH_MATH_COVARIANCE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_dual_quaternion.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_dual_quaternion.h new file mode 100644 index 000000000..ca811117b --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_dual_quaternion.h @@ -0,0 +1,198 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_DUAL_QUATERNION_H +#define OPENTISSUE_CORE_MATH_MATH_DUAL_QUATERNION_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2010 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +#include + +#include + +// 2010-07-16 Kenny : code review, where is the unit tests? + +namespace OpenTissue +{ + + namespace math + { + + // 2010-07-16 Kenny: code review, why do you keep uncommented code around? + // 2010-07-16 perb: I have keept this as I thought this was the direction OpenTissue was going, + // considering that it is in both the vector3 and quaternion class + // I thought it was a preparation to get rid of the next TODO... + template < + typename value_type_ + //, typename value_traits_ = ValueTraits + > + class DualQuaternion + { + protected: + + // 2010-07-16 Kenny: code review, the TODO comment seems unneeded? + // 2010-07-16 perb + typedef typename OpenTissue::math::ValueTraits value_traits_ ; // TODO value_traits_ should be parameterized as a class template parameter. + + public: + + typedef value_traits_ value_traits; ///< Convience typedef to make value traits accesible for all template functions using Vector3 types. + typedef value_type_ value_type; ///< Typedef is required for compliance with many other libraries and data containers! + typedef size_t index_type; + typedef Vector3 vector3_type; + typedef Quaternion quaternion_type; + + protected: + + quaternion_type m_real; ///< The real part of this dual quaternion. This represents the rotation when used as a rigid motion. + quaternion_type m_dual; ///< The dual part of this dual quaternion. This represents the displacement when used as a rigid motion. + + public: + + quaternion_type & r() { return m_real; } + quaternion_type const & r() const { return m_real; } + + quaternion_type & d() { return m_dual; } + quaternion_type const & d() const { return m_dual; } + + public: + + DualQuaternion() + : m_real( quaternion_type( value_traits::one(), value_traits::zero(), value_traits::zero(), value_traits::zero() ) ) + , m_dual( quaternion_type( value_traits::zero(), value_traits::zero(), value_traits::zero(), value_traits::zero() ) ) + {} + + /** + * This constructs a dual quaternion representing the transformation of the given vector + * + * @param v Vector describing a translation + * + * @return A dual quaternion describing the translation given + */ + DualQuaternion( vector3_type const & v ) + : m_real( quaternion_type( value_traits::one(), value_traits::zero(), value_traits::zero(), value_traits::zero() ) ) + , m_dual( quaternion_type( value_traits::one(), v[0] / value_traits::two(), v[1] / value_traits::two(), v[2] / value_traits::two() ) ) + {} + + /** + * This constructs a unit dual quaternion describing a rotation and a translation + * + * @param rotation A unit quaternion describing a rotation + * @param translation A vector describing a translation + * + * @return A unit dual quaternion describing the rotation and translation given + */ + DualQuaternion( quaternion_type const & rotation, vector3_type const & translation ) + : m_real( rotation ) + , m_dual( prod( quaternion_type( value_traits::zero(), translation[0], translation[1], translation[2] ), rotation) / value_traits::two() ) + {} + + DualQuaternion( DualQuaternion< value_type > const & d ) { *this = d; } + + DualQuaternion( quaternion_type const & s_val, quaternion_type const & d_val ) + : m_real( s_val ) + , m_dual( d_val ) + {} + + ~DualQuaternion() + {} + + DualQuaternion & operator=( DualQuaternion const & cpy ) + { + m_real = cpy.m_real; + m_dual = cpy.m_dual; + + return *this; + } + + DualQuaternion & operator+= ( DualQuaternion const & qd ) + { + // 2010-07-16 Kenny: code review, safe guard for self assignment? if (this!=&qd)??? + // 2010-07-16 perb: I am not sure why this would be a problem. + // 2010-07-16 kenny: Argh, sorry, I read the code too fast, I read it as an assignment operator. + m_real += qd.r(); + m_dual += qd.d(); + + return *this; + } + + public: + + friend std::ostream & operator<< ( std::ostream & o, DualQuaternion< value_type > const & d ) + { + o << "[" << d.r() << "," << d.d() << "]"; + + return o; + } + + friend std::istream & operator>>( std::istream & i, DualQuaternion< value_type > & d ) + { + char dummy; + + i >> dummy; + i >> d.r(); + i >> dummy; + i >> d.d(); + i >> dummy; + + return i; + } + }; // class DualQuaternion + + + ////////////////////////////////////////////////////////////////////////// + /// Declaration of DualQuaternion non-member functions + ////////////////////////////////////////////////////////////////////////// + + template < typename T, typename T2 > + inline DualQuaternion operator*( DualQuaternion const& dq, T2 const& v ) { return DualQuaternion< T >( dq.r() * v, dq.d() * v ); } + + template < typename T2, typename T > + inline DualQuaternion operator*( T2 const& v, DualQuaternion const& dq ) { return DualQuaternion< T >( dq.r() * v, dq.d() * v ); } + + template < typename T, typename T2 > + inline DualQuaternion operator/( DualQuaternion const& dq, T2 const& v ) { return DualQuaternion< T >( dq.r() / v, dq.d() / v ); } + + template < typename T > + inline DualQuaternion< T > operator-( DualQuaternion< T > q ) { return DualQuaternion< T >( -q.r(), -q.d() ); } + + + /** + * Dual conjugate. This is the conjugate that considers the dual quaternion as a dual number with quaternion elements. + * + * @warning There are two different types. The quaternion conjugate and the dual conjugate. + */ + template< typename T > + inline DualQuaternion< T > conj_dual( DualQuaternion< T > const & dq ) + { + return DualQuaternion< T >( dq.r(), -dq.d() ); + } + + /** + * Quaternion conjugate. This is the conjugate that considers the dual quaternion as a quaternion with dual number elements. + * + * @warning There are two different types. The quaternion conjugate and the dual conjugate. + */ + template< typename T > + inline DualQuaternion< T > conj_quaternion( DualQuaternion< T > const & dq ) + { + return DualQuaternion< T >( conj( dq.r() ), conj( dq.d() ) ); + } + + template< typename T > + inline DualQuaternion< T > prod( DualQuaternion< T > const & q, DualQuaternion< T > const & p ) + { + return DualQuaternion< T >( prod( q.r(), p.r() ), prod( q.r(), p.d() ) + prod( q.d(), p.r() ) ); + } + + } // namespace math + +} // namespace OpenTissue + +//OPENTISSUE_CORE_MATH_MATH_DUAL_QUATERNION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_eigen_system_decomposition.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_eigen_system_decomposition.h new file mode 100644 index 000000000..70bdf4108 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_eigen_system_decomposition.h @@ -0,0 +1,173 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_EIGEN_SYSTEM_DECOMPOSITION_H +#define OPENTISSUE_CORE_MATH_MATH_EIGEN_SYSTEM_DECOMPOSITION_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include // for sqrt() and fabs + +namespace OpenTissue +{ + + namespace math + { + /** + * Eigen System Decomposition. + * + * @param A Matrix to find eigenvectors and eigenvalues of. + * @param V Upon return the columns of this matrix contains the + * eigenvectors. IMPORTANT: V may not form a right-handed + * coordinate system (ie. V might not be a rotation matrix). If + * a rotation matrix is needed one should compute the determinant + * of V and flip the sign of one of V's columns in case the + * determinant is negative. That is: + * + * if(det(V)<0) { V(0,0) =- V(0,0); V(1,0) =- V(1,0); V(2,0) =- V(2,0); } + * + * + * @param diag Upon return this vector contains the + * eigenvalues, such that entry 0 correpsonds to + * eigenvector 0 and so on. + */ + template + inline void eigen(matrix3x3_type const & A,matrix3x3_type & V,vector3_type & diag) + { + typedef typename vector3_type::value_type real_type; + typedef typename vector3_type::value_traits value_traits; + + using std::sqrt; + using std::fabs; + + vector3_type sub_diag; + + sub_diag.clear(); + diag.clear(); + + V = A; + + real_type const & fM00 = V(0,0); + real_type fM01 = V(0,1); + real_type fM02 = V(0,2); + real_type const & fM11 = V(1,1); + real_type const & fM12 = V(1,2); + real_type const & fM22 = V(2,2); + + diag(0) = fM00; + sub_diag(2) = value_traits::zero(); + if ( fM02 != value_traits::zero() ) + { + real_type fLength = sqrt(fM01*fM01+fM02*fM02); + real_type fInvLength = (value_traits::one())/fLength; + fM01 *= fInvLength; + fM02 *= fInvLength; + real_type fQ = (value_traits::two())*fM01*fM12+fM02*(fM22-fM11); + diag(1) = fM11+fM02*fQ; + diag(2) = fM22-fM02*fQ; + sub_diag(0) = fLength; + sub_diag(1) = fM12-fM01*fQ; + V(0,0) = value_traits::one(); + V(0,1) = value_traits::zero(); + V(0,2) = value_traits::zero(); + V(1,0) = value_traits::zero(); + V(1,1) = fM01; + V(1,2) = fM02; + V(2,0) = value_traits::zero(); + V(2,1) = fM02; + V(2,2) = -fM01; + } + else + { + diag(1) = fM11; + diag(2) = fM22; + sub_diag(0) = fM01; + sub_diag(1) = fM12; + V(0,0) = value_traits::one(); + V(0,1) = value_traits::zero(); + V(0,2) = value_traits::zero(); + V(1,0) = value_traits::zero(); + V(1,1) = value_traits::one(); + V(1,2) = value_traits::zero(); + V(2,0) = value_traits::zero(); + V(2,1) = value_traits::zero(); + V(2,2) = value_traits::one(); + } + + const int max_iterations = 32; + const int dim = 3; + for (int i0 = 0; i0 < dim; ++i0) + { + int i1; + for (i1 = 0; i1 < max_iterations; ++i1) + { + int i2; + for (i2 = i0; i2 <= dim-2; ++i2) + { + real_type fTmp = fabs(diag(i2)) + fabs(diag(i2+1)); + if ( fabs(sub_diag(i2)) + fTmp == fTmp ) + break; + } + if ( i2 == i0 ) + break; + real_type fG = (diag(i0+1) - diag(i0))/(value_traits::two()* sub_diag(i0)); + real_type fR = sqrt(fG*fG+value_traits::one()); + if ( fG < value_traits::zero() ) + fG = diag(i2)-diag(i0)+sub_diag(i0)/(fG-fR); + else + fG = diag(i2)-diag(i0)+sub_diag(i0)/(fG+fR); + + real_type fSin = value_traits::one(); + real_type fCos = value_traits::one(); + real_type fP = value_traits::zero(); + + for (int i3 = i2-1; i3 >= i0; --i3) + { + real_type fF = fSin*sub_diag(i3); + real_type fB = fCos*sub_diag(i3); + if ( fabs(fF) >= fabs(fG) ) + { + fCos = fG/fF; + fR = sqrt(fCos*fCos+value_traits::one()); + sub_diag(i3+1) = fF*fR; + fSin = value_traits::one()/fR; + fCos *= fSin; + } + else + { + fSin = fF/fG; + fR = sqrt(fSin*fSin+value_traits::one()); + sub_diag(i3+1) = fG*fR; + fCos = value_traits::one()/fR; + fSin *= fCos; + } + fG = diag(i3+1)-fP; + fR = (diag(i3)-fG)*fSin+value_traits::two()*fB*fCos; + fP = fSin*fR; + diag(i3+1) = fG+fP; + fG = fCos*fR-fB; + for (int i4 = 0; i4 < dim; ++i4) + { + fF = V(i4,i3+1); + V(i4,i3+1) = fSin*V(i4,i3)+fCos*fF; + V(i4,i3) = fCos*V(i4,i3)-fSin*fF; + } + } + diag(i0) -= fP; + sub_diag(i0) = fG; + sub_diag(i2) = value_traits::zero(); + } + if ( i1 == max_iterations ) + break; + } + } + + } // namespace math + +} // namespace OpenTissue + +// OPENTISSUE_CORE_MATH_MATH_EIGEN_SYSTEM_DECOMPOSITION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_functions.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_functions.h new file mode 100644 index 000000000..04f19736a --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_functions.h @@ -0,0 +1,158 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_FUNCTIONS_H +#define OPENTISSUE_CORE_MATH_MATH_FUNCTIONS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + + + +namespace OpenTissue +{ + + /** + * The following functions are defined below: + * - clamp (and the variations: clamp_min, clamp_max, clamp_zero_one) + * - fac (faculty) + * - sgn (sign function) + * - sinc + */ + namespace math + { + + /** + * clamp function to clamp a value to be in the interval (min_value; max_value). + * + * @param value The value to be clamped. + * @param min_value The minimum allowed value. + * @param max_value The maximum allowed value (ohhh, really?). + * @return The clamped value. If value already is in (min_value; max_value) no clamping is performed. + */ + template + inline T clamp(T const & value, T const & min_value, T const & max_value) + { + assert(min_value <= max_value || !"max_value cannot be less than min_value"); + using std::min; + using std::max; + + return T(min(max_value, max(min_value, value))); + } + + + /** + * clamp function to clamp a value never to be less than min_value. + * + * @param value The value to be clamped. + * @param min_value The minimum allowed value. + * @return The clamped value if value is less than min_value, otherwise the original value is returned. + */ + template + inline T clamp_min(T const & value, T const & min_value) + { + using std::max; + return clamp(value, min_value, max(value, min_value)); + } + + + /** + * clamp function to clamp a value never to be greater than max_value. + * + * @param value The value to be clamped. + * @param max_value The maximum allowed value. + * @return The clamped value if value is greater than max_value, otherwise the original value is returned. + */ + template + inline T clamp_max(T const & value, T const & max_value) + { + using std::min; + return clamp(value, min(value, max_value), max_value); + } + + + /** + * clamp function to easily clamp a value between 0 and 1. + * note: this function is mostly usable for T = some real type. + * + * @param value The value to be clamped. + * @return The clamped value. If value already is in (0; 1) no clamping is performed. + */ + template + inline T clamp_zero_one(T const & value) + { + return clamp(value, detail::zero(), detail::one()); + } + + + template + inline T fac(unsigned long n) + { + // TODO what about implicit type conversions? This could have been done more elegangtly using partial specialization + unsigned long val = 1; + for(; n > 0; val *= n--); + return T(val); + } + + + template + inline T sgn(T const & val) + { + static T const zero = detail::zero(); + static T const one = detail::one(); + return val > zero ? one : val < zero ? -one : zero; + } + + + /** + * Compute Sinc Function. + * The implementation of this method was greatly inspired by the + * one in Open Dynamics Engine v. 0.039 + * + * This method returns sin(x)/x. this has a singularity at 0 so special + * handling is needed for small arguments. + * + * @param x + * @return The value of sin(x)/x + */ + template + inline T sinc(T & x) + { + using std::fabs; + using std::sin; + + static T const tiny = (T)(1.0e-4); + static T const factor = (T)(0.166666666666666666667); + + //--- if |x| < 1e-4 then use a taylor series expansion. this two term expansion + //--- is actually accurate to one LS bit within this range if double precision + //--- is being used - so don't worry! + return (fabs(x) < tiny) ? (detail::one() - x*x*factor) : (sin(x)/x); + } + + + // use this to convert radians into degrees + template + inline T to_degrees(T const & radians) + { + return radians*detail::radian(); + } + + + // use this to convert degrees into radians + template + inline T to_radians(T const & degrees) + { + return degrees*detail::degree(); + } + + } // namespace math + +} // namespace OpenTissue + +// #define OPENTISSUE_CORE_MATH_MATH_FUNCTIONS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_invert4x4.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_invert4x4.h new file mode 100644 index 000000000..0e738399f --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_invert4x4.h @@ -0,0 +1,65 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_INVERT4x4_H +#define OPENTISSUE_CORE_MATH_MATH_INVERT4x4_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + + +namespace OpenTissue +{ + + namespace math + { + + template + inline void invert4x4(real_type M[4][4]) + { + real_type a00 = M[0][0]; real_type a01 = M[0][1]; real_type a02 = M[0][2]; real_type a03 = M[0][3]; + real_type a10 = M[1][0]; real_type a11 = M[1][1]; real_type a12 = M[1][2]; real_type a13 = M[1][3]; + real_type a20 = M[2][0]; real_type a21 = M[2][1]; real_type a22 = M[2][2]; real_type a23 = M[2][3]; + real_type a30 = M[3][0]; real_type a31 = M[3][1]; real_type a32 = M[3][2]; real_type a33 = M[3][3]; + + M[0][0] = a11*a22*a33 - a11*a23*a32 - a21*a12*a33 + a21*a13*a32 + a31*a12*a23 - a31*a13*a22; + M[0][1] = - a01*a22*a33 + a01*a23*a32 + a21*a02*a33 - a21*a03*a32 - a31*a02*a23 + a31*a03*a22; + M[0][2] = a01*a12*a33 - a01*a13*a32 - a11*a02*a33 + a11*a03*a32 + a31*a02*a13 - a31*a03*a12; + M[0][3] = - a01*a12*a23 + a01*a13*a22 + a11*a02*a23 - a11*a03*a22 - a21*a02*a13 + a21*a03*a12; + M[1][0] = - a10*a22*a33 + a10*a23*a32 + a20*a12*a33 - a20*a13*a32 - a30*a12*a23 + a30*a13*a22; + M[1][1] = a00*a22*a33 - a00*a23*a32 - a20*a02*a33 + a20*a03*a32 + a30*a02*a23 - a30*a03*a22; + M[1][2] = - a00*a12*a33 + a00*a13*a32 + a10*a02*a33 - a10*a03*a32 - a30*a02*a13 + a30*a03*a12; + M[1][3] = a00*a12*a23 - a00*a13*a22 - a10*a02*a23 + a10*a03*a22 + a20*a02*a13 - a20*a03*a12; + M[2][0] = a10*a21*a33 - a10*a23*a31 - a20*a11*a33 + a20*a13*a31 + a30*a11*a23 - a30*a13*a21; + M[2][1] = - a00*a21*a33 + a00*a23*a31 + a20*a01*a33 - a20*a03*a31 - a30*a01*a23 + a30*a03*a21; + M[2][2] = a00*a11*a33 - a00*a13*a31 - a10*a01*a33 + a10*a03*a31 + a30*a01*a13 - a30*a03*a11; + M[2][3] = - a00*a11*a23 + a00*a13*a21 + a10*a01*a23 - a10*a03*a21 - a20*a01*a13 + a20*a03*a11; + M[3][0] = - a10*a21*a32 + a10*a22*a31 + a20*a11*a32 - a20*a12*a31 - a30*a11*a22 + a30*a12*a21; + M[3][1] = a00*a21*a32 - a00*a22*a31 - a20*a01*a32 + a20*a02*a31 + a30*a01*a22 - a30*a02*a21; + M[3][2] = - a00*a11*a32 + a00*a12*a31 + a10*a01*a32 - a10*a02*a31 - a30*a01*a12 + a30*a02*a11; + M[3][3] = a00*a11*a22 - a00*a12*a21 - a10*a01*a22 + a10*a02*a21 + a20*a01*a12 - a20*a02*a11; + //real_type D = + // ( + // a00*a11*a22*a33 - a00*a11*a23*a32 - a00*a21*a12*a33 + a00*a21*a13*a32 + a00*a31*a12*a23 - a00*a31*a13*a22 + // - a10*a01*a22*a33 + a10*a01*a23*a32 + a10*a21*a02*a33 - a10*a21*a03*a32 - a10*a31*a02*a23 + a10*a31*a03*a22 + // + a20*a01*a12*a33 - a20*a01*a13*a32 - a20*a11*a02*a33 + a20*a11*a03*a32 + a20*a31*a02*a13 - a20*a31*a03*a12 + // - a30*a01*a12*a23 + a30*a01*a13*a22 + a30*a11*a02*a23 - a30*a11*a03*a22 - a30*a21*a02*a13 + a30*a21*a03*a12 + // ); + real_type D = a00*M[0][0] + a10*M[0][1] + a20*M[0][2] + a30*M[0][3]; + if(D) + { + M[0][0] /=D; M[0][1] /=D; M[0][2] /=D; M[0][3] /=D; + M[1][0] /=D; M[1][1] /=D; M[1][2] /=D; M[1][3] /=D; + M[2][0] /=D; M[2][1] /=D; M[2][2] /=D; M[2][3] /=D; + M[3][0] /=D; M[3][1] /=D; M[3][2] /=D; M[3][3] /=D; + } + } + + } // namespace math + +} // namespace OpenTissue + +//OPENTISSUE_CORE_MATH_MATH_INVERT4x4_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_is_finite.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_is_finite.h new file mode 100644 index 000000000..b245f6f9c --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_is_finite.h @@ -0,0 +1,32 @@ +#ifndef OPENTISSUE_CORE_MATH_IS_FINITE_H +#define OPENTISSUE_CORE_MATH_IS_FINITE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#ifdef WIN32 +#include +#endif + +namespace OpenTissue +{ + namespace math + { + +#ifdef WIN32 +#define is_finite(val) (_finite(val)!=0) ///< Is finite number test +#else +#define is_finite(val) (finite(val)!=0) ///< Is finite number test +#endif + + } // namespace math + +} // namespace OpenTissue + +//OPENTISSUE_CORE_MATH_IS_FINITE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_is_number.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_is_number.h new file mode 100644 index 000000000..40e315803 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_is_number.h @@ -0,0 +1,38 @@ +#ifndef OPENTISSUE_CORE_MATH_IS_NUMBER_H +#define OPENTISSUE_CORE_MATH_IS_NUMBER_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#ifdef WIN32 +#include +#else +#include +#endif + +namespace OpenTissue +{ + namespace math + { + +#ifdef WIN32 +#define is_number(val) (_isnan(val)==0) ///< Is a number test +#else +#if (__APPLE__) +#define is_number(val) (std::isnan(val)==0) ///< Is a number test +#else +#define is_number(val) (isnan(val)==0) ///< Is a number test +#endif +#endif + + } // namespace math + +} // namespace OpenTissue + +//OPENTISSUE_CORE_MATH_IS_NUMBER_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_matrix3x3.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_matrix3x3.h new file mode 100644 index 000000000..aa811024d --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_matrix3x3.h @@ -0,0 +1,788 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_MATRIX3X3_H +#define OPENTISSUE_CORE_MATH_MATH_MATRIX3X3_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include + +#include +#include +#include + + +namespace OpenTissue +{ + + namespace math + { + + template class Quaternion; + + template< + typename value_type_ + //, typename value_traits_ = ValueTraits + > + class Matrix3x3 + { + protected: + + typedef typename math::ValueTraits value_traits_ ; // TODO value_traits_ should be parameterized as a class template parameter. + + public: + + typedef value_traits_ value_traits; ///< Convience typedef to make value traits accesible for all template functions using Vector3 types. + typedef value_type_ value_type; ///< Typedef is required for compliance with many other libraries and data containers! + typedef Vector3 vector3_type; // TODO should Vector3 type be template parameterized? + typedef Quaternion quaternion_type; + typedef size_t index_type; // TODO should the index type be template parameterized? + + protected: + + // TODO 2007-02-19 KE: Refactor this stuff into an array + vector3_type m_row0; ///< The 1st row of the matrix + vector3_type m_row1; ///< The 2nd row of the matrix + vector3_type m_row2; ///< The 3rd row of the matrix + + public: + + Matrix3x3() + : m_row0(value_traits::zero() ,value_traits::zero() ,value_traits::zero() ) + , m_row1(value_traits::zero() ,value_traits::zero() ,value_traits::zero() ) + , m_row2(value_traits::zero() ,value_traits::zero() ,value_traits::zero() ) + {} + + ~Matrix3x3(){} + + explicit Matrix3x3( + value_type const & m00 , value_type const & m01 , value_type const & m02 + , value_type const & m10 , value_type const & m11 , value_type const & m12 + , value_type const & m20 , value_type const & m21 , value_type const & m22 + ) + : m_row0(m00,m01,m02) + , m_row1(m10,m11,m12) + , m_row2(m20,m21,m22) + {} + + + explicit Matrix3x3(vector3_type const & row0, vector3_type const & row1, vector3_type const & row2) + : m_row0(row0) + , m_row1(row1) + , m_row2(row2) + {} + + explicit Matrix3x3(quaternion_type const & q) { *this = q; } + + Matrix3x3(Matrix3x3 const & M) + : m_row0(M.m_row0) + , m_row1(M.m_row1) + , m_row2(M.m_row2) + {} + + public: + + value_type & operator()(index_type i, index_type j) + { + assert( ( i>=0 && i<3 ) || !"Matrix3x3::(i,j) i must be in range [0..2]"); + assert( ( j>=0 && j<3 ) || !"Matrix3x3::(i,j) j must be in range [0..2]"); + return (*(&m_row0+i))(j); + } + + value_type const & operator()(index_type i, index_type j) const + { + assert( ( i>=0 && i<3 ) || !"Matrix3x3::(i,j) i must be in range [0..2]"); + assert( ( j>=0 && j<3 ) || !"Matrix3x3::(i,j) j must be in range [0..2]"); + return (*(&m_row0+i))(j); + } + + vector3_type & operator[](index_type i) + { + assert( ( i>=0 && i<3 ) || !"Matrix3x3::(i,j) i must be in range [0..2]"); + return *(&m_row0+i); + } + + vector3_type const & operator[](index_type i) const + { + assert( ( i>=0 && i<3 ) || !"Matrix3x3::(i,j) i must be in range [0..2]"); + return *(&m_row0+i); + } + + Matrix3x3 & operator=( Matrix3x3 const & cpy ) + { + m_row0=cpy.m_row0; + m_row1=cpy.m_row1; + m_row2=cpy.m_row2; + return *this; + } + + // TODO: Comparing floats with == or != is not safe NOTE T might not be a float type it could be anything? This suggest that we need some kinf of metaprogramming technique to deal with ths problem? + bool operator==(Matrix3x3 const & cmp ) const + { + return (m_row0==cmp.m_row0) && (m_row1==cmp.m_row1) && (m_row2==cmp.m_row2); + } + + bool operator!=( Matrix3x3 const & cmp ) const + { + return !(*this==cmp); + } + + Matrix3x3 operator+ ( Matrix3x3 const & m ) const { return Matrix3x3( m_row0+m.m_row0, m_row1+m.m_row1, m_row2+m.m_row2); } + Matrix3x3 operator- ( Matrix3x3 const & m ) const { return Matrix3x3( m_row0-m.m_row0, m_row1-m.m_row1, m_row2-m.m_row2); } + Matrix3x3 & operator+= ( Matrix3x3 const & m ) { m_row0+=m.m_row0; m_row1+=m.m_row1; m_row2+=m.m_row2; return *this; } + Matrix3x3 & operator-= ( Matrix3x3 const & m ) { m_row0-=m.m_row0; m_row1-=m.m_row1; m_row2-=m.m_row2; return *this; } + Matrix3x3 & operator*= ( value_type const & s ) {m_row0*=s;m_row1*=s;m_row2*=s;return *this;} + + Matrix3x3 & operator/= ( value_type const & s ) + { + assert(s || !"Matrix3x3/=(): division by zero"); + m_row0/=s; + m_row1/=s; + m_row2/=s; + return *this; + } + + vector3_type operator*(vector3_type const &v) const { return vector3_type(m_row0*v, m_row1*v, m_row2*v); } + Matrix3x3 operator-() const { return Matrix3x3(-m_row0,-m_row1,-m_row2); } + + size_t size1() const { return 3u; } + size_t size2() const { return 3u; } + + /** + * Assigns this quaternion to a matrix. + * This method performs a conversion of a quaternion that represents a rotation into the correponding rotationmatrix. + * + * @param q A reference to a quaternion. This is the quaternion that represents a rotation. + */ + Matrix3x3 & operator=(quaternion_type const & q) + { + m_row0(0) = value_traits::one() - value_traits::two() * ( (q.v()(1)*q.v()(1)) + (q.v()(2)*q.v()(2))); + m_row1(1) = value_traits::one() - value_traits::two() * ( (q.v()(0)*q.v()(0)) + (q.v()(2)*q.v()(2))); + m_row2(2) = value_traits::one() - value_traits::two() * ( (q.v()(1)*q.v()(1)) + (q.v()(0)*q.v()(0))); + m_row1(0) = value_traits::two() * ( (q.v()(0)*q.v()(1)) + (q.s()*q.v()(2))); + m_row0(1) = value_traits::two() * ( (q.v()(0)*q.v()(1)) - (q.s()*q.v()(2))); + m_row2(0) = value_traits::two() * (-(q.s()*q.v()(1)) + (q.v()(0)*q.v()(2))); + m_row0(2) = value_traits::two() * ( (q.s()*q.v()(1)) + (q.v()(0)*q.v()(2))); + m_row2(1) = value_traits::two() * ( (q.v()(2)*q.v()(1)) + (q.s()*q.v()(0))); + m_row1(2) = value_traits::two() * ( (q.v()(2)*q.v()(1)) - (q.s()*q.v()(0))); + return *this; + } + + void clear() + { + m_row0.clear(); + m_row1.clear(); + m_row2.clear(); + } + + public: + + friend Matrix3x3 fabs(Matrix3x3 const & A) + { + using std::fabs; + + return Matrix3x3( + fabs(A(0,0)), fabs(A(0,1)), fabs(A(0,2)), + fabs(A(1,0)), fabs(A(1,1)), fabs(A(1,2)), + fabs(A(2,0)), fabs(A(2,1)), fabs(A(2,2)) + ); + } + + public: + + vector3_type column(index_type i) const + { + assert((i>=0 && i<3) || !"Matrix3x3::column(): index must be in range [0..2]"); + + return vector3_type(m_row0(i), m_row1(i), m_row2(i)); + } + + void set_column(index_type i, vector3_type const & column) + { + assert((i>=0 && i<3) || !"Matrix3x3::set_column(): index must be in range [0..2]"); + + m_row0(i) = column(0); + m_row1(i) = column(1); + m_row2(i) = column(2); + } + + vector3_type & row(index_type i) { return (*this)[i]; } + vector3_type const & row(index_type i) const { return (*this)[i]; } + + }; // class Matrix3x3 + + + template + inline Matrix3x3 operator*( Matrix3x3 const & m, T const &s ) + { + return Matrix3x3(m.row(0)*s, m.row(1)*s, m.row(2)*s); + } + + template + inline Matrix3x3 operator*( T const & s, Matrix3x3 const & m ) + { + return Matrix3x3(m.row(0)*s, m.row(1)*s, m.row(2)*s); + } + + template + inline Matrix3x3 operator/( Matrix3x3 const & m, T const & s ) + { + return Matrix3x3(m.row(0)/s, m.row(1)/s, m.row(2)/s); + } + + template + inline Matrix3x3 operator/( T const & s, Matrix3x3 const & m ) + { + return Matrix3x3(m.row(0)/s, m.row(1)/s, m.row(2)/s); + } + + template + inline Matrix3x3 operator*(Matrix3x3 const & A,Matrix3x3 const & B) + { + typedef typename Matrix3x3::value_traits value_traits; + Matrix3x3 C; + + for (int c=0; c<3; ++c) + for (int r=0; r<3; ++r){ + C(r,c) = value_traits::zero(); + for(int i=0; i<3; ++i){ + C(r,c) += A(r,i) * B(i,c); + } + } + return C; + } + + /** + * Creates a rotation matrix. + * This method returns a rotation matrix around the x-axe. It + * assumes that post-multiplication by colum vectors is used. + * + * @param radians The rotation angle in radians. + */ + template + inline Matrix3x3 Rx(T const & radians) + { + typedef typename Matrix3x3::value_traits value_traits; + + using std::cos; + using std::sin; + + T cosinus = (T)( cos(radians) ); + T sinus = (T)( sin(radians) ); + + return Matrix3x3( + value_traits::one(), value_traits::zero(), value_traits::zero(), + value_traits::zero(), cosinus, -sinus, + value_traits::zero(), sinus, cosinus + ); + } + + /** + * Creates a rotation matrix. + * This method returns a rotation matrix around the y-axe. It + * assumes that post-multiplication by colum vectors is used. + * + * @param radians The rotation angle in radians. + */ + template + inline Matrix3x3 Ry(T const & radians) + { + typedef typename Matrix3x3::value_traits value_traits; + + using std::cos; + using std::sin; + + T cosinus = (T)( cos(radians) ); + T sinus = (T)( sin(radians) ); + return Matrix3x3( + cosinus, value_traits::zero(), sinus, + value_traits::zero(), value_traits::one(), value_traits::zero(), + -sinus, value_traits::zero(), cosinus + ); + } + + /** + * Creates a rotation matrix. + * This method returns a rotation matrix around the z-axe. It assumes that post-multiplication by colum vectors is used. + * + * @param radians The rotation angle in radians. + */ + template + inline Matrix3x3 Rz(T const & radians) + { + typedef typename Matrix3x3::value_traits value_traits; + + using std::cos; + using std::sin; + + T cosinus = (T)( cos(radians) ); + T sinus = (T)( sin(radians) ); + + return Matrix3x3( + cosinus, -sinus, value_traits::zero(), + sinus, cosinus, value_traits::zero(), + value_traits::zero(), value_traits::zero(), value_traits::one() + ); + } + + /** + * Creates a rotation matrix. + * This method returns a general rotation matrix around a specified axe. It assumes that post-multiplication by colum vectors is used. + * + * @param radians The rotation angle in radians. + * @param axe A vector. This is the rotation axe. + */ + template + inline Matrix3x3 Ru(T const & radians, Vector3 const & axis) + { + typedef typename Matrix3x3::value_traits value_traits; + + using std::cos; + using std::sin; + + T cosinus = (T)( cos(radians) ); + T sinus = (T)( sin(radians) ); + Vector3 u = unit(axis); + + //Foley p.227 (5.76) + return Matrix3x3( + u(0)*u(0) + cosinus*(value_traits::one() - u(0)*u(0)), u(0)*u(1)*(value_traits::one()-cosinus) - sinus*u(2), u(0)*u(2)*(value_traits::one()-cosinus) + sinus*u(1), + u(0)*u(1)*(value_traits::one()-cosinus) + sinus*u(2), u(1)*u(1) + cosinus*(value_traits::one() - u(1)*u(1)), u(1)*u(2)*(value_traits::one()-cosinus) - sinus*u(0), + u(0)*u(2)*(value_traits::one()-cosinus) - sinus*u(1), u(1)*u(2)*(value_traits::one()-cosinus) + sinus*u(0), u(2)*u(2) + cosinus*(value_traits::one() - u(2)*u(2)) + ); + } + + /** + * Direction of Flight (DoF) + * + * @param k The desired direction of flight. + */ + template + inline Matrix3x3 z_dof(Vector3 const & k) + { + Vector3 i,j; + orthonormal_vectors(i,j,unit(k)); + return Matrix3x3( + i(0) , j(0), k(0) + , i(1) , j(1), k(1) + , i(2) , j(2), k(2) + ); + } + + template + inline bool is_orthonormal(Matrix3x3 const & M,T const & threshold) + { + typedef typename Matrix3x3::value_traits value_traits; + + using std::fabs; + + assert(threshold>=value_traits::zero() || !"is_orthonormal(): threshold must be non-negative"); + + T dot01 = M.m_row0*M.m_row1; + T dot02 = M.m_row0*M.m_row2; + T dot12 = M.m_row1*M.m_row2; + if(fabs(dot01)>threshold) return false; + if(fabs(dot02)>threshold) return false; + if(fabs(dot12)>threshold) return false; + T dot00 = M.m_row0*M.m_row0; + T dot11 = M.m_row1*M.m_row1; + T dot22 = M.m_row2*M.m_row2; + if((dot00-value_traits::one())>threshold) return false; + if((dot11-value_traits::one())>threshold) return false; + if((dot22-value_traits::one())>threshold) return false; + return true; + } + + template + inline bool is_orthonormal(Matrix3x3 const & M) + { + typedef typename Matrix3x3::value_traits value_traits; + + return is_orthonormal(M, value_traits::zero()); + } + + template + inline bool is_zero(Matrix3x3 M, T const & threshold) + { + typedef typename Matrix3x3::value_traits value_traits; + + using std::fabs; + + assert(threshold>=value_traits::zero() || !"is_zero(): threshold must be non-negative"); + + if(fabs(M(0,0))>threshold) return false; + if(fabs(M(0,1))>threshold) return false; + if(fabs(M(0,2))>threshold) return false; + if(fabs(M(1,0))>threshold) return false; + if(fabs(M(1,1))>threshold) return false; + if(fabs(M(1,2))>threshold) return false; + if(fabs(M(2,0))>threshold) return false; + if(fabs(M(2,1))>threshold) return false; + if(fabs(M(2,2))>threshold) return false; + return true; + } + + template + inline bool is_zero(Matrix3x3 const & M) + { + typedef typename Matrix3x3::value_traits value_traits; + + return is_zero(M, value_traits::zero()); + } + + template + inline bool is_symmetric(Matrix3x3 M, T const & threshold) + { + typedef typename Matrix3x3::value_traits value_traits; + + using std::fabs; + + assert(threshold>=value_traits::zero() || !"is_symmetric(): threshold must be non-negative"); + + if(fabs(M(0,1)-M(1,0))>threshold) return false; + if(fabs(M(0,2)-M(2,0))>threshold) return false; + if(fabs(M(1,2)-M(2,1))>threshold) return false; + return true; + } + + template + inline bool is_symmetric(Matrix3x3 const & M) + { + typedef typename Matrix3x3::value_traits value_traits; + + return is_symmetric(M, value_traits::zero()); + } + + template + inline bool is_diagonal(Matrix3x3 M, T const & threshold) + { + typedef typename Matrix3x3::value_traits value_traits; + + using std::fabs; + + assert(threshold>=value_traits::zero() || !"is_diagonal(): threshold must be non-negative"); + + if(fabs(M(0,1))>threshold) return false; + if(fabs(M(0,2))>threshold) return false; + if(fabs(M(1,0))>threshold) return false; + if(fabs(M(1,2))>threshold) return false; + if(fabs(M(2,0))>threshold) return false; + if(fabs(M(2,1))>threshold) return false; + return true; + } + + template + inline bool is_diagonal(Matrix3x3 const & M) + { + typedef typename Matrix3x3::value_traits value_traits; + + return is_diagonal(M, value_traits::zero()); + } + + template + inline bool is_identity(Matrix3x3 M, T const & threshold) + { + typedef typename Matrix3x3::value_traits value_traits; + + using std::fabs; + + assert(threshold>=value_traits::zero() || !"is_identity(): threshold must be non-negative"); + + if(fabs(M(0,0)-value_traits::one())>threshold) return false; + if(fabs(M(0,1))>threshold) return false; + if(fabs(M(0,2))>threshold) return false; + if(fabs(M(1,0))>threshold) return false; + if(fabs(M(1,1)-value_traits::one())>threshold) return false; + if(fabs(M(1,2))>threshold) return false; + if(fabs(M(2,0))>threshold) return false; + if(fabs(M(2,1))>threshold) return false; + if(fabs(M(2,2)-value_traits::one())>threshold) return false; + return true; + } + + template + inline bool is_identity(Matrix3x3 const & M) + { + typedef typename Matrix3x3::value_traits value_traits; + + return is_identity(M, value_traits::zero()); + } + + template + inline Matrix3x3 outer_prod( Vector3 const & v1, Vector3 const & v2 ) + { + return Matrix3x3( + ( v1( 0 ) * v2( 0 ) ), ( v1( 0 ) * v2( 1 ) ), ( v1( 0 ) * v2( 2 ) ), + ( v1( 1 ) * v2( 0 ) ), ( v1( 1 ) * v2( 1 ) ), ( v1( 1 ) * v2( 2 ) ), + ( v1( 2 ) * v2( 0 ) ), ( v1( 2 ) * v2( 1 ) ), ( v1( 2 ) * v2( 2 ) ) + ); + } + + template + inline Matrix3x3 ortonormalize(Matrix3x3 const & A) + { + typedef typename Matrix3x3::vector3_type vector3_type; + + vector3_type row0(A(0,0),A(0,1),A(0,2)); + vector3_type row1(A(1,0),A(1,1),A(1,2)); + vector3_type row2(A(2,0),A(2,1),A(2,2)); + + T const l0 = length(row0); + if(l0) row0 /= l0; + + row1 -= row0 * dot(row0 , row1); + T const l1 = length(row1); + if(l1) row1 /= l1; + + row2 = cross( row0 , row1); + + return Matrix3x3( + row0(0), row0(1), row0(2), + row1(0), row1(1), row1(2), + row2(0), row2(1), row2(2) + ); + } + + template + inline Matrix3x3 trans(Matrix3x3 const & M) + { + return Matrix3x3( + M(0,0), M(1,0), M(2,0), + M(0,1), M(1,1), M(2,1), + M(0,2), M(1,2), M(2,2) + ); + } + + + template + inline Matrix3x3 diag(T const & d0,T const & d1,T const & d2 ) + { + typedef typename Matrix3x3::value_traits value_traits; + + return Matrix3x3( + d0, value_traits::zero(), value_traits::zero(), + value_traits::zero(), d1, value_traits::zero(), + value_traits::zero(), value_traits::zero(), d2 + ); + } + + template + inline Matrix3x3 diag(Vector3 const & d) { return diag( d(0), d(1), d(2)); } + + template + inline Matrix3x3 diag(T const & d ) { return diag(d,d,d); } + + template + inline Matrix3x3 inverse(Matrix3x3 const & A) + { + typedef typename Matrix3x3::value_traits value_traits; + + //From messer p.283 we know + // + // -1 1 + // A = ----- adj A + // det A + // + // i+j + // ij-cofactor of A = -1 det A + // ij + // + // i,j entry of the adjoint. + // i+j + // adjoint A = ji-cofactor = A = -1 det A + // ij ji + // + // As it can be seen the only numerical error + // in these calculations is from the resolution + // of the scalars. So it is a very accurate method. + // + Matrix3x3 adj; + adj(0,0) = A(1,1)*A(2,2) - A(2,1)*A(1,2); + adj(1,1) = A(0,0)*A(2,2) - A(2,0)*A(0,2); + adj(2,2) = A(0,0)*A(1,1) - A(1,0)*A(0,1); + adj(0,1) = A(1,0)*A(2,2) - A(2,0)*A(1,2); + adj(0,2) = A(1,0)*A(2,1) - A(2,0)*A(1,1); + adj(1,0) = A(0,1)*A(2,2) - A(2,1)*A(0,2); + adj(1,2) = A(0,0)*A(2,1) - A(2,0)*A(0,1); + adj(2,0) = A(0,1)*A(1,2) - A(1,1)*A(0,2); + adj(2,1) = A(0,0)*A(1,2) - A(1,0)*A(0,2); + T det = A(0,0)*adj(0,0) - A(0,1)*adj(0,1) + A(0,2)*adj(0,2); + if(det) + { + adj(0,1) = -adj(0,1); + adj(1,0) = -adj(1,0); + adj(1,2) = -adj(1,2); + adj(2,1) = -adj(2,1); + return trans(adj)/det; + } + + return diag(value_traits::one()); + } + + template + inline T max_value(Matrix3x3 const & A) + { + using std::max; + + return max(A(0,0), max( A(0,1), max( A(0,2), max ( A(1,0), max ( A(1,1), max ( A(1,2), max (A(2,0), max ( A(2,1) , A(2,2) ) ) ) ) ) ) ) ); + } + + template + inline T min_value(Matrix3x3 const & A) + { + using std::min; + + return min(A(0,0), min( A(0,1), min( A(0,2), min ( A(1,0), min ( A(1,1), min ( A(1,2), min (A(2,0), min ( A(2,1) , A(2,2) ) ) ) ) ) ) ) ); + } + + template + inline T det(Matrix3x3 const & A) + { + return A(0,0)*(A(1,1)*A(2,2) - A(2,1)*A(1,2)) - A(0,1)*(A(1,0)*A(2,2) - A(2,0)*A(1,2)) + A(0,2)*(A(1,0)*A(2,1) - A(2,0)*A(1,1)); + } + + template + inline T trace(Matrix3x3 const & A) + { + return (A(0,0) + A(1,1) + A(2,2)); + } + + template + inline T norm_1(Matrix3x3 const & A) + { + using std::fabs; + using std::max; + + T r0 = fabs(A(0,0)) + fabs(A(0,1)) + fabs(A(0,2)); + T r1 = fabs(A(1,0)) + fabs(A(1,1)) + fabs(A(1,2)); + T r2 = fabs(A(2,0)) + fabs(A(2,1)) + fabs(A(2,2)); + + return max ( r0, max( r1, r2 ) ); + } + + template + inline T norm_2(Matrix3x3 const & A) + { + using std::fabs; + using std::max; + using std::sqrt; + + Matrix3x3 V; + typename Matrix3x3::vector3_type d; + math::eigen(A,V,d); + + T lambda = max( fabs(d(0)), max( fabs(d(1)) , fabs(d(2)) )); + return sqrt( lambda ); + } + + template + inline T norm_inf(Matrix3x3 const & A) + { + using std::fabs; + using std::max; + + T c0 = fabs(A(0,0)) + fabs(A(1,0)) + fabs(A(2,0)); + T c1 = fabs(A(0,1)) + fabs(A(1,1)) + fabs(A(2,1)); + T c2 = fabs(A(0,2)) + fabs(A(1,2)) + fabs(A(2,2)); + return max ( c0, max( c1, c2 ) ); + } + + template + inline Matrix3x3 truncate(Matrix3x3 const & A,T const & epsilon) + { + typedef typename Matrix3x3::value_traits value_traits; + + using std::fabs; + + assert(epsilon>value_traits::zero() || !"truncate(Matrix3x3,epsilon): epsilon must be positive"); + + return Matrix3x3( + fabs(A(0,0)) + inline Matrix3x3 star(Vector3 const & v) + { + typedef typename Matrix3x3::value_traits value_traits; + + //--- Changes a cross-product into a matrix multiplication. + //--- Rewrites the component of a vector3_type cross-product as a matrix. + //--- a x b = a*b = ba* + return Matrix3x3( + value_traits::zero(), -v(2), v(1), + v(2), value_traits::zero(), -v(0), + -v(1), v(0), value_traits::zero() + ); + } + + template + inline std::ostream & operator<< (std::ostream & o,Matrix3x3 const & A) + { + o << "[" << A(0,0) + << "," << A(0,1) + << "," << A(0,2) + << ";" << A(1,0) + << "," << A(1,1) + << "," << A(1,2) + << ";" << A(2,0) + << "," << A(2,1) + << "," << A(2,2) + << "]"; + return o; + } + + template + inline std::istream & operator>>(std::istream & i,Matrix3x3 & A) + { + char dummy; + i >> dummy; + i >> A(0,0); + i >> dummy; + i >> A(0,1); + i >> dummy; + i >> A(0,2); + i >> dummy; + i >> A(1,0); + i >> dummy; + i >> A(1,1); + i >> dummy; + i >> A(1,2); + i >> dummy; + i >> A(2,0); + i >> dummy; + i >> A(2,1); + i >> dummy; + i >> A(2,2); + i >> dummy; + return i; + } + + } // namespace math + +} // namespace OpenTissue + +//OPENTISSUE_CORE_MATH_MATH_MATRIX3X3_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_polar_decomposition.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_polar_decomposition.h new file mode 100644 index 000000000..4cec1a7cd --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_polar_decomposition.h @@ -0,0 +1,149 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_POLAR_DECOMPOSITION_H +#define OPENTISSUE_CORE_MATH_MATH_POLAR_DECOMPOSITION_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include + +#include + + +namespace OpenTissue +{ + + namespace math + { + + // TODO: [micky] don't introduce yet another sub namespace to avoid name clashing. + // Rename the function 'eigen' instead!! + namespace polar_decomposition + { + /* + * Polar Decomposition of matrix A (as described by Etzmuss et. al in ``A Fast Finite Solution for Cloth Modelling'') + * + * A = R S + * + * Where R is a special orthogonal matrix (R*R^T = I and det(R)=1) + * + * S^2 = A^T A + * + * let d be vector of eigenvalues and let v_0,v_1, and v_2 be corresponding eigenvectors of (A^T A), then + * + * S = sqrt(d_0) v_0 * v_0^T + ... + sqrt(d_2) v_2 * v_2^T + * + * Now compute + * + * R = A * S^-1 + * + * + * @return If the decompostion is succesfull then the return value is true otherwise it is false. + */ + template + inline bool eigen(matrix3x3_type const & A,matrix3x3_type & R,matrix3x3_type & S) + { + using std::sqrt; + + // A = U D V^T // A is square so: thin SVD = SVD + // A = (U V^T) (V D V^T) + // = R S + // + //Notice S is symmetric R should be orthonormal? + // + // proof of + // S = sqrt( A^T A ) + // + // start by + // + // S * S = A^T A + // V D V^T V D V^T = V D U^T U D V^T + // V D D V^T = V D D V^T + //Assume + //A = R S + //pre-multiply and use assumption then + // A^T A = A^T R S + // A^T A = S^T S = S S + // last step used S is symmetric + typedef typename matrix3x3_type::vector3_type vector3_type; + typedef typename matrix3x3_type::value_traits value_traits; + matrix3x3_type V; + vector3_type d; + matrix3x3_type S2 = trans(A)*A; + eigen(S2,V,d); + + //--- Test if all eigenvalues are positive + if( d(0) <= value_traits::zero() || d(1) <= value_traits::zero() || d(2) <= value_traits::zero() ) + return false; + + vector3_type v0 = vector3_type( V(0,0), V(1,0), V(2,0) ); + vector3_type v1 = vector3_type( V(0,1), V(1,1), V(2,1) ); + vector3_type v2 = vector3_type( V(0,2), V(1,2), V(2,2) ); + S = outer_prod(v0,v0)* sqrt(d(0)) + outer_prod(v1,v1)* sqrt(d(1)) + outer_prod(v2,v2)* sqrt(d(2)); + R = A * inverse(S); + return true; + } + + /** + * This method is rather bad, it is iterative and there is no way of telling how good the solution is. thus we recommend the eigen method for polar decomposition. + * + * Polar Decomposition as described by Shoemake and Duff in ``Matrix Animation and Polar Decomposition'' + * + * A = R S + * + * Where R is a special orthogonal matrix (R*R^T = I and det(R)=1) + * + * Set R_0 = A + * Do + * R_{i+1} = 1/2( R_i + R^{-T}_i ) + * Until R_{i+1}-R_i = 0 + * + * + * @return If a solution was found within the specified threshold value then the return value is true otherwise it is false. + */ + template + inline bool newton(matrix3x3_type const & A, matrix3x3_type & R,unsigned int max_iterations, typename matrix3x3_type::value_type const & threshold) + { + typedef typename matrix3x3_type::value_traits value_traits; + typedef typename matrix3x3_type::value_type real_type; + + assert(max_iterations>0 || !"polar_decompostion::newton() max_iterations must be positive"); + assert(threshold>value_traits::zero() || !"polar_decomposition::newton(): theshold must be positive"); + + matrix3x3_type Q[2]; + int cur = 0, next = 1; + Q[cur] = A; + + bool within_threshold = false; + + for(unsigned int iteration=0;iteration< max_iterations;++iteration) + { + Q[next] = ( Q[cur] + trans(inverse(Q[cur])) )*.5; + real_type test = max_value ( Q[next] - Q[cur] ); + if( test < threshold ) + { + within_threshold = true; + break; + } + cur = (cur + 1)%2; + next = (next + 1)%2; + } + R = Q[next]; + return within_threshold; + } + + }// namespace polar_decomposition + + }// namespace math + +}// namespace OpenTissue + +// OPENTISSUE_CORE_MATH_MATH_POLAR_DECOMPOSITION_H +#endif + diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_power2.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_power2.h new file mode 100644 index 000000000..533a80cce --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_power2.h @@ -0,0 +1,83 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_POWER2_H +#define OPENTISSUE_CORE_MATH_MATH_POWER2_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + + namespace math + { + + /** + * This method test whether there exist some positive integer, n, such that + * + * 2^n == val + * + * In which case it returns true otherwise it returns false. + */ + template< class T> + inline bool is_power2( T val ) + { + T next = 1u; + for ( unsigned int i = 0u; i < 32u; ++i ) + { + if ( val == next ) + return true; + next = next << 1u; + } + return false; + } + + /** + * This function finds the smallest positive integer, n, such that + * + * val <= 2^n + * + * and returns the value 2^n. + */ + template + inline T upper_power2( T val ) + { + T next = 1u; + for ( unsigned int i = 0u; i < 32u; ++i ) + { + if ( next >= val ) + return next; + next = next << 1u; + } + return 0u; + } + + /** + * This function finds the largest positive integer, n, such that + * + * 2^n <= val + * + * and returns the value 2^n. + */ + template + inline T lower_power2( T val ) + { + T next = 1u << 31u; + for ( unsigned int i = 0u; i < 32u; ++i ) + { + if ( next <= val ) + return next; + next = next >> 1u; + } + return 0u; + } + + } // namespace math + +} // namespace OpenTissue + +//OPENTISSUE_CORE_MATH_MATH_POWER2_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_precision.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_precision.h new file mode 100644 index 000000000..5fc3cfab9 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_precision.h @@ -0,0 +1,42 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_PRECISION_H +#define OPENTISSUE_CORE_MATH_MATH_PRECISION_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include // for std::numeric_limits::epsilon() + + +namespace OpenTissue +{ + + namespace math + { + template + inline T machine_precision() + { + return std::numeric_limits::epsilon(); + } + + template + inline T working_precision() + { + return std::numeric_limits::epsilon()*10; + } + + template + inline T working_precision(unsigned int scale_factor) + { + return std::numeric_limits::epsilon()*scale_factor; + } + + } +} + +//OPENTISSUE_CORE_MATH_MATH_PRECISION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_prime_numbers.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_prime_numbers.h new file mode 100644 index 000000000..4b975d89a --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_prime_numbers.h @@ -0,0 +1,209 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_PRIME_NUMBERS_H +#define OPENTISSUE_CORE_MATH_MATH_PRIME_NUMBERS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +#include //--- for floor and sqrt + + +namespace OpenTissue +{ + + namespace math + { + + + namespace detail + { + + /** + * Used exclusively by the Miller-Rabin algorithm. It + * is essential the same as modular exponentiation a + * raised to n-1 modulo n. + * + * This "version" is specialized to test for notrivial + * square roots of 1. + */ + inline bool withness(int a,int n) + { + int ndec = n-1;int d = 1; int k = ndec; + if(k!=0) + { + int c=0; + while((k&0x80000000)==0) + k<<=1;c++; + while(c<32) + { + int x =d; + d=(d*d)%n; + if((d==1)&&(x!=1)&&(x!=ndec)) + return true;//--- Notrival square root of 1 was discovered. + if((k&0x80000000)!=0) + d=(d*a)%n; + k<<=1;c++; + } + } + if(d!=1) + return true; + return false; + } + + }//namespace detail + + + /** + * Prime Test. + * This method tests if the specified integer is a prime. + * + * Do Always find a correct solution, but is + * very slow for large numbers. + * + * Runs in time 2^(beta/2) where beta is the + * number of bits of n. + * + * @param n The number to test. + * @return If n was prime then the return value is + * true otherwise it is false. + */ + inline bool trial_division(int n) + { + using std::sqrt; + using std::floor; + + double sqrN = sqrt(static_cast(n)); + int j = static_cast( floor(sqrN) ); + for(int i=2;i<=j;++i) + { + if((n%i)==0) + return false; + } + return true; + } + + /** + * Modular Exponentiation. + * Computes a raised to the power of b modular n + */ + inline int modular_exponentiation(int a,int b,int n) + { + int d = 1; + while(b!=0) + { + if((b&1)!=0) + d=(d*a)%n; + a = (a*a)%n; + b>>=1; + } + return d; + } + + /** + * Prime Test. + * This method tests if the specified integer is a prime. + * + * Extremely fast, does not allways find + * a correct solution. However it fails + * rarely and it only makes error of one + * type. + * + * Some base-2 pseudo primes are 341, 561,645 and 1105. + * + * @param n The number to test. + * @return If n was prime then the return value is + * true otherwise it is false. + */ + inline bool pseudo_prime(int n) + { + if((n==1)||(n==2))//--- trivial cases + return true; + if(modular_exponentiation(2,n-1,n)!=1) + return false;//--- definitely not a prime, i.e. composite + return true;//--- possible prime or a base-2 pseudoprime + } + + + /** + * Closest Prime Search. + * This method tries to find the closest + * prime number to n. + * + * @param n The seed number from where the + * search should be done from. + * @return The closest prime number to n. + */ + inline int prime_search(int n) + { + int l = n; + if((l%2)==0) + l--; + for(;l>1;l=l-2) + { + // if(trial_division(l)) + // if(pseudo_prime(l)) + if(miller_rabin(l,4)) + break; + } + int o = n; + if((o%2)==0) + o++; + int max_val = detail::highest(); + for(;o + +#include +#include +#include +#include + +#include +#include + + +namespace OpenTissue +{ + + namespace math + { + + template< + typename value_type_ + //, typename value_traits_ = ValueTraits + > + class Quaternion + { + protected: + + typedef typename OpenTissue::math::ValueTraits value_traits_ ; // TODO value_traits_ should be parameterized as a class template parameter. + + public: + + typedef value_traits_ value_traits; ///< Convenience typedef to make value traits accessible for all template functions using Vector3 types. + typedef value_type_ value_type; ///< Typedef is required for compliance with many other libraries and data containers! + typedef Vector3 vector3_type; // TODO should Vector3 type be template parameterized? + typedef Matrix3x3 matrix3x3_type; + typedef size_t index_type; // TODO should the index type be template parameterized? + + protected: + + value_type m_s; ///< The real part. + vector3_type m_v; ///< The R3 part or imaginary part. + + public: + + value_type & s() { return m_s; } + value_type const & s() const { return m_s; } + + vector3_type & v() { return m_v; } + vector3_type const & v() const { return m_v; } + + public: + + Quaternion() + : m_s(value_traits::one()) + , m_v(value_traits::zero(),value_traits::zero(),value_traits::zero()) + {} + + ~Quaternion(){} + + Quaternion(Quaternion const & q) { *this = q; } + + explicit Quaternion(matrix3x3_type const & M){ *this = M; } + + explicit Quaternion(value_type const & s_val, value_type const & x, value_type const & y, value_type const & z) + : m_s(s_val) + , m_v(x,y,z) + { } + + explicit Quaternion(value_type const & s_val, vector3_type const & v_val) + : m_s(s_val) + , m_v(v_val) + { } + + Quaternion & operator=(Quaternion const & cpy) + { + m_s = cpy.m_s; + m_v = cpy.m_v; + return *this; + } + + /** + * Assignment operator. + * This method is a little special. Quaternions can be used to represent rotations, so can matrices. + * This method transforms a rotation matrix into a Quaternion and then it assigns it to this Quaternion. + * In short this method converts matrices into quaternions. + * + * @param M A reference to a matrix. This matrix should be a rotation matrix. That is an orthogonal matrix. + */ + Quaternion & operator=(matrix3x3_type const & M) + { + using std::sqrt; + + value_type const & M00 = M(0,0); + value_type const & M01 = M(0,1); + value_type const & M02 = M(0,2); + value_type const & M10 = M(1,0); + value_type const & M11 = M(1,1); + value_type const & M12 = M(1,2); + value_type const & M20 = M(2,0); + value_type const & M21 = M(2,1); + value_type const & M22 = M(2,2); + + value_type tr = M00 + M11 + M22; + value_type r; + + value_type const half = value_traits::one()/value_traits::two(); + + if(tr>=value_traits::zero()) + { + r = sqrt(tr + value_traits::one()); + m_s = half*r; + r = half/r; + m_v[0] = (M21 - M12) * r; + m_v[1] = (M02 - M20) * r; + m_v[2] = (M10 - M01) * r; + } + else + { + int i = 0; + if(M11>M00) + i = 1; + if(M22>M(i,i)) + i = 2; + switch(i) + { + case 0: + r = sqrt((M00 - (M11+M22)) + value_traits::one()); + m_v[0] = half*r; + r = half/r; + m_v[1] = (M01 + M10) * r; + m_v[2] = (M20 + M02) * r; + m_s = (M21 - M12) * r; + break; + case 1: + r = sqrt((M11 - (M22+M00)) + value_traits::one()); + m_v[1] = half*r; + r = half/r; + m_v[2] = (M12 + M21)*r; + m_v[0] = (M01 + M10)*r; + m_s = (M02 - M20)*r; + break; + case 2: + r = sqrt((M22 - (M00+M11)) + value_traits::one()); + m_v[2] = half*r; + r = half/r; + m_v[0] = (M20 + M02) * r; + m_v[1] = (M12 + M21) * r; + m_s = (M10 - M01) * r; + break; + }; + } + return *this; + } + + public: + + // TODO: Comparing floats with == or != is not safe NOTE value_type might not be a float type it could be anything? This suggest that we need some kind of metaprogramming technique to deal with this problem? + bool operator==(Quaternion const & cmp) const { return m_s==cmp.m_s && m_v==cmp.m_v; } + bool operator!=(Quaternion const & cmp) const { return !(*this==cmp); } + + public: + + Quaternion & operator+= (Quaternion const & q ) + { + m_s += q.m_s; + m_v += q.m_v; + return *this; + } + + Quaternion & operator-= (Quaternion const & q ) + { + m_s -= q.m_s; + m_v -= q.m_v; + return *this; + } + + Quaternion operator+ ( Quaternion const &q ) const { return Quaternion(m_s + q.m_s, m_v + q.m_v); } + Quaternion operator- ( Quaternion const &q ) const { return Quaternion(m_s - q.m_s, m_v - q.m_v); } + + Quaternion & operator*=(value_type const &s_val ) + { + m_s *= s_val; + m_v *= s_val; + return *this; + } + + Quaternion & operator/=( value_type const &s_val ) + { + assert(s_val || !"Quaternion::operator/=(): division by zero"); + m_s /= s_val; + m_v /= s_val; + return *this; + } + + Quaternion operator-() const { return Quaternion(-m_s,-m_v); } + + value_type operator*(Quaternion const &q) const { return m_s*q.m_s + m_v*q.m_v; } + + /** + * Multiplication of quaternions. + * This method multiplies two quaternions with each other + * and assigns the result to this Quaternion. + * + * @param b The second Quaternion. + */ + Quaternion operator%(Quaternion const & b) + { + return Quaternion( m_s*b.s() - dot(m_v , b.v()), cross(m_v , b.v()) + b.v()*m_s + m_v*b.s() ); + } + + /** + * Quaternion Vector Multiplication. + * + * @param v A Quaternion as a vector, i.e. zero scalar value. + */ + Quaternion operator%(vector3_type const & v_val) + { + return Quaternion( - dot( m_v() , v_val) , cross(m_v , v_val) + v_val*m_s ); + } + + public: + + /** + * Assigns the quaternion to the identity rotation. + * This is a commonly used constant. It corresponds to the identity matrix. + */ + void identity() + { + m_s = value_traits::one(); + m_v.clear(); + } + + /** + * This method constructs a unit Quaternion which represents the specified rotation around the x-axis. + * + * @param rad The rotation angle in radians around the axis. + */ + void Rx(value_type const & rad) + { + using std::cos; + using std::sin; + + value_type teta = rad/value_traits::two(); + value_type cteta = (value_type)( cos(teta) ); + value_type steta = (value_type)( sin(teta) ); + + m_s = cteta; + m_v(0) = steta; + m_v(1) = value_traits::zero(); + m_v(2) = value_traits::zero(); + } + + /** + * This method constructs a unit Quaternion which represents the specified rotation around the y-axis. + * + * @param rad The rotation angle in radians around the axis. + */ + void Ry(value_type const & rad) + { + using std::cos; + using std::sin; + + value_type teta = rad/value_traits::two(); + value_type cteta = (value_type)( cos(teta) ); + value_type steta = (value_type)( sin(teta) ); + + m_s = cteta; + m_v(0) = value_traits::zero(); + m_v(1) = steta; + m_v(2) = value_traits::zero(); + } + + /** + * This method constructs a unit Quaternion which represents the specified rotation around the z-axis. + * + * @param rad The rotation angle in radians around the axis. + */ + void Rz(value_type const & rad) + { + using std::cos; + using std::sin; + + value_type teta = rad/value_traits::two(); + value_type cteta = (value_type)( cos(teta) ); + value_type steta = (value_type)( sin(teta) ); + + m_s = cteta; + m_v(0) = value_traits::zero(); + m_v(1) = value_traits::zero(); + m_v(2) = steta; + } + + /** + * Rotate Vector by Quaternion. + * + * computes r' = q*r*conj(q) + */ + vector3_type rotate(vector3_type const & r) const { return ((*this) % r % conj(*this)).v(); } + + public: + + /** + * Equality comparison with an error bound. + * The test is exactly the same as with the method without error bound the only difference + * is that the corresponding terms of the quaternions are said to be equal if they do not + * differ by more than the error bound value. + * + * @param q A reference to a Quaternion. This is the + * Quaternion to compare with. + * @param threshold A reference to the acceptable error bound. + * + * @return The test result. The return value is one if + * the quaternions are equal otherwise the return + * value is zero. + */ + bool is_equal(Quaternion const & q,value_type const & threshold) const + { + using std::fabs; + + assert( threshold>=value_traits::zero() || !"is_equal(): threshold must be non-negative"); + + return fabs(m_s-q.m_s) + inline Vector3 rotate(Quaternion const & q, Vector3 const & r) + { + return prod( prod(q , r) , conj(q)).v(); + } + + template + inline Quaternion prod(Quaternion const & a, Quaternion const & b) + { + return Quaternion( a.s()*b.s() - dot(a.v() , b.v()), cross(a.v() , b.v()) + b.v()*a.s() + a.v()*b.s() ); + } + + template + inline Quaternion prod(Quaternion const & a, Vector3 const & b) + { + return Quaternion( - dot(a.v() , b), cross(a.v() , b) + b*a.s() ); + } + + template + inline Quaternion prod(Vector3 const & a, Quaternion const & b) + { + return Quaternion( - dot(a , b.v()), cross(a , b.v()) + a*b.s() ); + } + + template + inline Quaternion operator%(Quaternion const & a, Quaternion const & b) { return prod(a,b); } + template + inline Quaternion operator%(Quaternion const & a, Vector3 const & b) { return prod(a,b); } + template + inline Quaternion operator%(Vector3 const & a, Quaternion const & b) { return prod(a,b); } + template + inline Quaternion operator*( const Quaternion &q, const T2 &s_val ) { return Quaternion( q.s()*s_val, q.v()*s_val); } + template + inline Quaternion operator*( const T2 &s_val, const Quaternion &q ) { return Quaternion( q.s()*s_val, q.v()*s_val); } + template + inline Quaternion operator/( const Quaternion &q, const T2 &s_val ) { return Quaternion( q.s()/s_val, q.v()/s_val); } + template + inline Quaternion operator/( const T2 &s_val, const Quaternion &q ) { return Quaternion( q.s()/s_val, q.v()/s_val); } + + template + inline T const length(Quaternion const & q) + { + using std::sqrt; + return sqrt( q*q ); + } + + template + inline Quaternion unit(Quaternion const & q) + { + typedef typename Quaternion::value_traits value_traits; + + using std::sqrt; + using std::fabs; + + T l = length(q); + + if(fabs(l) > value_traits::zero()) + return Quaternion (q.s()/l,q.v()/l); + return Quaternion (value_traits::zero(),value_traits::zero(),value_traits::zero(),value_traits::zero()); + } + + template + inline Quaternion normalize(Quaternion const & q) { return unit(q); } + + /** + * Natural Logarithm + * Returns the Quaternion equal to the natural logarithm of + * the specified Quaternion. + * + * @param q A reference to an unit quaternion. + * @return + */ + template + inline Quaternion log(Quaternion const & q) + { + typedef typename Quaternion::value_traits value_traits; + + using std::acos; + using std::sin; + + if(q.s()==value_traits::one() && is_zero(q.v())) + return Quaternion(value_traits::zero(),value_traits::zero(),value_traits::zero(),value_traits::zero()); + + T teta = (T)( acos(q.s()) ); + T st = (T)( sin(teta) ); + return Quaternion( value_traits::zero(), q.v()*(teta/st) ); + } + + /** + * Orthogonal Quaternion. + * This method sets this Quaternion to an orthogonal Quaternion + * of the specified Quaternion. In other words the resulting + * angle between the specified Quaternion and this Quaternion + * is pi/2. + */ + template + inline Quaternion hat(Quaternion const & q) + { + return Quaternion( q.v()(2), - q.v()(1) , q.v()(0), -q.s()); + } + + /** + * Exponent + * Sets the Quaternion equal to the exponent of + * the specified Quaternion. + * + * @param q A reference to a pure Quaternion (zero + * T part). + */ + template + inline Quaternion exp(Quaternion const & q) + { + using std::sqrt; + using std::cos; + using std::sin; + + //--- teta^2 x^2 + teta^2 y^2 +teta^2 z^2 = + //--- teta^2 (x^2 + y^2 + z^2) = teta^2 + T teta = (T)( sqrt(q.v() *q.v()) ); + T ct = (T)( cos(teta) ); + T st = (T)( sin(teta) ); + + return Quaternion(ct,q.v()*st); + } + + /** + * QLERP - Linear Interpolation of Quaterions. + * + * @param A Quaternion A + * @param B Quaternion B + * @param w The weight + * + * @return The resulting Quaternion, (1-w)A+w B. Note the resulting + * Quaternion may not be a unit quaternion even though A and B are + * unit quaternions. If a unit Quaternion is needed write + * unit(qlerp(A,B,w)). + * + * -Added by spreak for the SBS algorithm, see OpenTissue/kinematics/skinning/SBS + */ + template + // TODO why not simply call this function lerp? + inline Quaternion qlerp(Quaternion const & A,Quaternion const & B,T const & w) + { + typedef typename Quaternion::value_traits value_traits; + + assert(w>=value_traits::zero() || !"qlerp(): w must not be less than 0"); + assert(w<=value_traits::one() || !"qlerp(): w must not be larger than 1"); + T mw = value_traits::one() - w; + return ((mw * A) + (w * B)); + } + + /** + * Spherical Linear Interpolation of Quaternions. + * + * @param A + * @param B + * @param w The weight + * + * @return The resulting Quaternion. + */ + template + inline Quaternion slerp(Quaternion const & A,Quaternion const & B,T const & w) + { + typedef typename Quaternion::value_traits value_traits; + + using std::acos; + using std::sin; + + assert(w>=value_traits::zero() || !"slerp(): w must not be less than 0"); + assert(w<=value_traits::one() || !"slerp(): w must not be larger than 1"); + + T q_tiny = (T)( 10e-7 ); // TODO prober constant type conversion? + + T norm = A*B; + + bool flip = false; + if( norm < value_traits::zero() ) + { + norm = -norm; + flip = true; + } + T weight = w; + T inv_weight; + if(value_traits::one() - norm < q_tiny) + { + inv_weight = value_traits::one() - weight; + } + else + { + T theta = (T)( acos(norm) ); + T s_val = (T)( value_traits::one() / sin(theta) ); + inv_weight = (T)( sin((value_traits::one() - weight) * theta) * s_val ); + weight = (T)( sin(weight * theta) * s_val ); + } + if(flip) + { + weight = -weight; + } + return ( inv_weight * A + weight * B); + } + + /** + * "Cubical" Spherical Interpolation. + * In popular terms this corresponds to a cubic spline in + * ordinary 3D space. However it is really a series of + * spherical linear interpolations, which defines a + * cubic on the unit Quaternion sphere. + * + * @param q0 + * @param q1 + * @param q2 + * @param q3 + * @param u + * + * @return + */ + template + inline Quaternion squad( + Quaternion const & q0 + , Quaternion const & q1 + , Quaternion const & q2 + , Quaternion const & q3 + , T const & u + ) + { + typedef typename Quaternion::value_traits value_traits; + + assert(u>=value_traits::zero() || !"squad(): u must not be less than 0"); + assert(u<=value_traits::one() || !"squad(): u must not be larger than 1"); + + T u2 = value_traits::two() *u*(value_traits::one() -u); + return slerp( slerp(q0,q3,u), slerp(q1,q2,u), u2); + } + + /** + * Quaternion Conjugate. + */ + template + inline Quaternion conj(Quaternion const & q) + { + return Quaternion(q.s(),-q.v()); + } + + + /** + * Get Axis Angle Representation. + * This function converts a unit-quaternion into the + * equivalent angle-axis representation. + * + * @param Q The quaternion + * @param axis Upon return this argument holds value of the equivalent rotation axis. + * @param theta Upon return this argument holds value of the equivalent rotation angle. + */ + template + inline void get_axis_angle(Quaternion const & Q,Vector3 & axis, T & theta) + { + using std::atan2; + + typedef typename Quaternion::value_traits value_traits; + typedef Vector3 V; + + // + // By definition a unit quaternion Q can be written as + // + // Q = [s,v] = [cos(theta/2), n sin(theta/2)] + // + // where n is a unit vector. This is the same as a rotation of + // theta radian around the axis n. + // + // + // Rotations are difficult to work with for several reasons. + // + // Firstly both Q and -Q represent the same rotation. This is + // easily proven, rotate a arbitrary vector r by Q then we have + // + // r^\prime = Q r Q^* + // + // Now rotate the same vector by -Q + // + // r^\prime = (-Q) r (-Q)^* = Q r Q^* + // + // because -Q = [-s,-v] and (-Q)^* = [-s , v] = - [s,-v]^* = - Q^*. + // + // Thus the quaternion representation of a single rotation is not unique. + // + // Secondly the rotation it self is not well-posed. A rotation of theta + // radians around the unit axis n could equally well be done as a rotation + // of -theta radians around the negative unit axis n. + // + // This is seen by straightforward substitution + // + // [ cos(-theta/2), sin(-theta/2) (-n) ] = [ cos(theta/2), sin(theta/2) n ] + // + // Thus we get the same quaternion regardless of whether we + // use (+theta,+n) or (-theta,-n). + // + // + // From the Quaternion we see that + // + // \frac{v}{\norm{v}} = \frac{ sin(theta/2) n }{| sin(theta/2) | } = sign(sin(theta/2)) n + // + // Thus we can easily get the rotation axis. However, we can not immediately + // determine the positive rotation axis direction. The problem boils down to the + // fact that we can not see the sign of the sinus-factor. + // + // Let us proceed by setting + // + // x = cos(theta/2) = s + // y = | sin(theta/2) | = \norm{v} + // + // Then we basically have two possibilities for finding theta + // + // theta_1 = 2 atan2( y, x) equivalent to sign(sin(theta/2)) = 1 + // + // or + // + // theta_2 = 2 atan2( -y, x) equivalent to sign(sin(theta/2)) = -1 + // + // If theta_1 is the solution we have + // + // n = \frac{v}{\norm{v}} + // + // If theta_2 is the solution we must have + // + // n = - \frac{v}{\norm{v}} + // + // Observe that we always have theta_2 = 2 pi - theta_1. Therefore theta_1 < theta_2. + // + // Let us imagine that we always choose $theta_1$ as the solution then + // the corresponding quaternion for that solution would be + // + // + // Q_1 = [cos(theta_1/2), sin(theta_1/2) \frac{v}{\norm{v}}] + // = [s , \norm{v} \frac{v}{\norm{v}}] + // = Q + // + // Now if we choose theta_2 as the solution we would have + // + // Q_2 = [cos(theta_2/2), sin(theta_2/2) -\frac{v}{\norm{v}}] + // = [s , -\norm{v} -\frac{v}{\norm{v}}] + // = [s , \norm{v} \frac{v}{\norm{v}}] + // = Q + // + // Thus we observe that regardless of which solution we pick we always have Q = Q_1 = Q_2. + // + // At this point one may be confused. However, it should be clear that theta_2 is equivalent + // to the theta_1 rotation. The difference is simply that theta_2 corresponds to flipping the + // rotation axis of the theta_1 case. + // + T const ct2 = Q.s(); //--- cos(theta/2) + T const st2 = length( Q.v() ); //--- |sin(theta/2)| + + theta = value_traits::two()* atan2(st2,ct2); + + assert( st2 >= value_traits::zero() || !"get_axis_angle(): |sin(theta/2)| must be non-negative"); + assert( theta >= value_traits::zero() || !"get_axis_angle(): theta must be non-negative"); + assert( is_number(theta) || !"get_axis_angle(): NaN encountered"); + + axis = st2 > value_traits::zero() ? Q.v() / st2 : V(value_traits::zero(), value_traits::zero(), value_traits::zero()); + } + + /** + * Get Rotation Angle wrt. an Axis. + * Think of it as if you have a fixated body A so only body B is allowed to move. + * + * Let BF_B' indicate the initial orientation of body B's body frame + * and BF_B the current orientation, now BF_A should be thought of as + * being immovable ie. constant. + * + * Q_initial : BF_A -> BF_B' + * Q_cur : BF_A -> BF_B + * + * Now we must have the relation + * + * Q_rel Q_initial = Q_cur + * + * From which we deduce + * + * Q_rel = Q_cur conj(Q_initial) + * + * And we see that + * + * Q_rel : BF_B' -> BF_B + * + * That is how much the body frame of body B have rotated (measured + * with respect to the fixed body frame A). + * + * + * @param Q_rel Rotation from initial orientation of B to current orientation of B. + * @param axis The rotation axis in the body frame of body B. + * + * @return The angle in radians. + */ + template + inline T get_angle(Quaternion const & Q_rel,Vector3 const & axis) + { + typedef typename Quaternion::value_traits value_traits; + + using std::atan2; + + //--- The angle between the two bodies is extracted from the Quaternion Q_rel + //--- + //--- [s,v] = [ cos(theta/2) , sin(theta/2) * u ] + //--- + //--- where s is a value_type and v is a 3-vector. u is a unit length axis and + //--- theta is a rotation along that axis. + //--- + //--- we can get theta/2 by: + //--- + //--- theta/2 = atan2 ( sin(theta/2) , cos(theta/2) ) + //--- + //--- but we can not get sin(theta/2) directly, only its absolute value: + //--- + //--- |v| = |sin(theta/2)| * |u| = |sin(theta/2)| + //--- + //--- using this value will have a strange effect. + //--- + //--- Recall that there are two Quaternion representations of a given + //--- rotation, q and -q. + //--- + //--- Typically as a body rotates along the axis it will go through a + //--- complete cycle using one representation and then the next cycle + //--- will use the other representation. + //--- + //--- This corresponds to u pointing in the direction of the joint axis and + //--- then in the opposite direction. The result is that theta + //--- will appear to go "backwards" every other cycle. + //--- + //--- Here is a fix: if u points "away" from the direction of the joint + //--- axis (i.e. more than 90 degrees) then use -q instead of q. This + //--- represents the same rotation, but results in the cos(theta/2) value + //--- being sign inverted. + T ct2 = Q_rel.s(); //--- cos(theta/2) + T st2 = length( Q_rel.v() ); //--- |sin(theta/2)| + T theta = value_traits::zero(); + + //--- Remember that Q_rel : BF_B' -> BF_B, so we need the axis in body B's local frame + if( Q_rel.v() * axis >= value_traits::zero()) + { + //--- u points in direction of axis. + //std::cout << "u points in direction of axis" << std::endl; + theta = value_traits::two()* atan2(st2,ct2); + } + else + { + //--- u points in opposite direction. + //std::cout << "u points in opposite direction" << std::endl; + theta = value_traits::two() * atan2(st2,-ct2); + } + //--- The angle we get will be between 0..2*pi, but we want + //--- to return angles between -pi..pi + if (theta > value_traits::pi()) + theta -= value_traits::two()*value_traits::pi(); + + //--- The angle we've just extracted has the wrong sign (Why???). + theta = -theta; + // + // Say we have a rotation, R, relating the coordinates of two frames X and Y, now + // let the coordinates of the vector v be given in frame X as + // + // [v]_X + // + // And in frame Y as + // + // [v]_Y + // + // Now R is defined such that + // + // R [v]_X = [v]_Y + // + // That is it changes the coordinates of v from X to Y. + // + // This is pretty straightforward, but there is some subtlety in it, say + // frame Y is rotated theta radians around the z-axis, then the rotation + // matrix relating the coordinates is the opposite rotation. + // + // What we want to measure is how much the frame axis have rotated, but + // what we are given is a rotation transforming coordinates, therefore + // we need to flip the sign of the extracted angle!!! + return theta; + } + + template + inline std::ostream & operator<< (std::ostream & o,Quaternion const & q) + { + o << "[" << q.s() << "," << q.v()(0) << "," << q.v()(1) << "," << q.v()(2) << "]"; + return o; + } + + template + inline std::istream & operator>>(std::istream & i,Quaternion & q) + { + char dummy; + + i >> dummy; + i >> q.s(); + i >> dummy; + i >> q.v()(0); + i >> dummy; + i >> q.v()(1); + i >> dummy; + i >> q.v()(2); + i >> dummy; + + return i; + } + + } // namespace math + +} // namespace OpenTissue + +//OPENTISSUE_CORE_MATH_MATH_QUATERNION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_value_traits.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_value_traits.h new file mode 100644 index 000000000..a09def20c --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_value_traits.h @@ -0,0 +1,48 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_VALUE_TRAITS_H +#define OPENTISSUE_CORE_MATH_MATH_VALUE_TRAITS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + + +namespace OpenTissue +{ + + namespace math + { + + template + class ValueTraits + { + public: + + static T zero() { return detail::zero(); } + static T one() { return detail::one(); } + static T two() { return detail::two(); } + static T three() { return detail::three(); } + static T four() { return detail::four(); } + static T eight() { return detail::eight(); } + static T half() { return detail::half(); } + static T pi() { return detail::pi(); } + static T pi_2() { return detail::pi_half(); } + static T pi_half() { return detail::pi_half(); } + static T pi_quarter(){ return detail::pi_quarter();} + static T pi_4() { return detail::pi_quarter();} + static T degree() { return detail::degree(); } + static T radian() { return detail::radian(); } + + }; + + } // namespace math + +} // namespace OpenTissue + +//OPENTISSUE_CORE_MATH_MATH_VALUE_TRAITS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/core/math/math_vector3.h b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_vector3.h new file mode 100644 index 000000000..661ff659b --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/core/math/math_vector3.h @@ -0,0 +1,577 @@ +#ifndef OPENTISSUE_CORE_MATH_MATH_VECTORX3_H +#define OPENTISSUE_CORE_MATH_MATH_VECTORX3_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include + + + +#include +#include +#include + +// TODO 2007-06-05 hod - Could not get this to work on linux. +// Might have somthing to do with http://gcc.gnu.org/faq.html#friend +//#include +#include + + +namespace OpenTissue +{ + + namespace math + { + + template < + typename value_type_ + //, typename value_traits_ = ValueTraits + > + class Vector3 + { + protected: + + typedef typename OpenTissue::math::ValueTraits value_traits_ ; // TODO value_traits_ should be parameterized as a class template parameter. + + public: + + typedef value_traits_ value_traits; ///< Convience typedef to make value traits accesible for all template functions using Vector3 types. + typedef value_type_ value_type; ///< Typedef is required for compliance with many other libraries and data containers! + typedef size_t index_type; // TODO should the index type be template parameterized? + + private: + + // TODO 2007-02-17 KE: Refactor this stuff into an array + + value_type x; ///< The first coordinate of this vector. + value_type y; ///< The second coordinate of this vector. + value_type z; ///< The third coordinate of this vector. + + public: + + + Vector3() + : x( value_traits::zero() ) + , y( value_traits::zero() ) + , z( value_traits::zero() ) + {} + + Vector3( Vector3 const & v ) + : x(v(0)) + , y(v(1)) + , z(v(2)) + {} + + explicit Vector3( value_type const& val ) + : x( val ) + , y( val ) + , z( val ) + {} + + template + Vector3( T1 const & x_val, T2 const & y_val, T3 const & z_val ) + : x( (value_type)(x_val) ) + , y( (value_type)(y_val) ) + , z( (value_type)(z_val) ) + {} + + ~Vector3() + {} + + Vector3 & operator=(Vector3 const & cpy) + { + x=cpy(0); + y=cpy(1); + z=cpy(2); + return *this; + } + + public: + + void clear() { x = y = z = value_traits::zero(); } + + size_t size() const { return 3u;} + + public: + + value_type & operator() ( index_type index ) + { + assert( (index>=0 && index<3) || !"vecto3():index should be in range [0..2]"); + return *((&x) + index); + } + + value_type const & operator() ( index_type index ) const + { + assert( (index>=0 && index<3) || !"vecto3():index should be in range [0..2]"); + return *((&x) + index); + } + + value_type & operator[] ( index_type index ) + { + assert( (index>=0 && index<3) || !"vecto3[]:index should be in range [0..2]"); + return *((&x) + index); + } + + value_type const & operator[] ( index_type index ) const + { + assert( (index>=0 && index<3) || !"vecto3[]:index should be in range [0..2]"); + return *((&x) + index); + } + + public: + + bool operator< (Vector3 const & v) const + { + if ( x < v(0) ) return true; + if ( x > v(0) ) return false; + if ( y < v(1) ) return true; + if ( y > v(1) ) return false; + return z < v(2); + } + + bool operator> (Vector3 const & v) const + { + if ( x > v(0) ) return true; + if ( x < v(0) ) return false; + if ( y > v(1) ) return true; + if ( y < v(1) ) return false; + return z > v(2); + } + + // TODO: Comparing floats with == or != is not safe NOTE value_type might not be a float type it could be anything? This suggest that we need some kinf of metaprogramming technique to deal with ths problem? + bool operator==(Vector3 const & cmp) const { return x==cmp.x && y==cmp.y && z==cmp.z; } + bool operator!=(Vector3 const & cmp) const { return x!=cmp.x || y!=cmp.y || z!=cmp.z; } + + public: + + Vector3 & operator+= ( Vector3 const & v ) + { + x += v(0); + y += v(1); + z += v(2); + return *this; + } + + Vector3 & operator-= ( Vector3 const & v ) + { + x -= v(0); + y -= v(1); + z -= v(2); + return *this; + } + + Vector3 & operator*=( value_type const & s ) + { + x *= s; + y *= s; + z *= s; + return *this; + } + + Vector3 & operator/=( value_type const & s ) + { + assert(s || !"Vector3::/=(): division by zero"); + x /= s; + y /= s; + z /= s; + return *this; + } + + Vector3 operator+ ( Vector3 const & v ) const { return Vector3( x+v(0), y+v(1), z+v(2)); } + Vector3 operator- ( Vector3 const & v ) const { return Vector3( x-v(0), y-v(1), z-v(2)); } + Vector3 operator- ( ) const { return Vector3(-x,-y,-z); } + Vector3 operator% ( Vector3 const & v ) const { return Vector3(y*v(2)-v(1)*z, v(0)*z-x*v(2), x*v(1)-v(0)*y); } + value_type operator* ( Vector3 const & v ) const { return x*v(0) + y*v(1) + z*v(2); } + bool operator<=( Vector3 const & v ) const { return x <= v(0) && y <= v(1) && z <= v(2); } + bool operator>=( Vector3 const & v ) const { return x >= v(0) && y >= v(1) && z >= v(2); } + + public: + + friend Vector3 fabs( Vector3 const & v ) + { + using std::fabs; + return Vector3 ( + (value_type)( fabs( v(0) ) ) + , (value_type)( fabs( v(1) ) ) + , (value_type)( fabs( v(2) ) ) + ); + } + + friend Vector3 min( Vector3 const & A, Vector3 const & B ) + { + using std::min; + return Vector3( min( A(0), B(0) ), min( A(1), B(1) ), min( A(2), B(2) ) ); + } + + friend Vector3 max( Vector3 const & A, Vector3 const & B ) + { + using std::max; + return Vector3( max( A(0), B(0) ), max( A(1), B(1) ), max( A(2), B(2) ) ); + } + + friend Vector3 floor(Vector3 const & v) + { + using std::floor; + return Vector3( + (value_type)( floor(v(0)) ) + , (value_type)( floor(v(1)) ) + , (value_type)( floor(v(2)) ) + ); + } + + friend Vector3 ceil(Vector3 const & v) + { + using std::ceil; + return Vector3( + (value_type)( ceil(v(0)) ) + , (value_type)( ceil(v(1)) ) + , (value_type)( ceil(v(2)) ) + ); + } + + friend std::ostream & operator<< (std::ostream & o,Vector3 const & v) + { + o << "["; + o << v(0); + o << ","; + o << v(1); + o << "," << v(2) << "]"; + return o; + } + + friend std::istream & operator>>(std::istream & i,Vector3 & v) + { + char dummy; + i >> dummy; + i >> v(0); + i >> dummy; + i >> v(1); + i >> dummy; + i >> v(2); + i >> dummy; + return i; + } + public: + + // TODO 2007-02-17 KE: Kill this it should be handled with comparator traits in operator== and operator!= + bool is_equal(Vector3 const & v, value_type const & threshold) const + { + using std::fabs; + return fabs(x-v.x)<=threshold && fabs(y-v.y)<=threshold && fabs(z-v.z)<=threshold; + } + + }; // class Vector3 + + + ////////////////////////////////////////////////////////////////////////// + /// Declaration of vector3 non-member functions + ////////////////////////////////////////////////////////////////////////// + + template + inline Vector3 round(Vector3 const & v) + { + using std::floor; + + static T const half = detail::half(); + + return Vector3 ( + floor( v(0) + half ) + , floor( v(1) + half ) + , floor( v(2) + half ) + ); + } + + template + inline typename Vector3::index_type min_index(Vector3 const & v) + { + return v(0) <= v(1) && v(0) < v(2) ? 0 : v(1) <= v(0) && v(1) < v(2) ? 1 : 2; + } + + template + inline typename Vector3::index_type max_index(Vector3 const & v) + { + return v(2) >= v(0) && v(2) >= v(1) ? 2 : v(1) >= v(0) && v(1) > v(2) ? 1 : 0; + } + + template + inline typename Vector3::index_type mid_index(Vector3 const & v) + { + typename Vector3::index_type test = min_index(v) + max_index(v); + return test == 2 ? 1 : test == 1 ? 2 : 0; + } + + template + inline T min_value(Vector3 const & v) + { + using std::min; + return min(v(0),min(v(1),v(2))); + } + + template + inline T max_value(Vector3 const & v) + { + using std::max; + return max(v(0),max(v(1),v(2))); + } + + template + inline T mid_value(Vector3 const & v) + { + return v(mid_index(v)); + } + + template + inline bool is_zero(Vector3 const & v, T const & threshold) + { + using std::fabs; + return fabs(v(0))<=threshold && fabs(v(1))<=threshold && fabs(v(2))<=threshold; + } + + template + inline bool is_zero(Vector3 const & v) + { + typedef typename Vector3::value_traits value_traits; + return is_zero(v,value_traits::zero()); + } + + + + + template + inline Vector3 cross( Vector3 const & a, Vector3 const & b ) + { + return Vector3( a[1] * b[2] - b[1] * a[2], -a[0] * b[2] + b[0] * a[2], a[0] * b[1] - b[0] * a[1] ); + } + + template + inline T dot( Vector3 const & a, Vector3 const & b ) { return a(0)*b(0) + a(1)*b(1) + a(2)*b(2); } + + template + inline T inner_prod( Vector3 const & a, Vector3 const & b ) { return dot(a,b); } + + template + inline T length(Vector3 const & v) + { + using std::sqrt; + return (T)( sqrt( dot(v,v) ) ); + } + + template + inline T sqr_length(Vector3 const & v) { return (T)(v*v); } + + template + inline T norm(Vector3 const & v) { return length(v); } + + template + inline T norm_1(Vector3 const & v) + { + using std::fabs; + using std::max; + return max( fabs(v(0)), max( fabs(v(1)), fabs(v(2)) ) ); + } + + template + inline T distance(Vector3 const & a, Vector3 const & b) + { + return length(b-a); + } + + template + inline T sqr_distance(Vector3 const & a, Vector3 const & b) + { + return sqr_length(b-a); + } + + template + inline Vector3 sgn( Vector3 const & v ) + { + using OpenTissue::math::sgn; + return Vector3(sgn(v(0)), sgn(v(1)), sgn(v(2))); + } + + template + inline Vector3 unit( Vector3 const & v ) + { + typedef typename Vector3::value_traits value_traits; + + + using std::sqrt; + T const l = length(v); + if (l <= value_traits::zero()) + { + return Vector3( value_traits::zero() ); + } + T const inv = value_traits::one()/l; + return Vector3( inv*v(0), inv*v(1), inv*v(2) ); + } + + template + inline Vector3 normalize( Vector3 const & v ) { return unit(v); } + + template + inline void truncate(Vector3 & v, T const & precision_value) + { + typedef typename Vector3::value_traits value_traits; + + v(0) = ((v(0)>-precision_value)&&(v(0)-precision_value)&&(v(1)-precision_value)&&(v(2) + inline void truncate(Vector3 & v) + { + typedef typename Vector3::value_traits value_traits; + + truncate( v, value_traits::zero() ); + } + + /** + * Get Orthogonal Vector. + * + * @param v Any vector + * + * @return A vector orthogonal to v. + */ + template + inline Vector3 orthogonal(Vector3 const & v) + { + typedef typename Vector3::value_traits value_traits; + + using std::fabs; + + //--- Find a vector not collinear with v, we simply pick the + //--- y-axis direction as our prefered direction, afterwards + //--- we test if this was a good choice if not we pick the + //--- x-axis direction + Vector3 tmp; + if (fabs(v(1)) > fabs(v(0))) + tmp = Vector3(value_traits::one(),value_traits::zero(),value_traits::zero()); + else + tmp = Vector3(value_traits::zero(),value_traits::zero(),value_traits::one()); + //--- Now we find an orthogonal vector by using + //--- the cross product + return cross(v,tmp); + } + + template + inline Vector3 operator*( Vector3 const& v, T2 const& s ) { return Vector3( v(0)*s, v(1)*s, v(2)*s ); } + + template + inline Vector3 operator*( T2 const& s, Vector3 const& v ) { return Vector3( v(0)*s, v(1)*s, v(2)*s ); } + + template + inline Vector3 operator/( Vector3 const& v, T2 const& s ) { return Vector3( v(0)/s, v(1)/s, v(2)/s ); } + + /** + * Compute Orthonormal Vectors. + * Compute unit vectors of a right handed coordinate frame, given initial z-axis (k-vector). + * + * @param i Upon return contains a orthogonal vector to j and k + * @param j Upon return contains a orthogonal vector to i and k + * @param k A given direction for the last vector, assumed to be a unit-vector. + */ + template + inline void orthonormal_vectors( vector3_type & i, vector3_type & j, vector3_type const & k ) + { + typedef typename vector3_type::value_traits value_traits; + + using std::fabs; + vector3_type m_abs_k = fabs( k); + + if ( m_abs_k( 0 ) > m_abs_k( 1 ) ) + { + if ( m_abs_k( 0 ) > m_abs_k( 2 ) ) + i = vector3_type( value_traits::zero(), value_traits::one(), value_traits::zero() ); + else + i = vector3_type( value_traits::one(), value_traits::zero(), value_traits::zero() ); + } + else + { + if ( m_abs_k( 1 ) > m_abs_k( 2 ) ) + i = vector3_type( value_traits::zero(), value_traits::zero(), value_traits::one() ); + else + i = vector3_type( value_traits::one(), value_traits::zero(), value_traits::zero() ); + } + j = unit( cross(k,i) ); + i = cross(j,k); + } + + /** + * Get increasing order of vector elements as a permutation array. + * + * Note this template function generalizes to any size of vectors + * (not just 3-dimensional ones). + * + * @param v The vector to be examined. + * @param pi Upon return this container contains the permuation + * order, v(pi[0]) is smallest element of v, v(pi[1]) the + * next smallest and so on. + */ + template + inline void get_increasing_order( vector_type const & v, permutation_container & pi ) + { + unsigned int n = v.size(); + + for(unsigned int i=0u;i 0u;--j ) + { + if ( v( pi[ j ] ) < v( pi[ j - 1 ] ) ) + { + unsigned int tmp = pi[ j - 1 ]; + pi[ j - 1 ] = pi[ j ]; + pi[ j ] = tmp; + } + } + } + } + + + namespace detail + { + + template + inline Vector3 const & axis_x() + { + static Vector3 const xa(one(), zero(), zero()); + return xa; + } + + + template + inline Vector3 const & axis_y() + { + static Vector3 const ya(zero(), one(), zero()); + return ya; + } + + + template + inline Vector3 const & axis_z() + { + static Vector3 const za(zero(), zero(), one()); + return za; + } + + } + + } // namespace math + +} // namespace OpenTissue + +//OPENTISSUE_CORE_MATH_MATH_VECTORX3_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem.h new file mode 100644 index 000000000..d91785e2b --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem.h @@ -0,0 +1,17 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +//#include +#include +#include + +//OPENTISSUE_DYNAMICS_FEM_FEM_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_add_plasticity_force.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_add_plasticity_force.h new file mode 100644 index 000000000..78ad78e31 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_add_plasticity_force.h @@ -0,0 +1,221 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_ADD_PLASTICITY_FORCE_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_ADD_PLASTICITY_FORCE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + + + template + void add_plasticity_force_single1(tetrahedron_type& tet, real_type const & dt) + { + + using std::min; + using std::sqrt; + + typedef typename tetrahedron_type::vector3_type vector3_type; + + + assert(tet.m_yield>=0 || !"add_plasticity_force(): yield must be non-negative"); + assert(tet.m_creep>=0 || !"add_plasticity_force(): creep must be non-negative"); + assert(tet.m_creep<=(1.0/dt) || !"add_plasticity_force(): creep must be less that reciprocal time-step"); + assert(tet.m_max>=0 || !"add_plasticity_force(): max must be non-negative"); + + //--- Storage for total and elastic strains (plastic strains are stored in the tetrahedra) + real_type e_total[6]; + real_type e_elastic[6]; + + for(int i=0;i<6;++i) + e_elastic[i] = e_total[i] = 0; + + //--- Compute total strain: e_total = Be (Re^{-1} x - x0) + for(unsigned int j=0;j<4;++j) + { + vector3_type & x_j = tet.node(j)->m_coord; + vector3_type & x0_j = tet.node(j)->m_model_coord; + + vector3_type tmp = (trans(tet.m_Re)*x_j) - x0_j; + real_type bj = tet.m_B[j](0); + real_type cj = tet.m_B[j](1); + real_type dj = tet.m_B[j](2); + e_total[0] += bj*tmp(0); + e_total[1] += cj*tmp(1); + e_total[2] += dj*tmp(2); + e_total[3] += cj*tmp(0) + bj*tmp(1); + e_total[4] += dj*tmp(0) + bj*tmp(2); + e_total[5] += dj*tmp(1) + cj*tmp(2); + } + + //--- Compute elastic strain + for(int i=0;i<6;++i) + e_elastic[i] = e_total[i] - tet.m_plastic[i]; + + //--- if elastic strain exceeds c_yield then it is added to plastic strain by c_creep + real_type norm_elastic = 0; + for(int i=0;i<6;++i) + norm_elastic += e_elastic[i]*e_elastic[i]; + norm_elastic = sqrt(norm_elastic); + //max_elastic = max(max_elastic,norm_elastic); + + if(norm_elastic > tet.m_yield) + { + real_type amount = dt*min(tet.m_creep,(1.0/dt)); //--- make sure creep do not exceed 1/dt + for(int i=0;i<6;++i) + tet.m_plastic[i] += amount*e_elastic[i]; + } + + //--- if plastic strain exceeds c_max then it is clamped to maximum magnitude + real_type norm_plastic = 0; + for(int i=0;i<6;++i) + norm_plastic += tet.m_plastic[i]*tet.m_plastic[i]; + norm_plastic = sqrt(norm_plastic); + //max_plastic = max(max_plastic,norm_plastic); + + if(norm_plastic > tet.m_max) + { + real_type scale = tet.m_max/norm_plastic; + for(int i=0;i<6;++i) + tet.m_plastic[i] *= scale; + } + //--- Compute plastic forces: f_plastic = Re Pe e_plastic; where Pe = Ve Be^T E + for(unsigned int j=0;j<4;++j) + { + real_type * plastic = tet.m_plastic; + real_type bj = tet.m_B[j](0); + real_type cj = tet.m_B[j](1); + real_type dj = tet.m_B[j](2); + real_type E0 = tet.m_D(0); + real_type E1 = tet.m_D(1); + real_type E2 = tet.m_D(2); + vector3_type f; + + //--- + //--- + //--- Recall the structure of the B and E matrices + //--- + //--- | bj 0 0 | | E0 E1 E1 | + //--- B_j = | 0 cj 0 | | E1 E0 E0 | + //--- | 0 0 dj | E = | E1 E1 E0 | + //--- | cj bj 0 | | E2 | + //--- | dj 0 bj | | E2 | + //--- | 0 dj cj | | E2 | + //--- + //--- This implyies that the product B_j^T E is + //--- + //--- | bj E0 bj E1 bj E1 cj E2 dj E2 0 | + //--- | cj E1 cj E0 cj E1 bj E2 0 dj E2 | + //--- | dj E1 dj E1 dj E0 0 bj E2 cj E2 | + //--- + //--- Notice that eventhough this is a 3X6 matrix with 18-3=15 nonzero elements + //--- it actually only contains 9 different value. + //--- + //--- In fact these values could be precomputed and stored in each tetrahedron. + //--- Furthermore they could be pre-multiplied by the volume of the tetrahedron + //--- to yield the matrix, + //--- + //--- P_j = Ve B_j^T E + //--- + //--- The plastic force that should be subtracted from node j is then computed + //--- in each iteration as + //--- + //--- f_j = Re P_j e_plastic + //--- + //--- + real_type bjE0 = bj*E0; + real_type bjE1 = bj*E1; + real_type bjE2 = bj*E2; + real_type cjE0 = cj*E0; + real_type cjE1 = cj*E1; + real_type cjE2 = cj*E2; + real_type djE0 = dj*E0; + real_type djE1 = dj*E1; + real_type djE2 = dj*E2; + + f(0) = bjE0*plastic[0] + bjE1*plastic[1] + bjE1*plastic[2] + cjE2*plastic[3] + djE2*plastic[4]; + f(1) = cjE1*plastic[0] + cjE0*plastic[1] + cjE1*plastic[2] + bjE2*plastic[3] + + djE2*plastic[5]; + f(2) = djE1*plastic[0] + djE1*plastic[1] + djE0*plastic[2] + bjE2*plastic[4] + cjE2*plastic[5]; + + f *= tet.m_V; + tet.node(j)->m_f_external += tet.m_Re*f; + } + } + /** + * Add plasticity forces. + * + * @param begin Iterator to first tetrahedron. + * @param end Iterator to one past last tetrahedron. + * @param dt Simulation time step + * + */ + template < typename tetrahedron_iterator,typename real_type > + inline void add_plasticity_force2( + tetrahedron_iterator begin + , tetrahedron_iterator end + , real_type const & dt + ) + { + //typedef typename tetrahedron_iterator::value_type::matrix3x3_type matrix3x3_type; + + //real_type max_elastic = 0; + //real_type max_plastic = 0; + + assert(dt>0 || !"add_plasticity_force(): time-step must be positive"); + //--- + //--- The total strain is given by + //--- + //--- e_total = Be u + //--- + //--- Where u is the nodal displacement, ie u = x - x0. Applying + //--- the idea of stiffness warping we have + //--- + //--- e_total = Be ( Re^{-1} x - x0) + //--- + //--- where R_e is the rotational deformation of the e'th + //--- tetrahedral element. + //--- + //--- The elastic strain is given as the total strain minus the plastic strain + //--- + //--- e_elastic = e_total - e_plastic + //--- + //--- e_plastic is initial initialized to zero. Plastic strain causes plastic forces in the material + //--- + //--- f_plastic = Re Ke u_plastic + //--- + //--- Using e_plastic = Be u_plastic + //--- + //--- f_plastic = Re Ke Be^{-1} e_plastic + //--- f_plastic = Re (Ve Be^T E Be ) Be^{-1} e_plastic + //--- f_plastic = Re (Ve Be^T E) e_plastic + //--- + //--- Introducing the plasticity matrix Pe = (Ve Be^T E) we have + //--- + //--- f_plastic = Re Pe e_plastic + //--- + + for (tetrahedron_iterator T = begin; T != end; ++T) + { + add_plasticity_force_single(T,dt); + + } + //std::cout << "max elastic = " << max_elastic << std::endl; + //std::cout << "max plastic = " << max_plastic << std::endl; + + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_ADD_PLASTICITY_FORCE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_clear_stiffness_assembly.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_clear_stiffness_assembly.h new file mode 100644 index 000000000..c0cce081f --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_clear_stiffness_assembly.h @@ -0,0 +1,37 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_CLEAR_STIFFNESS_ASSEMBLY_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_CLEAR_STIFFNESS_ASSEMBLY_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + /** + * + * @param begin + * @param end + */ + template < typename node_type > + inline void clear_stiffness_assembly_single(node_type* node) + { + typedef typename node_type::matrix_iterator matrix_iterator; + node->m_f0.clear(); + for (matrix_iterator Kij = node->Kbegin() ; Kij != node->Kend(); ++Kij ) + Kij->second.clear(); + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_CLEAR_STIFFNESS_ASSEMBLY_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_b.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_b.h new file mode 100644 index 000000000..bf7c01d82 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_b.h @@ -0,0 +1,403 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_B_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_B_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + + /** + * Calculate B-matrices (derivatives of shape functions N, i.e B = SN). + * + * @param e10 Given the four corner points p0,p1,p2, and p3. This is p1-p0. + * @param e20 Given the four corner points p0,p1,p2, and p3. This is p2-p0. + * @param e30 Given the four corner points p0,p1,p2, and p3. This is p3-p0. + * @param volume The volume of the tetrahedron. + * @param B Upon return this array of 4 B-vectors contains the computed values. + */ + template + inline void compute_B( + vector3_type const& e10, + vector3_type const& e20, + vector3_type const& e30, + real_type volume, + vector3_type * B + ) + { + // + // In summary we want to calculate + // + // B = S N + // + // where + // + // B = [ B0 B1 B2 B3 ], N = [ N0 N1 N2 N3 ] + // + // Here S is the operational matrix given by + // - - + // | d/dx 0 0 | + // | 0 d/dy 0 | + // | 0 0 d/dz | + // S = | d/dy d/dx 0 | + // | d/dz 0 d/dx | + // | 0 d/dz d/dy | + // - - + // N_i's are called the shape functions and they are used to interpolate values. For + // 3D linear elastostatics these can be written as + // + // - - + // | w_i 0 0 | + // N_i = | 0 w_i 0 | + // | 0 0 w_i | + // - - + // + // The w_i's are functions in the coordinates x,y, and z and will be derived + // shortly below of here. + // + // The matrix B_i is given by: + // + // | b_i 0 0 | + // | 0 c_i 0 | + // B_i = | 0 0 d_i | + // | c_i b_i 0 | + // | d_i 0 b_i | + // | 0 d_i c_i | + // + // The entries are stored as + // + // B[i] = [ b_i c_i d_i ] + // + // In the following we go into details of deriving formulas for the interpolating + // functions N_i. From these formulas it follows how to compute the derivatives. + // Finally we show a clever way of implementing the computaitons. + // + // Using a linear polynomial for interpolating a u(x,y,z)-value at a given + // position x,y,z inside the tetrahedron + // + // u(x,y,z) = a_0 + a_1 x + a_2 y + a_3 z = P^T A + // + // where the a's are the polynomial coefficients (to be determed) and + // + // | 1 | | a0 | + // P = | x | , and A = | a1 | + // | y | | a2 | + // | z | | a3 | + // + // Say we know the u-values, u0(x0,y0,z0),...,u3(x3,y2,z3), at the four tetrahedron corner + // points, P0^T [x0 y0 z0],...,P3^T=[x3 y3 z3], then we can set up the linear system + // + // | u0 | | 1 x0 y0 z0 | | a0 | + // | u1 | = | 1 x1 y1 z1 | | a1 | = C A + // | u2 | | 1 x2 y2 z2 | | a2 | + // | u3 | | 1 x3 y3 z3 | | a3 | + // + // The C matrix is invertible (as long as the four points are in general postion, meaning + // that no point can be written as a linear combination of any of the three other points), + // so we can solve the polynomial coefficients by + // + // | u0 | + // A = C^-1 | u1 | + // | u2 | + // | u3 | + // Substitution into the polynomial interpolation formula yields + // + // | u0 | + // u(x,y,z) = P^T A = P^T C^{-1} | u1 | (*1) + // | u2 | + // | u3 | + // + // Denoting the i'th column of, P^T C^{-1}, by N_i we have u(x,y,z) = sum_i N_i u_i + // + // + // Let us now try to look at the bary-centric coordinates, w0,...,w1 of (x,y,z), these + // can also be used for interpolation + // + // | u0 | |u0| + // u(z,y,z) = w0 u0 + w1 u1 + w2 u2 + w3 u3 = [w0 w1 w2 w3] | u1 | = W^T |u1| + // | u2 | |u2| + // | u3 | |u3| + // + // From the coordinates of the four points and the condition 1 = w0 + w1 + w2 + w3 we can set + // up the linear system + // + // | 1 | | 1 1 1 1 | | w0 | + // | x | = | x0 x1 x2 x3 | | w1 | + // | y | | y0 y1 y2 y3 | | w2 | + // | z | | z0 z1 z2 z3 | | w3 | + // + // + // P = Q W + // + // Since the four points are in general postion, Q is invertible and we can solve for W + // + // W = Q^{-1} P + // + // Insertion into the barycentric interpolation formula yields + // + // |u0| |u0| |u0| + // u(x,y,z) = W^T |u1| = ( Q^{-1} P )^T |u1| = P^T Q^{-T} |u1| (*2) + // |u2| |u2| |u2| + // |u3| |u3| |u3| + // + // Comparision with the polynomial interpolation derivation we see that C = Q^T, furthermore + // we notice that w_i = N_i. So (not very surprinsingly) bary-centric interpolation is really + // just linear polynomial interpolation. + // + // Notice that for the volume, V, of the tetrahedron we have the relation: 6 V = det( C ) = det ( Q^T ) + // + // When computing the stiffness matrix we are interested in derivatives of the N_i's with + // respect to the x,y and z coordinates. + // + // + // From (*1) and (*2) we see (recall we use zero-indexing) + // + // d N_i + // ----- = C^{-1}_{1,i} = Q^{-T}_{1,i} (*3a) + // d x + // + // d N_i + // ----- = C^{-1}_{2,i} = Q^{-T}_{2,i} (*3b) + // d y + // + // d N_i + // ----- = C^{-1}_{3,i} = Q^{-T}_{3,i} (*3c) + // d z + // + // Instead of actually computing the inverse of the 4x4 C or Q matrices a computational + // more efficient solution can be derived, which only requires us to solve for a 3x3 system. + // + // By Cramers rule we have + // + // (-1)^{i+j} det( C_ji) + // C^{-1}_{i,j} = ---------------------- + // det(C) + // + // where det(C_ji) is the determinant of C with the j'th row and i'th column removed. + // + // Now defined + // + // e10 = P1 - P0 + // e20 = P2 - P0 + // e30 = P3 - P0 + // + // and the matrix E + // + // | e10^T | | (x1-x0) (y1-y0) (z1-z0) | + // E = | e20^T | = | (x2-x0) (y2-y0) (z2-z0) | + // | e30^T | | (x3-x0) (y3-y0) (z3-z0) | + // + // Then + // + // det(C) = det(E) and C^{-1}_{i+1,j+1} = E^{-1}_{i,j} (*4) + // + // This can shown by straightforward computation, immediately is is verified that + // + // {-1}^((i+1)+(j+1)) = {-1}^(i+j) + // + // So we have to show + // + // det( C_(i+1,j+1)) det( E_(ij)) + // --------------------- = -------------- + // det(C) det(E) + // + // assume det(C)= det(E) (left for reader as exercise:-) then inorder to prove + // second half of (*4) we have to show + // + // det( C_(i+1,j+1)) = det( E_(ij)) + // + // A total of 9 cases exist, for here we will show a single case and leave + // the remaining cases for the reader, use i=1 and j= 0, implying + // + // det( C_(12)) = det( E_(01)) + // + // So + // - - + // | 1 x0 z0 | - - + // det | 1 x2 z2 | = det | (x2-x0) (z2-z0)| + // | 1 x3 z3 | | (x3-x0) (z3-z0)| + // - - - - + // + // Which is trivially true. So we have + // + // | . ... | | . ... | + // C^{-1} = | . E^{-1} | ; Q^{-T} = | . E^{-T} | + // + // As can be seen from (*3) we do not have all the values needed since + // the first columns of C^{-1} and Q^{-T} are missing. To remedy this problem we can use + // the condition of the bary-centric coordinates + // + // w0 = 1 - w1 - w2 - w3 + // + // Taking the derivate wrt. x,y, and z yields + // + // d N_0 + // ----- = 1 - E^{-1}_01 - E^{-1}_02 - E^{-1}_03 (*5a) + // d x + // + // d N_0 + // ----- = 1 - E^{-1}_11 - E^{-1}_12 - E^{-1}_13 (*5b) + // d y + // + // d N_0 + // ----- = 1 - E^{-1}_21 - E^{-1}_22 - E^{-1}_23 (*5c) + // d z + // + // + // and for i>0 we have + // + // d N_i + // ----- = E^{-1}_0i (*6a) + // d x + // + // d N_i + // ----- = E^{-1}_1i (*6b) + // d y + // + // + // d N_i + // ----- = E^{-1}_2i (*6c) + // d z + // + // The notation b_i = d N_i / dx, c_i = d N_i / dy, and d_i = d N_i / dz is used. Furthermore + // all the derivatives are returned as four B-vectors, where + // + // B[i] = [ b_i c_i d_i] + // + // + // + // The above may seem as a mathematical trick, so let us try to derive our + // equations a litlle differently, but before doing so we must first develop + // some equations for the volume of a tetrahedron. + // + // | e10^T | + // 6 V = det(E) = | e20^T | = e10 \cdot (e20 \times e30) + // | e30^T | + // + // where e10 = p1-p0, e20 = p2-p0 and so on. + // In general given the right-hand order i,j,k, and m of the nodes we + // write vol(i,j,k,m) = eji \cdot (eki \times emi) / 6 + // + // Barycentric coordinates are infact the volume weighted weights coresponding to + // the four tetrahedra lying inside the tetrahedron and having apex at the + // point p=[x,y,z]^T and bases equal to the 4 triangular faces of the enclosing + // tetrahedron. To realize this we will examine bary-centric coordinates + // in the 2D case, that is the case of the Triangle. In 2D area corresponds + // to the volume, so given a trianle and a point p inside it + // + // + 2 + // /| + // / | + // / | + // / | + // / + p| + // / | + // / | + // 0 +-------+ 1 + // + // Let the area weighted weights be defined as + // + // w_0 = area(1,2,p) / area(0,1,2) + // w_1 = area(0,P,2) / area(0,1,2) = area(2,0,P) / area(0,1,2) + // w_2 = area(0,1,p) / area(0,1,2) + // + // It is intuitive to see that if p moves towards the i'th corner point then + // the nominator of w_i goes towards area(0,1,2). This means that w_i -> 1 + // while w_j ->0 for j\neq i. + // + // Having introduced the barycentric coordinates as the area weighted weights + // it is quite intuitive to see that we also must have + // + // 1 = \sum_i w_i + // + // In 3D the barycentric coordinates are the volume weighted weights. That is + // given the right handed order 0,1,2 and 3 of the corner points, we have by + // analogy to the 2D case + // + // vol(0,1,2,p) + // w_3 = ------------ = (p-p0) \codt ( e10 \times \e20 ) / 6V + // V + // + // vol(0,1,3,p) + // w_2 = ------------ = (p-p0) \codt ( e10 \times \e30 ) / 6V + // V + // + // vol(0,2,3,p) + // w_1 = ------------ = (p-p0) \codt ( e20 \times \e30 ) / 6V + // V + // + // vol(1,3,2,p) + // w_0 = ------------ = (p-p0) \codt ( e31 \times \e21 ) / 6V + // V + // + // But since we allready know w_3, w_2 and w_1 it is faster to compute w_0 as + // + // w_0 = 1 - w_1 - w_2 - w_3 + // + // Recal from previous that we seek the derivatives of w_i's wrt. the coordinaes when + // we compute the B functions. Let us write the [x y z] coordinates as [x_0 x_1 x_2] then + // + // d w_3 + // ------- = ( e10 \times \e20 )_{x_j} / 6V + // d x_j + // + // That is the j'th coordinate of the cross product divided by 6 times the volume. Similar for + // + // d w_2 + // ------- = ( e10 \times \e30 )_{x_j} / 6V + // d x_j + // + // d w_1 + // ------- = ( e20 \times \e30 )_{x_j} / 6V + // d x_j + // + // and finally + // + // d w_0 d w_i + // ------- = 1 - sum_i>0 ------- + // d x_j d x_j + // + // Our formulas may seem differnt from those in (*6), however + // + // d w_k + // ------- = ( eji \times \emi )_{x_j} / 6V = E_{-1}_{k,j} + // d x_j + // + // That is the adjoint of E_(j,k) is given by ( eji \times \emi )_{x_j} / 6. + // + // + // + + real_type div6V = 1/(6.0*volume); + + B[1](0) = (e20(2) * e30(1) - e20(1) * e30(2)) * div6V; // b0 = -det(E_11), (where E_ij is the submatrix of E with row i and column j removed) + B[2](0) = (e10(1) * e30(2) - e10(2) * e30(1)) * div6V; // b1 = det(E_12) + B[3](0) = (e10(2) * e20(1) - e10(1) * e20(2)) * div6V; // b2 = -det(E_13) + B[0](0) = -B[1](0) - B[2](0) - B[3](0); // b3 = -b0 - b1 - b2 + + B[1](1) = (e20(0) * e30(2) - e20(2) * e30(0)) * div6V; // c0 = det(E_21) + B[2](1) = (e10(2) * e30(0) - e10(0) * e30(2)) * div6V; // c1 = -det(E_22) + B[3](1) = (e10(0) * e20(2) - e10(2) * e20(0)) * div6V; // c2 = det(E_23) + B[0](1) = -B[1](1) - B[2](1) - B[3](1); // c3 = -c0 - c1 - c2 + + B[1](2) = (e20(1) * e30(0) - e20(0) * e30(1)) * div6V; // d0 = -det(E_31) + B[2](2) = (e10(0) * e30(1) - e10(1) * e30(0)) * div6V; // d1 = det(E_32) + B[3](2) = (e10(1) * e20(0) - e10(0) * e20(1)) * div6V; // d2 = -det(E_33) + B[0](2) = -B[1](2) - B[2](2) - B[3](2); // d3 = -d0 - d1 - d2 + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_B_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_isotropic_elasticity.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_isotropic_elasticity.h new file mode 100644 index 000000000..555f68ca7 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_isotropic_elasticity.h @@ -0,0 +1,121 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_ISOTROPIC_ELASTICITY_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_ISOTROPIC_ELASTICITY_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + /** + * Compute Isotropic Elasticity Matrix. + * + * @param young Young Modulus. + * @param poisson Poisson ratio. + * @param D Upon return holds the elasticity matrix in vector form D = [D0,D1,D2] + */ + template + inline void compute_isotropic_elasticity_vector( + real_type const & young + , real_type const & poisson + , vector3_type & D + ) + { + assert(young>0 || !"compute_isotropic_elasticity_vector(): Young modulus must be positive"); + assert(poisson>0 || !"compute_isotropic_elasticity_vector(): Poisson ratio must be positive"); + assert(poisson<0.5 || !"compute_isotropic_elasticity_vector(): Poisson ratio must be less than a half"); + + // The isotropic Elasticity matrix D is given by + // - - - - + // | 1-nu nu nu 0 0 0 | | D0 D1 D1 0 0 0 | + // young | nu 1-nu nu 0 0 0 | | D1 D0 D1 0 0 0 | + // ------------- | nu nu 1-nu 0 0 0 | | D1 D1 D0 0 0 0 | + // (1+nu)(1-2nu) | 0 0 0 (1-2nu)/2 0 0 | = | 0 0 0 D2 0 0 | + // | 0 0 0 0 (1-2nu)/2 0 | | 0 0 0 0 D2 0 | + // | 0 0 0 0 0 (1-2nu)/2 | | 0 0 0 0 0 D2 | + // - - - - + + real_type poisson2 = 2.0*poisson; + real_type scale = young / ((1.0 + poisson) * (1.0 - poisson2)); + D(0) = (1.0 - poisson) * scale; + D(1) = poisson * scale; + D(2) = young / (2.0 + poisson2); + } + + /** + * Compute Elasticy Matrix. + * + * @param young Youngs modulus (stiffness) + * @param poisson Poissons ratio (compressability) + * @param D Upon return holds the isotropoc linear elasticity matrix. + */ + template + inline void compute_isotropic_elasticity_matix( + real_type const & young + , real_type const & poisson + , matrix_type & D + ) + { + assert(young>0 || !"compute_isotropic_elasticity_matix(): Young modulus must be positive"); + assert(poisson>0 || !"compute_isotropic_elasticity_matix(): Poisson ratio must be positive"); + assert(poisson<0.5 || !"compute_isotropic_elasticity_matix(): Poisson ratio must be less than a half"); + assert(D.size1()==6 || !"compute_isotropic_elasticity_matix(): D-matrix did not have 6 rows"); + assert(D.size2()==6 || !"compute_isotropic_elasticity_matix(): D-matrix did not have 6 columns"); + + typedef typename matrix_type::value_type value_type; + + value_type lambda = (poisson*young)/(( 1+poisson )*( 1- (2*poisson) )); //--- Lame modulus + value_type mu = young/(2*(1+poisson)); //--- Shear Modulus + value_type tmp = lambda+(2*mu); + + D(0,0) = tmp; D(0,1) = lambda; D(0,2) = lambda; D(0,3) = 0; D(0,4) = 0; D(0,5) = 0; + D(1,0) = lambda; D(1,1) = tmp; D(1,2) = lambda; D(1,3) = 0; D(1,4) = 0; D(1,5) = 0; + D(2,0) = lambda; D(2,1) = lambda; D(2,2) = tmp; D(2,3) = 0; D(2,4) = 0; D(2,5) = 0; + D(3,0) = 0; D(3,1) = 0; D(3,2) = 0; D(3,3) = mu; D(3,4) = 0; D(3,4) = 0; + D(4,0) = 0; D(4,1) = 0; D(4,2) = 0; D(4,3) = 0; D(4,4) = mu; D(4,5) = 0; + D(5,0) = 0; D(5,1) = 0; D(5,2) = 0; D(5,3) = 0; D(5,4) = 0; D(5,5) = mu; + } + + /** + * Alternative representation of isotropic elasticity matrix. + * + * + * @param young Youngs modulus (stiffness) + * @param poisson Poissons ratio (compressability) + * @param D00 + * @param D01 + * @param D33 + * + */ + template + inline void compute_isotropic_elasticity_vector( + real_type const & young + , real_type const & poisson + , real_type & D00 + , real_type & D01 + , real_type & D33 + ) + { + assert(young>0 || !"compute_isotropic_elasticity_vector(): Young modulus must be positive"); + assert(poisson>0 || !"compute_isotropic_elasticity_vector(): Poisson ratio must be positive"); + assert(poisson<0.5 || !"compute_isotropic_elasticity_vector(): Poisson ratio must be less than a half"); + + D01 = (poisson*young)/(( 1 + poisson )*( 1- (2*poisson) )); //--- Lame modulus + D33 = young/(2*(1+poisson)); //--- Shear Modulus + D00 = D01+(2*D33); + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_ISOTROPIC_ELASTICITY_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_ke.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_ke.h new file mode 100644 index 000000000..a5f742af0 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_ke.h @@ -0,0 +1,299 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_KE_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_KE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + /** + * Compute Stiffness matrix of tetrahedral element. + * + * @param p0 + * @param p1 + * @param p2 + * @param p3 + * @param E Youngs modulus. + * @param nu Poisson ratio. + * @param Ke Upon return contains the computed stiffness values. + */ + template + inline void compute_Ke( + vector3_type const & p0, + vector3_type const & p1, + vector3_type const & p2, + vector3_type const & p3, + real_type const & E, + real_type const & nu, + matrix3x3_type Ke[4][4] + ) + { + using std::fabs; + + real_type d = p0(0); + real_type d4 = p0(1); + real_type d8 = p0(2); + real_type d1 = p1(0) - d; + real_type d5 = p1(1) - d4; + real_type d9 = p1(2) - d8; + real_type d2 = p2(0) - d; + real_type d6 = p2(1) - d4; + real_type d10 = p2(2) - d8; + real_type d3 = p3(0) - d; + real_type d7 = p3(1) - d4; + real_type d11 = p3(2) - d8; + real_type d12 = (d1 * d6 * d11 + d2 * d7 * d9 + d3 * d5 * d10) - d1 * d7 * d10 - d2 * d5 * d11 - d3 * d6 * d9; + real_type d13 = 1.0 / d12; + real_type d14 = fabs(d12 / 6.0); + vector3_type B[4]; + B[0].clear(); + B[1].clear(); + B[2].clear(); + B[3].clear(); + B[1](0) = (d10 * d7 - d6 * d11) * d13; + B[2](0) = (d5 * d11 - d9 * d7) * d13; + B[3](0) = (d9 * d6 - d5 * d10) * d13; + B[0](0) = -B[1](0) - B[2](0) - B[3](0); + B[1](1) = (d2 * d11 - d10 * d3) * d13; + B[2](1) = (d9 * d3 - d1 * d11) * d13; + B[3](1) = (d1 * d10 - d9 * d2) * d13; + B[0](1) = -B[1](1) - B[2](1) - B[3](1); + B[1](2) = (d6 * d3 - d2 * d7) * d13; + B[2](2) = (d1 * d7 - d5 * d3) * d13; + B[3](2) = (d5 * d2 - d1 * d6) * d13; + B[0](2) = -B[1](2) - B[2](2) - B[3](2); + real_type d15 = E / (1.0 + nu) / (1.0 - 2 * nu); + real_type d16 = (1.0 - nu) * d15; + real_type d17 = nu * d15; + real_type d18 = E / 2 / (1.0 + nu); + for(int i = 0; i < 4; ++i) + { + for(int j = 0; j < 4; ++j) + { + real_type d19 = B[i](0); + real_type d20 = B[i](1); + real_type d21 = B[i](2); + real_type d22 = B[j](0); + real_type d23 = B[j](1); + real_type d24 = B[j](2); + Ke[i][j](0,0) = d16 * d19 * d22 + d18 * (d20 * d23 + d21 * d24); + Ke[i][j](0,1) = d17 * d19 * d23 + d18 * (d20 * d22); + Ke[i][j](0,2) = d17 * d19 * d24 + d18 * (d21 * d22); + Ke[i][j](1,0) = d17 * d20 * d22 + d18 * (d19 * d23); + Ke[i][j](1,1) = d16 * d20 * d23 + d18 * (d19 * d22 + d21 * d24); + Ke[i][j](1,2) = d17 * d20 * d24 + d18 * (d21 * d23); + Ke[i][j](2,0) = d17 * d21 * d22 + d18 * (d19 * d24); + Ke[i][j](2,1) = d17 * d21 * d23 + d18 * (d20 * d24); + Ke[i][j](2,2) = d16 * d21 * d24 + d18 * (d20 * d23 + d19 * d22); + Ke[i][j] *= d14; + } + } + } + + /** + * Compute Stiffness matrix of tetrahedral element. + * + */ + template + inline void compute_Ke( + vector3_type * B, + vector3_type const& D, + real_type const & volume, + matrix3x3_type Ke[4][4] + ) + { + for (unsigned int i=0; i<4; ++i) + for (unsigned int j=i; j<4; ++j) + compute_Ke_ij( B[i], D, B[j], volume, Ke[i][j] ); + + for (unsigned int i=1; i<4; ++i) + for (unsigned int j=0; j + inline void compute_Ke_ij( + vector3_type const& Bi, + vector3_type const& D, + vector3_type const& Bj, + real_type const & volume, + matrix3x3_type & Ke_ij + ) + { + assert(Ke_ij.size1() == 3 || !"compute_Ke_ij(): Incompatible dimension"); + assert(Ke_ij.size2() == 3 || !"compute_Ke_ij(): Incompatible dimension"); + + // + // Calculate Ke_ij = Bi^T D Bj V + // + // The matrix Bi is given by: + // + // | bi 0 0 | + // | 0 ci 0 | + // Bi = | 0 0 di | + // | ci bi 0 | + // | di 0 bi | + // | 0 di ci | + // + // Similar for Bj. The elasticity matrix D is given by + // + // + // - - - - + // | 1-nu nu nu 0 0 0 | | D0 D1 D1 0 0 0 | + // young | nu 1-nu nu 0 0 0 | | D1 D0 D1 0 0 0 | + // D= ------------- | nu nu 1-nu 0 0 0 | | D1 D1 D0 0 0 0 | + // (1+nu)(1-2nu) | 0 0 0 (1-2nu)/2 0 0 | = | 0 0 0 D2 0 0 | + // | 0 0 0 0 (1-2nu)/2 0 | | 0 0 0 0 D2 0 | + // | 0 0 0 0 0 (1-2nu)/2 | | 0 0 0 0 0 D2 | + // - - - - + // + // + // We have something like + // + // Bi^T D Bj + // + // These matrices have a particular pattern, especially the B-matrices: + // + // + // |x 0 0|T + // |0 x 0| + // |0 0 x| + // | x 0 0 x 0 x| |x x 0| + // | 0 x 0 x x 0| = |0 x x| Bk(i,j)^T = Bk(j,i) + // | 0 0 x 0 x x| |x 0 x| + // + // And the product has the pattern + // + // + // | x x x 0 0 0 | |x 0 0| + // | x x x 0 0 0 | |0 x 0| + // | x x x 0 0 0 | |0 0 x| + // | x 0 0 x 0 x| | 0 0 0 x 0 0 | |x x 0| + // | 0 x 0 x x 0| | 0 0 0 0 x 0 | |0 x x| + // | 0 0 x 0 x x| | 0 0 0 0 0 x | |x 0 x| + // + // + // If we compute T = Bi^T E, then we get a matrix with pattern + // + // | x x x x 0 x| + // | x x x x x 0| + // | x x x 0 x x| + // + // Now computing Kij = T Bj, + // + // |x 0 0 | + // |0 x 0 | + // |0 0 x | + // |x x x | | x x x x 0 x| |x x 0 | + // |x x x | = | x x x x x 0| |0 x x | + // |x x x | | x x x 0 x x| |x 0 x | + // + // + // and exploiting that for isotropic materials we have, + // + // D(0,0) = D(1,1) = D(2,2) = D0 + // D(0,1) = D(1,2) = D(0,2) = D(1,0) = D(2,1) = D(2,0) = D1 + // D(3,3) = D(4,4) = D(5,5) = D2 + // + // Finally we do not need to store the B-matrices, observe that since + // + // b(0) = invC(1,0); b(1) = invC(1,1); b(2) = invC(1,2); b(3) = invC(1,3); + // c(0) = invC(2,0); c(1) = invC(2,1); c(2) = invC(2,2); c(3) = invC(2,3); + // d(0) = invC(3,0); d(1) = invC(3,1); d(2) = invC(3,2); d(3) = invC(3,3); + // + // and + // + // | bi 0 0 | + // | 0 ci 0 | + // Bi = | 0 0 di | + // | ci bi 0 | + // | 0 di ci | + // | di 0 bi | + // + // therefore + // + // | invC(1,i) 0 0 | + // | 0 invC(2,i) 0 | + // Bi = | 0 0 invC(3,i) | + // | invC(2,i) invC(1,i) 0 | + // | 0 invC(3,i) invC(2,i) | + // | invC(3,i) 0 invC(1,i) | + // + // Putting it all together and using matlab, we have, + // + //syms bi ci di real + //Bi = [ bi 0 0 ; 0 ci 0 ; 0 0 di ; ci bi 0 ; 0 di ci ; di 0 bi ] + //syms bj cj dj real + //Bj = [ bj 0 0 ; 0 cj 0 ; 0 0 dj ; cj bj 0 ; 0 dj cj ; dj 0 bj ] + //syms G H J real + //D = [ G H H 0 0 0; H G H 0 0 0; H H G 0 0 0; 0 0 0 J 0 0; 0 0 0 0 J 0; 0 0 0 0 0 J] + //T = Bi'*E + //Kij = T*Bj + //syms invC1i invC2i invC3i real; + //Kij = subs(Kij,bi,invC1i); + //Kij = subs(Kij,ci,invC2i); + //Kij = subs(Kij,di,invC3i); + //syms invC1j invC2j invC3j real; + //Kij = subs(Kij,bj,invC1j); + //Kij = subs(Kij,cj,invC2j); + //Kij = subs(Kij,dj,invC3j); + //Kij = collect(Kij,G) + //Kij = collect(Kij,H) + //Kij = collect(Kij,J) + // + // which yields + // + // - - + // | D0 bi bj + D2 (ci cj + di dj) D1 bi cj + D2 ci bj D1 bi dj + D2 di bj | + // Ke_ij = V | D1 ci bj + D2 bi cj D0 ci cj + D2 (bi bj + di dj) D1 ci dj + D2 di cj | + // | D1 di bj + D2 bi dj D1 di cj + D2 ci dj D0 di dj + D2 (bi bj + ci cj) | + // - - + real_type bi = Bi(0); + real_type ci = Bi(1); + real_type di = Bi(2); + + real_type bj = Bj(0); + real_type cj = Bj(1); + real_type dj = Bj(2); + + real_type D0 = D(0)*volume; + real_type D1 = D(1)*volume; + real_type D2 = D(2)*volume; + + Ke_ij(0,0) = D0 * bi * bj + D2 * (ci * cj + di * dj); + Ke_ij(0,1) = D1 * bi * cj + D2 * (ci * bj); + Ke_ij(0,2) = D1 * bi * dj + D2 * (di * bj); + + Ke_ij(1,0) = D1 * ci * bj + D2 * (bi * cj); + Ke_ij(1,1) = D0 * ci * cj + D2 * (bi * bj + di * dj); + Ke_ij(1,2) = D1 * ci * dj + D2 * (di * cj); + + Ke_ij(2,0) = D1 * di * bj + D2 * (bi * dj); + Ke_ij(2,1) = D1 * di * cj + D2 * (ci * dj); + Ke_ij(2,2) = D0 * di * dj + D2 * (ci * cj + bi * bj); + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_KE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_mass.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_mass.h new file mode 100644 index 000000000..1ff3cbd2e --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_mass.h @@ -0,0 +1,175 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_MASS_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_MASS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + + /** + * Compute Mass. + * + * Note: Volume should have been computed prior to invoking this + * function. Thus make sure that initialize_stiffness_elements have + * been invoked prior to this function. + * + * @param mesh + */ + template + inline void compute_mass(fem_mesh & mesh) + { + typedef typename fem_mesh::real_type real_type; + + //--- + //--- A mass matrix is a discrete representation of a continuous mass distribution. + //--- To compute our mass matrix for a tetrahedral element with linear shape + //--- functions we need the formula (pp. 266 in Cook) + //--- + //--- a!b!c!d! + //--- \int_V N_1^a N_2^b N_3^c N_4^d dV = 6V -------------------------- (**) + //--- (3 + a + b +c + d)! + //--- + //--- A consistent element mass matrix (pp. 376 Cook) is defined as + //--- + //--- m = \int_V \rho N^T N dV (***) + //--- + //--- This equation can be derived from work balance, the details of which is unimportant + //--- here (look Cook pp. 375-376 for details). + //--- Assumping \rho is constant over each tetrahedral element and using the linear shape + //--- functions the above definition (***) results in + //--- + //--- |N_1| + //--- m = \rho \int_V |N_2| |N_1 N_2 N_3 N_4| dV + //--- |N_3| + //--- |N_4| + //--- + //--- |(N_1 N_1) (N_1 N_2) (N_1 N_3) (N_1 N_4)| + //--- m = \rho \int_V |(N_2 N_1) (N_2 N_2) (N_2 N_3) (N_2 N_4)| dV + //--- |(N_3 N_1) (N_3 N_2) (N_3 N_3) (N_3 N_4)| + //--- |(N_4 N_1) (N_4 N_2) (N_4 N_3) (N_4 N_4)| + //--- + //--- by (**) + //--- + //--- | 2 1 1 1| + //--- m = \rho V/20 | 1 2 1 1| (****) + //--- | 1 1 2 1| + //--- | 1 1 1 2| + //--- + //--- V + //--- m_ij = \rho --- (1+delta_ij) + //--- 20 + //--- + //--- in 3D this means that for the tetrahedral element + //--- + //--- | 2 2 2 1 1 1 1 1 1 1 1 1 | + //--- | 2 2 2 1 1 1 1 1 1 1 1 1 | + //--- | 2 2 2 1 1 1 1 1 1 1 1 1 | + //--- | | + //--- | 1 1 1 2 2 2 1 1 1 1 1 1 | + //--- | 1 1 1 2 2 2 1 1 1 1 1 1 | + //--- V | 1 1 1 2 2 2 1 1 1 1 1 1 | + //--- Me = \rho --- | | + //--- 20 | 1 1 1 1 1 1 2 2 2 1 1 1 | + //--- | 1 1 1 1 1 1 2 2 2 1 1 1 | + //--- | 1 1 1 1 1 1 2 2 2 1 1 1 | + //--- | | + //--- | 1 1 1 1 1 1 1 1 1 2 2 2 | + //--- | 1 1 1 1 1 1 1 1 1 2 2 2 | + //--- | 1 1 1 1 1 1 1 1 1 2 2 2 | + //--- + //--- Notice that in order to obtain the global/system mass matrix an assembly similar to the + //--- stiffnees matrix assembly must be carried out. Further, the global M matrix will + //--- have the same sub-block pattern as the global K matrix. + //--- + //--- A consistent mass matrix is often not used in computer graphics. Instead and + //--- ad-hoc approach named ``lumped'' mass matrix is applied. + //--- The lumped mass matrix is obtained by placing particle masses at the nodes. + //--- This corresponds to shifting all the masses in the rows of (****) onto the + //--- diagonal. In 3D this yields the element mass matrix + //--- + //--- | 1 0 0 0 0 0 0 0 0 0 0 0 | + //--- | 0 1 0 0 0 0 0 0 0 0 0 0 | + //--- | 0 0 1 0 0 0 0 0 0 0 0 0 | + //--- | | + //--- | 0 0 0 1 0 0 0 0 0 0 0 0 | + //--- | 0 0 0 0 1 0 0 0 0 0 0 0 | + //--- V | 0 0 0 0 0 1 0 0 0 0 0 0 | + //--- Me = \rho --- | | + //--- 4 | 0 0 0 0 0 0 1 0 0 0 0 0 | + //--- | 0 0 0 0 0 0 0 1 0 0 0 0 | + //--- | 0 0 0 0 0 0 0 0 1 0 0 0 | + //--- | | + //--- | 0 0 0 0 0 0 0 0 0 1 0 0 | + //--- | 0 0 0 0 0 0 0 0 0 0 1 0 | + //--- | 0 0 0 0 0 0 0 0 0 0 0 1 | + //--- + //--- Thus a lumped mass matrix is diagonal whereas a consistent mass matrix + //--- is not. Observe that the global mass matrix would also diagonal and the + //--- assembly is simplified to an iteration over all tetrahedra, while + //--- incementing the nodal mass by one fourth of the tetrahedral mass. + //--- + //--- for each node n + //--- mass(n) = 0 + //--- next n + //--- for each tetrahedron e + //--- mass(n_i) += \rho_e Ve / 4 + //--- mass(n_j) += \rho_e Ve / 4 + //--- mass(n_k) += \rho_e Ve / 4 + //--- mass(n_m) += \rho_e Ve / 4 + //--- next e + //--- + //--- where n_i,n_j,n_k and n_m are the four nodes of the e'th tetrahedron. + //--- + //--- The advantage of lumping is less storage and higher performace. On the downside + //--- lumping introduces a discontinouty in the displacement field. + //--- + //--- Obrien.shen state that the errors in lumping is negligeble for small-size course + //--- meshes used in computer graphics. However, for finer meshes the errors becomes + //--- noticeable. + //--- + //--- There do exist other approaches for computing mass matrices, even mehtods which + //--- combine other methods. We refer the interested reader to Cook for more details. Here + //--- we have limited our selfes to the two most common methods. + //--- + //--- It is worthwhile to notice that under the reasonable assumptions that V and \rho are + //--- positive for all elements both the element mass matrices and the global mass matrices + //--- are symmetric positive definite matrices. + + for (int n=0;n::max(); + else + mesh.m_nodes[n].m_mass = 0; + } + + for (int t=0;tm_mass += amount; + mesh.m_tetrahedra[t].j()->m_mass += amount; + mesh.m_tetrahedra[t].k()->m_mass += amount; + mesh.m_tetrahedra[t].m()->m_mass += amount; + } + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_MASS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_volume.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_volume.h new file mode 100644 index 000000000..6b673774a --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_compute_volume.h @@ -0,0 +1,93 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_VOLUME_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_VOLUME_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + /** + * Compute volume of tetrahedron. + * + * @param e10 edge vector from p0 to p1, i.e. e10 = p1-p0. + * @param e20 edge vector from p0 to p2, i.e. e20 = p2-p0. + * @param e30 edge vector from p0 to p3, i.e. e30 = p3-p0. + * + * @return The signed volume of the tetrahedra with nodes p0,p1,p2 and p3. + */ + template + inline typename vector3_type::value_type compute_volume(vector3_type const& e10, vector3_type const& e20, vector3_type const& e30) + { + typedef typename vector3_type::value_type real_type; + // Calculate the volume given by: + // | 1 1 1 1 | + // | e1x e2x e3x | | p0x p1x p2x p3x | + // 6V = | e1 \cdot (e2 x e3) | = | e1y e2y e3y | = 6V | p0y p1y p2y p3y | + // | e1z e2z e3z | | p0z p1z p2z p3z | + // + real_type sixV = e10*(e20 % e30); + return sixV / 6.0; + } + + /** + * Compute Tetrahedron Volume + * + * @param p0 + * @param p1 + * @param p2 + * @param p3 + * + * @return The signed volume of the tetrahedra with nodes p0,p1,p2 and p3. + */ + template + inline typename vector3_type::value_type compute_volume(vector3_type const & p0,vector3_type const & p1,vector3_type const & p2,vector3_type const & p3) + { + typedef typename vector3_type::value_type real_type; + + real_type d = p0(0); + real_type d4 = p0(1); + real_type d8 = p0(2); + real_type d1 = p1(0) - d; + real_type d5 = p1(1) - d4; + real_type d9 = p1(2) - d8; + real_type d2 = p2(0) - d; + real_type d6 = p2(1) - d4; + real_type d10 = p2(2) - d8; + real_type d3 = p3(0) - d; + real_type d7 = p3(1) - d4; + real_type d11 = p3(2) - d8; + real_type d12 = (d1 * d6 * d11 + d2 * d7 * d9 + d3 * d5 * d10) - d1 * d7 * d10 - d2 * d5 * d11 - d3 * d6 * d9; + return d12 / 6.0; + /* + ** From Zienkiewicz & Taylor p.637 + ** V = 1/6*det( 1 x0 y0 z0; 1 x1 y1 z1; 1 x2 y2 z2; 1 x3 y3 z3) + ** where x0 = n0->i; y0 = n0(1); ... + ** Calculated by Mathematica ;-) + */ + /* + return (p0(2)*p1(1)*p2(0) - p0(1)*p1(2)*p2(0) - p0(2)*p1(0)*p2(1) + + p0(0)*p1(2)*p2(1) + p0(1)*p1(0)*p2(2) - p0(0)*p1(1)*p2(2) + - p0(2)*p1(1)*p3(0) + p0(1)*p1(2)*p3(0) + p0(2)*p2(1)*p3(0) + - p1(2)*p2(1)*p3(0) - p0(1)*p2(2)*p3(0) + p1(1)*p2(2)*p3(0) + + p0(2)*p1(0)*p3(1) - p0(0)*p1(2)*p3(1) - p0(2)*p2(0)*p3(1) + + p1(2)*p2(0)*p3(1) + p0(0)*p2(2)*p3(1) - p1(0)*p2(2)*p3(1) + - p0(1)*p1(0)*p3(2) + p0(0)*p1(1)*p3(2) + p0(1)*p2(0)*p3(2) + - p1(1)*p2(0)*p3(2) - p0(0)*p2(1)*p3(2) + p1(0)*p2(1)*p3(2))/6.; + */ + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_COMPUTE_VOLUME_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_conjugate_gradients.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_conjugate_gradients.h new file mode 100644 index 000000000..684a927f3 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_conjugate_gradients.h @@ -0,0 +1,141 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_CONJUGATE_GRADIENTS_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_CONJUGATE_GRADIENTS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#define n_i (&mesh.m_nodes[n]) +#define n_j (&mesh.m_nodes[j]) + + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + /** + * Conjugate Gradient Solver. + * This solver has been hard-wired for a WarpT4Mesh to solve the equation + * + * A v = b + * + */ + template < typename fem_mesh > + inline void conjugate_gradients( + fem_mesh & mesh + , unsigned int min_iterations + , unsigned int max_iterations + ) + { + using std::fabs; + + typedef typename fem_mesh::real_type real_type; + typedef typename fem_mesh::vector3_type vector3_type; + typedef typename fem_mesh::matrix3x3_type matrix3x3_type; + typedef typename fem_mesh::node_type::matrix_iterator matrix_iterator; + + + real_type tiny = 1e-010; // TODO: Should be user controllable + real_type tolerence = 0.001; // TODO: Should be user controllable + + //--- r = b - A v + //--- p = r + for(int n=0;nm_fixed) + continue; + + n_i->m_residual = n_i->m_b; + + matrix_iterator Abegin = n_i->Abegin(); + matrix_iterator Aend = n_i->Aend(); + for (matrix_iterator A = Abegin; A != Aend;++A) + { + unsigned int j = A->first; + matrix3x3_type & A_ij = A->second; + vector3_type & v_j = n_j->m_velocity; + + n_i->m_residual -= A_ij * v_j; + } + n_i->m_prev = n_i->m_residual; + } + + for(unsigned int iteration = 0; iteration < max_iterations; ++iteration) + { + real_type d = 0.0; + real_type d2 = 0.0; + + //--- u = A p + //--- d = r*r + //--- d2 = p*u + + for(int n=0;nm_fixed) + continue; + + n_i->m_update.clear(); + + matrix_iterator Abegin = n_i->Abegin(); + matrix_iterator Aend = n_i->Aend(); + for (matrix_iterator A = Abegin; A != Aend;++A) + { + + unsigned int j = A->first; + matrix3x3_type & A_ij = A->second; + + n_i->m_update += A_ij * n_j->m_prev; + + } + d += n_i->m_residual * n_i->m_residual; + d2 += n_i->m_prev * n_i->m_update; + } + + if(fabs(d2) < tiny) + d2 = tiny; + + real_type d3 = d / d2; + real_type d1 = 0.0; + //--- v += p * d3 + //--- r -= u * d3 + //--- d1 = r*r + + for(int n=0;nm_fixed) + continue; + n_i->m_velocity += n_i->m_prev * d3; + n_i->m_residual -= n_i->m_update * d3; + d1 += n_i->m_residual * n_i->m_residual; + } + if(iteration >= min_iterations && d1 < tolerence) + break; + + if(fabs(d) < tiny) + d = tiny; + + real_type d4 = d1 / d; + //--- p = r + d4 * p + for(int n=0;nm_fixed) + continue; + n_i->m_prev = n_i->m_residual + n_i->m_prev * d4; + } + } + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_CONJUGATE_GRADIENTS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_dynamics_assembly.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_dynamics_assembly.h new file mode 100644 index 000000000..8a8d9919f --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_dynamics_assembly.h @@ -0,0 +1,104 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_DYNAMICS_ASSEMBLY_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_DYNAMICS_ASSEMBLY_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#define n_i (&mesh.m_nodes[n]) +#define n_j (&mesh.m_nodes[j]) + + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + /** + * Setup dynamic equation. + * This is the equation of motion given as: + * + * + * A v^{i+1} = b + * + * + * where + * + + * A = M + \delta t C + \delta t^2 K + * b = M v^i - \delta t (K x^i + f_0 + f_plas - f_ext) + * + * In this implementation, the following holds: + * + * M is a diagonal matrix with diagonal elements given by m_mass. + * C is a diagonal matrix given by massCoef*M, which is Raleigh damping with stiffness coefficient zero. + * + * Also plastic forces, f_plas, is ignored. + * + * @param dt The time step, \delta t, which is about to be taken. + * @param mass_damping Coefficient for mass damping in the Raleigh damping equation. + * The coefficient \alpha in C = \alpha M + \beta K. In this implementation \beta = 0. + * + * + * + * + */ + template < typename fem_mesh, typename real_type > + inline void dynamics_assembly( + fem_mesh & mesh, + real_type const & mass_damping, + real_type const & dt + ) + { + typedef typename fem_mesh::vector3_type vector3_type; + typedef typename fem_mesh::matrix3x3_type matrix3x3_type; + typedef typename fem_mesh::node_type::matrix_iterator matrix_iterator; + + for (int n = 0;nidx(); + vector3_type & b_i = n_i->m_b; + real_type & m_i = n_i->m_mass; + + b_i.clear(); + + matrix_iterator Kbegin = n_i->Kbegin(); + matrix_iterator Kend = n_i->Kend(); + for (matrix_iterator K = Kbegin; K != Kend;++K) + { + unsigned int j = K->first; + matrix3x3_type & K_ij = K->second; + vector3_type & x_j = n_j->m_coord; + matrix3x3_type & A_ij = n_i->A(j); + + A_ij = K_ij * (dt*dt); + b_i -= K_ij * x_j; + if (i == j) + { + real_type c_i = mass_damping*m_i; + real_type tmp = m_i + dt*c_i; + A_ij(0,0) += tmp; A_ij(1,1) += tmp; A_ij(2,2) += tmp; + } + } + b_i -= n_i->m_f0; + b_i += n_i->m_f_external; + b_i *= dt; + b_i += n_i->m_velocity * m_i; + } + } + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_DYNAMICS_ASSEMBLY_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_init.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_init.h new file mode 100644 index 000000000..1d984e1d3 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_init.h @@ -0,0 +1,84 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_INIT_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_INIT_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace OpenTissue +{ + namespace fem + { + /** + * Initialize Data Structres. + * Should be invoked at start-up before any calls to animate_warping. + * + * This function assumes that mesh geometry have been set up prior + * to invokation (i.e. both world coord and orignial coord are initlized + * and the same). + * + * This method initialized the model with the same material parameters. End + * user could write his or her own initilization method replacing + * the steps where young modules, poisson ratio and densities are + * assigned to the model. + * + * @param mesh + * @param young Young Moduls, a value of 500000 seems work quite well. + * @param poisson Poisson ration, a value of 0.33 seems work quite well. + * @param density Mass density, a value of 1000 seems to work quite well. + * @param c_yield Plastic yield. + * @param c_creep Plastic creep. + * @param c_max Plastic max. + */ + template < typename fem_mesh,typename real_type > + inline void init( + fem_mesh & mesh, + real_type const & young, + real_type const & poisson, + real_type const & density, + real_type const & c_yield, + real_type const & c_creep, + real_type const & c_max + ) + { + assert(poisson>=0 || !"uniform_poisson() : Poisson ratio must be non-negative"); + assert(poisson<=0.5 || !"uniform_poisson() : Poisson ratio must not be larger than a half"); + assert(density>0 || !"uniform_density(): density must be positive"); + + //--- Assign material parameters + for (int t=0;t + +#include +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + /** + * Initialize plasticity material parameters. + * + * Note: Initialize_stiffness_elements must have been invoked prior + * to calling this function. + * + * @param begin Iterator to first tetrahedron. + * @param end Iterator to one past last tetrahedron. + * @param c_yield Plastic yield. + * @param c_creep Plastic creep. + * @param c_max Plastic max. + */ + template < typename tetrahedron_type,typename real_type > + inline void initialize_plastic_single( + tetrahedron_type* T + , real_type const & c_yield + , real_type const & c_creep + , real_type const & c_max + ) + { + assert(c_yield>=0 || !"initialize_plastic(): yield must be non-negative"); + assert(c_creep>=0 || !"initialize_plastic(): creep must be non-negative"); + assert(c_max>=0 || !"initialize_plastic(): max must be non-negative"); + + T->m_yield = c_yield; + T->m_creep = c_creep; + T->m_max = c_max; + for(int i=0;i<6;++i) + T->m_plastic[i] = 0; + + compute_B(T->m_e10, T->m_e20, T->m_e30, T->m_V, T->m_B); + compute_isotropic_elasticity_vector (T->m_young, T->m_poisson, T->m_D); + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_INITIALIZE_PLASTIC_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_initialize_stiffness_elements.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_initialize_stiffness_elements.h new file mode 100644 index 000000000..c3507c62b --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_initialize_stiffness_elements.h @@ -0,0 +1,68 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_INITIALIZE_STIFFNESS_ELEMENTS_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_INITIALIZE_STIFFNESS_ELEMENTS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + /** + * + * + * NOTE: Material parameters (young modulus and poisson ratio) must have + * been set prior to invoking this function. + * + * @param begin + * @param end + */ + template < typename tetrahedron_type > + inline void initialize_stiffness_elements_single(tetrahedron_type* T) + { + { + T->m_e10 = T->j()->m_model_coord - T->i()->m_model_coord; + T->m_e20 = T->k()->m_model_coord - T->i()->m_model_coord; + T->m_e30 = T->m()->m_model_coord - T->i()->m_model_coord; + + T->m_V = compute_volume(T->m_e10, T->m_e20, T->m_e30); + + //T->m_V = compute_volume( + // T->i()->m_model_coord, + // T->j()->m_model_coord, + // T->k()->m_model_coord, + // T->m()->m_model_coord + // ); + + assert(T->m_V>0 || !"initialize_stiffness_elements(): Element with negative volume is encountered! Maybe you got a bad mesh?"); + + T->m_Re = math::diag(1.0); + + //compute_Ke(T->m_B, T->m_D, T->m_V, T->m_Ke); + compute_Ke( + T->node(0)->m_model_coord, + T->node(1)->m_model_coord, + T->node(2)->m_model_coord, + T->node(3)->m_model_coord, + T->m_young, T->m_poisson, + T->m_Ke + ); + } + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_INITIALIZE_STIFFNESS_ELEMENTS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_mesh.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_mesh.h new file mode 100644 index 000000000..5025f6533 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_mesh.h @@ -0,0 +1,40 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_MESH_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_MESH_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include + +namespace OpenTissue +{ + namespace fem + { + + template + class Mesh + : public OpenTissue::t4mesh::T4Mesh< + math_types + , OpenTissue::fem::detail::NodeTraits + , OpenTissue::fem::detail::TetrahedronTraits + > + { + public: + + typedef typename math_types::real_type real_type; + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::matrix3x3_type matrix3x3_type; + }; + + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_MESH_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_node_traits.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_node_traits.h new file mode 100644 index 000000000..3129e5c63 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_node_traits.h @@ -0,0 +1,86 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_NODE_TRAITS_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_NODE_TRAITS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + + template + class NodeTraits + { + public: + + typedef typename math_types::real_type real_type; + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::matrix3x3_type matrix3x3_type; + + typedef typename std::map matrix_container; + typedef typename matrix_container::iterator matrix_iterator; + + public: + + NodeTraits(int i,int j) + { + } + matrix_container m_K_row; ///< Currently stored in a map container, key correspond to column and mapped value to 3-by-3 sub block. + matrix_container m_A_row; + vector3_type m_f0; + vector3_type m_b; + + bool m_fixed; ///< If the node is in-moveable this flag is set to true otherwise it is false. + + vector3_type m_model_coord; + vector3_type m_coord; ///< World coord + + vector3_type m_velocity; + real_type m_mass; + vector3_type m_f_external; + + // Needed by the ConjugateGradient method + vector3_type m_update; + vector3_type m_prev; + vector3_type m_residual; + + public: + + matrix_iterator Kbegin() + { + return m_K_row.begin(); + } + matrix_iterator Kend() { return m_K_row.end(); } + matrix_iterator Abegin() { return m_A_row.begin(); } + matrix_iterator Aend() { return m_A_row.end(); } + + matrix3x3_type & K(int const & column_idx) { + return m_K_row[column_idx]; + } + matrix3x3_type & A(int const & column_idx) { return m_A_row[column_idx]; } + + public: + + NodeTraits() + : m_fixed(false) + { + } + + }; + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_NODE_TRAITS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_position_update.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_position_update.h new file mode 100644 index 000000000..7765886d6 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_position_update.h @@ -0,0 +1,41 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_POSITION_UPDATE_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_POSITION_UPDATE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + /** + * Position Update. + * + * Note: Velocities must have been computed prior to invoking this function. + * @param mesh + * @param dt + */ + template < typename fem_mesh, typename real_type > + inline void position_update(fem_mesh & mesh, real_type const & dt) + { + for (int n=0;n + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + + /** + * + */ + template + inline void reset_orientation_single(tetrahedron_type* T) + { + T->m_Re = math::diag(1.0); + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_RESET_ORIENTATION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_simulate.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_simulate.h new file mode 100644 index 000000000..15314d366 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_simulate.h @@ -0,0 +1,131 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_SIMULATE_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_SIMULATE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace OpenTissue +{ + namespace fem + { + /** + * Simulate. + * Note external forces must have been computed prior to invocation. + * + * The dynamic equation has the following form (where u=x-x_0, and x' is derivative wrt. time) + * + * M x'' + Cx' + K (x-x_0) = f_ext + * + * This can be transformed to a system of 2x3n equations of first order derivative: + * + * x' = v + * M v' = - C v - K (x-x_0) + f_ext + * + * The semi-implicit Euler scheme approximates the above with: + * + * x^(i+1) = x^i + \delta t * v^(i+1) + * M v^(i+1) = M v^i + \delta t ( - C v^(i+1) - K ( x^i - x_0 ) + f^i_ext + * + * This is solved using implicit integration. + * + * @param mesh + * @param time_step + * @param use_stiffness_warping + */ + template < typename fem_mesh, typename real_type > + inline void simulate( + fem_mesh & mesh + , real_type const & time_step + , bool use_stiffness_warping, + real_type mass_damping = 2.0, + unsigned int min_iterations=20, + unsigned int max_iterations=20 + ) + { + // Some theory: + // + // + // Implicit discretization of + // + // M d^2x/dt^2 + C dx/dt + K (x-x0) = f_ext + // + // Evaluate at (i+1), and use d^2x/dt^2 = (v^{i+1} - v^i)/timestep and dx/dt = v^{i+1} + // + // M (v^{i+1} - v^i)/timestep + C v^{i+1} + K (x^{i+1}-x0) = f_ext + // + // and x^{i+1} = x^i + v^{i+1}*timestep + // + // M (v^{i+1} - v^i)/timestep + C v^{i+1} + K ( (x^i + v^{i+1}*timestep) -x0) = f_ext + // + // M (v^{i+1} - v^i)/timestep + C v^{i+1} + K*x^i + timestep*K*v^{i+1} - K x0 = f_ext + // + // M v^{i+1} - M v^i + timestep*C v^{i+1} + timestep*timestep*K*v^{i+1} = timestep * (f_ext - K*x^i + K x0) + // + // (M + timestep*C + timestep*timestep*K) v^{i+1} = M v^i + timestep * (f_ext - K*x^i + K x0) + // + // let f0 = -K x0 + // + // (M + timestep*C + timestep*timestep*K) v^{i+1} = M v^i + timestep * (f_ext - K*x^i -f0) + // + // (M + timestep*C + timestep*timestep*K) v^{i+1} = M v^i - timestep * (K*x^i + f0 - f_ext) + // + // so we need to solve A v^{i+1} = b for v^{i+1}, where + // + // A = (M + timestep*C + timestep*timestep*K) + // b = M v^i - timestep * (K*x^i + f0 - f_ext) + // + // afterwards we do a position update + // + // x^{i+1} = x^i + v^{i+1}*timestep + // + // Notice that a fully implicit scheme requres K^{i+1}, however for linear elastic materials K is constant. + + for (int n=0;n + +#define p_i (&T->m_owner->m_nodes[T->m_nodes[i]]) +#define p_j (&T->m_owner->m_nodes[T->m_nodes[j]]) + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + /** + * Stiffness Matrix Assembly. + * Observe that prior to invocations of this method the rotations of all tetrahedra + * must have been setted. Also all stiffness assembly data should have been cleared + * prior to invocations by using the clear_stiffness_assembly method. + * + * + * From linear elastostatics we have + * + * K u = f + * + * where u is vertex displacements and f is the resulting nodal forces and K is the stiffness matrix. + * Using u = x - x0, where x is current position and x0 original position we have + * + * K (x - x0) = f + * K x - K x0 = f + * + * Introducing f0 = - K x0 as the force offset vectors we have + * + * f = K x + f0 + * + * The idea of stiffness warping is to rotate the x-position back into the + * original coordinate system in which K were original computed, then compute + * nodal forces in this frame and finally rotate teh nodal forces back to + * the current frame. + * + * Let R denote the current orientation, then R^{-1} rotates back to the + * orignal frame which means we have + * + * f = R K ( R^{-1} x - x0 ) + * f = R K R^{-1} x - R K x0 + * + * So with stiffness warping we have to compute + * + * K' = R K R^{-1} + * f0' = - R K x0 + * + * For n nodes the system stiffness matrix K' is a 3n X 3n symmetric and sparse matrix. + * It would be insane to actual allocated such a matrix instead the matrix is + * stored inside the nodes of the volume mesh. + * + * Each node stores a row of K' and the correspond entries of the f0' vector. + * + * That is the i'th node stores all non-zero 3-by-3 sub-block of the i'th row + * of the stiffness matrix and it also stores the i'th 3-dimensional subvector of f0 + * + * K' f0' + * j + * - . - - - + * | . | | | + * | . | | | + * i-> |......X....| |x| + * | . | | | + * | . | | | + * - . - - - + * + * + * Let M denote the set of all tetrahedral elements then the assembly + * of the warped stiffness matrix can be written as + * + * K'_ij = sum Re Ke_ij Re^T + * e \in M + * and + * i, j \in e + * + * And the assembly of the force offset vector + * + * f0'_i = sum - Re Ke_ij x0_j + * e \in M + * and + * i, j \in e + * + * + * Notice that this can be optimized if node i as a meber of the e'th element + * then the contribution to the above summation can be written as (recal + * the indices j,k and m denote the three ofther nodes of e): + * + * -Re Ke_ii x0_i - Re Ke_ij x0_j -Re Ke_im x0_m - Re Ke_ik x0_k + * -Re ( Ke_ii x0_i + Ke_ij x0_j + Ke_im x0_m + Ke_ik x0_k) + * + * which saves us a few matrix multiplications and we then finally have + * + * f0'_i = sum -Re ( Ke_ii x0_i + Ke_ij x0_j + Ke_im x0_m + Ke_ik x0_k) + * e \in M + * and + * i \in e + * + * Assuming that f0'_i is initially cleared to zero for all i. This result in the + * implementation strategy: + * + * for each element e do + * for each node i of e do + * tmp = 0 + * for each node j of e do + * tmp += Ke_ij * xo_j + * next j + * f0'_i -= Re*tmp + * next i + * next e + * + * Also the stiffness matrix can be optimized slightly by exploiting the symmetry property. + * The symmetry indicates that it is sufficient to only compute the upper triangular and + * diagonal parts. The lower triangular parts can be found simply be taking the transpose + * of the upper triangular parts. + * + * So the e'th tetrahedral element contributes with + * + * K'_ij += Re Ke_ij Re^{T} + * K'_ji += Re Ke_ji Re^{T} = ( Re Ke_ij Re^{T} )^T + * + * because Ke_ji = Ke_ij^T. Assuming that all K'_ij is initially cleared to zero this results + * in the implementation strategy: + * + * for each element e do + * for each node i of e do + * for each node j of e do + * if i >= j then + * tmp = Re Ke_ij Re^{T} + * K'_ij += tmp + * if j > i then + * K'_ji += trans(tmp) + * end if + * end if + * next j + * next i + * next e + * + * The two implementation strategies can now be combined into one, which + * is what we have implemented below. + * + * Note that if Re is initially set to the identity matrix, then the stiffness + * warping reduces to the traditional assembly of stiffness matrix (as it is + * used in linear elastostatics). + * + * + * If the i'th node is set to be fixed, this actually corresponds to + * letting K'_ii = identity and letting K'_ij and K'_ji be zero for all j not + * equal to i. This is known as a Direchlet boundary condition. However, we do + * not this during our assembly. Instead we assemble the K' matrix as though + * there were no fixed nodes. Later on when we use the K' matrix in computations + * such as K' x, we simply test whether x_j is fixed when it is multilpied by + * the j'th column of K' i.e. K'_*j. If so we simply do nothing!!! This is + * computatonally more tracjktable and it also allow us to more easily turn + * nodes fixed and un-fixed dynamically during animation. + * + * + * + * + * + * @param begin + * @param end + * + */ + template + inline void stiffness_assembly_single1(tetrahedron_type* T) + { + typedef typename tetrahedron_type::real_type real_type; + typedef typename tetrahedron_type::vector3_type vector3_type; + typedef typename tetrahedron_type::matrix3x3_type matrix3x3_type; + + { + matrix3x3_type & Re = T->m_Re; + for (int i = 0; i < 4; ++i) + { + vector3_type f; + f.clear(); + for (int j = 0; j < 4; ++j) + { + matrix3x3_type & Ke_ij = T->m_Ke[i][j]; + vector3_type & x0_j = p_j->m_model_coord; + + f += Ke_ij * x0_j; + if (j >= i) + { + + matrix3x3_type tmp = Re * Ke_ij * trans(Re); + + p_i->K(p_j->idx()) += tmp; + if (j > i) + p_j->K(p_i->idx()) += trans(tmp); + } + } + + p_i->m_f0 -= Re*f; + + } + } + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_STIFFNESS_ASSEMBLY_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_tetrahedron_traits.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_tetrahedron_traits.h new file mode 100644 index 000000000..72382fd13 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_tetrahedron_traits.h @@ -0,0 +1,56 @@ +#ifndef OPENTISSUE_DYNAMICS_DEFORMATION_FEM_STIFFNESSWARPING_H +#define OPENTISSUE_DYNAMICS_DEFORMATION_FEM_STIFFNESSWARPING_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + template + class TetrahedronTraits + { + public: + + typedef typename math_types::real_type real_type; + typedef typename math_types::vector3_type vector3_type; + typedef typename math_types::matrix3x3_type matrix3x3_type; + + public: + + real_type m_young; + real_type m_poisson; + real_type m_density; + + matrix3x3_type m_Ke[4][4]; ///< Stiffness element matrix + matrix3x3_type m_Re; ///< Rotational warp of tetrahedron. + real_type m_V; ///< Volume of tetrahedron + + vector3_type m_e10; ///< edge from p0 to p1 + vector3_type m_e20; ///< edge from p0 to p2 + vector3_type m_e30; ///< edge from p0 to p3 + + //--- Stuff used exclusive by plastic effects + + vector3_type m_B[4]; ///< placeholders for Jacobian of shapefunctions: B = SN. + vector3_type m_D; ///< Elasticity Matrix in vector from + real_type m_plastic[6]; ///< Plastic strain tensor. + real_type m_yield; + real_type m_creep; + real_type m_max; + }; + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_DEFORMATION_FEM_STIFFNESSWARPING_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_uniform_density.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_uniform_density.h new file mode 100644 index 000000000..e648056d4 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_uniform_density.h @@ -0,0 +1,38 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_UNIFORM_DENSITY_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_UNIFORM_DENSITY_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + /** + * Set Uniform Density. + * + * @param begin + * @param end + * @param density + */ + template + inline void uniform_density(tetrahedron_iterator const & begin, tetrahedron_iterator const & end,real_type const & density) + { + assert(density>0 || !"uniform_density(): density must be positive"); + for (tetrahedron_iterator T = begin;T!=end;++T) + T->m_density = density; + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_UNIFORM_DENSITY_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_uniform_poisson.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_uniform_poisson.h new file mode 100644 index 000000000..204d9cdd8 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_uniform_poisson.h @@ -0,0 +1,44 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_UNIFORM_POISSON_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_UNIFORM_POISSON_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + + /** + * + * @param begin + * @param end + * @param poisson + */ + template + inline void uniform_poisson( + tetrahedron_iterator const & begin + , tetrahedron_iterator const & end + , real_type const & poisson + ) + { + assert(poisson>=0 || !"uniform_poisson() : Poisson ratio must be non-negative"); + assert(poisson<=0.5 || !"uniform_poisson() : Poisson ratio must not be larger than a half"); + + for (tetrahedron_iterator T = begin;T!=end;++T) + T->m_poisson = poisson; + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_UNIFORM_POISSON_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_uniform_young.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_uniform_young.h new file mode 100644 index 000000000..84ff51f1c --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_uniform_young.h @@ -0,0 +1,43 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_UNIFORM_YOUNG_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_UNIFORM_YOUNG_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + + /** + * + * @param begin + * @param end + * @param young + */ + template + inline void uniform_young( + tetrahedron_iterator const & begin + , tetrahedron_iterator const & end + ,real_type const & young + ) + { + assert(young>=0 || !"uniform_young(): Young modulus must not be negative"); + + for (tetrahedron_iterator T = begin;T!=end;++T) + T->m_young = young; + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_UNIFORM_YOUNG_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_update_orientation.h b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_update_orientation.h new file mode 100644 index 000000000..3717133ed --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/dynamics/fem/fem_update_orientation.h @@ -0,0 +1,222 @@ +#ifndef OPENTISSUE_DYNAMICS_FEM_FEM_UPDATE_ORIENTATION_H +#define OPENTISSUE_DYNAMICS_FEM_FEM_UPDATE_ORIENTATION_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace fem + { + namespace detail + { + + /** + * + */ + template + inline void update_orientation_single(tetrahedron_type* T) + { + typedef typename tetrahedron_type::real_type real_type; + typedef typename tetrahedron_type::vector3_type vector3_type; + typedef typename tetrahedron_type::matrix3x3_type matrix3x3_type; + + { + real_type div6V = 1.0 / T->m_V*6.0; + //--- The derivation in the orignal paper on stiffness warping were stated as + //--- + //--- | n1^T | + //--- N = | n2^T | : WCS -> U + //--- | n3^T | + //--- + //--- | n1'^T | + //--- N' = | n2'^T | : WCS -> D + //--- | n3'^T | + //--- + //--- From which we have + //--- + //--- R = N' N^T + //--- + //--- This is valid under the assumption that the n-vectors form a orthonormal basis. In that + //--- paper the n-vectors were determined using a heuristic approach. Besides + //--- the rotation were computed on a per vertex basis not on a per-tetrahedron + //--- bases as we have outline above. + //--- + //--- + //--- Later Muller et. al. used barycentric coordinates to find the transform. We + //--- will here go into details on this method. Let the deformed corners be q0, q1, + //--- q2, and q3 and the undeformed corners p0, p1, p2,and p3. Looking at some + //--- point p = [x y z 1]^T inside the undeformed tetrahedron this can be written + //--- + //--- | w0 | + //--- p = | p0 p1 p2 p3 | | w1 | = P w (*1) + //--- | 1 1 1 1 | | w2 | + //--- | w3 | + //--- + //--- The same point in the deformed tetrahedron has the same barycentric coordinates + //--- which mean + //--- + //--- | w0 | + //--- q = | q0 q1 q2 q3 | | w1 | = Q w (*2) + //--- | 1 1 1 1 | | w2 | + //--- | w3 | + //--- + //--- We can now use (*1) to solve for w and insert this into (*2), this yields + //--- + //--- q = Q P^{-1} p + //--- + //--- The matirx Q P^{-1} transforms p into q. Due to P and Q having their fourth rows + //--- equal to 1 it can be shown that this matrix have the block structure + //--- + //--- Q P^{-1} = | R t | (*3) + //--- | 0^T 1 | + //--- + //--- Which we recognize as a transformation matrix of homegeneous coordinates. The t vector + //--- gives the translation, the R matrix includes, scaling, shearing and rotation. + //--- + //--- We thus need to extract the rotational information from this R matrix. In their + //--- paper Mueller et. al. suggest using Polar Decompostions (cite Shoemake and Duff; Etzmuss). + //--- However, a simpler although more imprecise approach would simply be to apply a Grahram-Schimdt + //--- orthonormalization to transform R into an orthonormal matrix. This seems to work quite well + //--- in practice. We have not observed any visual difference in using polar decomposition or + //--- orthonormalization. + //--- + //--- Now for some optimizations. Since we are only interested in the R part of (*3) we can + //--- compute this more efficiently exploiting the fact that barycentric coordinates sum + //--- up to one. If we substitute w0 = 1 - w1 - w2 - w3 in (*1) and (*2) we can throw away + //--- the fourth rows since they simply state 1=1, which is trivially true. + //--- + //--- | 1-w1-w2-w3 | + //--- p = | p0 p1 p2 p3 | | w1 | + //--- | w2 | + //--- | w3 | + //--- + //--- Which is + //--- + //--- | 1 | + //--- p = | p0 (p1-p0) (p2-p0) (p3-p0) | | w1 | + //--- | w2 | + //--- | w3 | + //--- + //--- We can move the first column over on the left hand side + //--- + //--- | w1 | + //--- (p-p0) = | (p1-p0) (p2-p0) (p3-p0) | | w2 | + //--- | w3 | + //--- + //--- Introducing e10 = p1-p0, e20 = p2-p0, e30 = p3-p0, and E = [e10 e20 e30 ] we have + //--- + //--- w1 + //--- (p-p0) = E w2 (*5) + //--- w3 + //--- + //--- Similar for *(2) we have + //--- + //--- w1 + //--- (q-q0) = E' w2 (*6) + //--- w3 + //--- + //--- Where E' = [e10' e20' e3'] and e10' = q1-q0, e20' = q2-q0, e30' = q3-q0. Now + //--- inverting (*5) and insertion into (*6) yields + //--- + //--- (q-q0) = E' E^{-1} (p-p0) + //--- + //--- By comparison with (*3) we see that + //--- + //--- R = E' E^{-1} + //--- + //--- Using Cramers rule the inverse of E can be written as + //--- + //--- | (e2 x e3)^T | + //--- E^{-1} = 1/6V | (e3 x e1)^T | + //--- | (e1 x e2)^T | + //--- + //--- This can easily be confirmed by straigthforward computation + //--- + //--- | e1 \cdot (e2 x e3) e2 \cdot (e2 x e3) e3 \cdot (e2 x e3) | + //--- E^{-1} E = 1/6v | e1 \cdot (e3 x e1) e2 \cdot (e3 x e1) e3 \cdot (e3 x e1) | + //--- | e1 \cdot (e1 x e2) e2 \cdot (e1 x e2) e3 \cdot (e1 x e2) | + //--- + //--- | 6V 0 0 | + //--- = 1/6V | 0 6V 0 | = I + //--- | 0 0 6V | + //--- + //--- Using the notation + //--- + //--- n1 = e2 x e3 / 6V + //--- n2 = e3 x e1 / 6V + //--- n3 = e1 x e2 / 6V + //--- + //--- we write + //--- + //--- | n1^T | + //--- E^{-1} = | n2^T | + //--- | n3^T | + //--- + //--- And we end up with + //--- + //--- | n1^T | + //--- R = [e10' e20' e30'] | n2^T | + //--- | n3^T | + //--- + //--- Observe that E' is very in-expensive to compute and all non-primed quantities (n1, n2, n3) can + //--- be precomputed and stored on a per tetrahedron basis if there is enough memory available. Even + //--- in case where memory is not available n1, n2 and n3 are quite cheap to compute. + //--- + + real_type e1x = T->m_e10(0); + real_type e1y = T->m_e10(1); + real_type e1z = T->m_e10(2); + real_type e2x = T->m_e20(0); + real_type e2y = T->m_e20(1); + real_type e2z = T->m_e20(2); + real_type e3x = T->m_e30(0); + real_type e3y = T->m_e30(1); + real_type e3z = T->m_e30(2); + real_type n1x = (e2y * e3z - e3y * e2z) * div6V; + real_type n1y = (e3x * e2z - e2x * e3z) * div6V; + real_type n1z = (e2x * e3y - e3x * e2y) * div6V; + real_type n2x = (e1z * e3y - e1y * e3z) * div6V; + real_type n2y = (e1x * e3z - e1z * e3x) * div6V; + real_type n2z = (e1y * e3x - e1x * e3y) * div6V; + real_type n3x = (e1y * e2z - e1z * e2y) * div6V; + real_type n3y = (e1z * e2x - e1x * e2z) * div6V; + real_type n3z = (e1x * e2y - e1y * e2x) * div6V; + const vector3_type & p0 = T->m_owner->m_nodes[T->m_nodes[0]].m_coord; + const vector3_type & p1 = T->m_owner->m_nodes[T->m_nodes[1]].m_coord; + const vector3_type & p2 = T->m_owner->m_nodes[T->m_nodes[2]].m_coord; + const vector3_type & p3 = T->m_owner->m_nodes[T->m_nodes[3]].m_coord; + + e1x = p1(0) - p0(0); + e1y = p1(1) - p0(1); + e1z = p1(2) - p0(2); + e2x = p2(0) - p0(0); + e2y = p2(1) - p0(1); + e2z = p2(2) - p0(2); + e3x = p3(0) - p0(0); + e3y = p3(1) - p0(1); + e3z = p3(2) - p0(2); + T->m_Re(0,0) = e1x * n1x + e2x * n2x + e3x * n3x; T->m_Re(0,1) = e1x * n1y + e2x * n2y + e3x * n3y; T->m_Re(0,2) = e1x * n1z + e2x * n2z + e3x * n3z; + T->m_Re(1,0) = e1y * n1x + e2y * n2x + e3y * n3x; T->m_Re(1,1) = e1y * n1y + e2y * n2y + e3y * n3y; T->m_Re(1,2) = e1y * n1z + e2y * n2z + e3y * n3z; + T->m_Re(2,0) = e1z * n1x + e2z * n2x + e3z * n3x; T->m_Re(2,1) = e1z * n1y + e2z * n2y + e3z * n3y; T->m_Re(2,2) = e1z * n1z + e2z * n2z + e3z * n3z; + + T->m_Re = ortonormalize(T->m_Re); + + //matrix3x3_type M = T->m_Re,S; + //OpenTissue::PolarDecomposition3x3 decomp; + //decomp.eigen(M, T->m_Re, S);//--- Etzmuss-style + //decomp.newton(M, T->m_Re );//--- Shoemake-Duff-style + } + } + + } // namespace detail + } // namespace fem +} // namespace OpenTissue + +//OPENTISSUE_DYNAMICS_FEM_FEM_UPDATE_ORIENTATION_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_class_id.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_class_id.h new file mode 100644 index 000000000..e3f868bee --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_class_id.h @@ -0,0 +1,162 @@ +#ifndef OPENTISSUE_UTILITY_UTILITY_CLASS_ID_H +#define OPENTISSUE_UTILITY_UTILITY_CLASS_ID_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace utility + { + + /** @internal ID Generator class. + * This class encapsulates the generation of unique IDs as used by ClassID. + * + * @warning + * This class should never be used directly. + */ + class BaseIDGenerator + { + + template < class T > + friend class ClassID; + + private: + + BaseIDGenerator(); // Disable default constructor + + /** ID generation method + * + * @return + * An increasing unique ID. + */ + static size_t const generate_id() + { + static size_t id = 0; + return id++; + } + + }; + + + /** The Class ID interface. + */ + class ClassIDInterface + { + public: + + /** Destructor + */ + virtual ~ClassIDInterface(){} + + /** Query the class ID. + * + * @return + * The object's class ID + */ + virtual size_t const class_id() const = 0; + }; + + + + /** + * CRTP Implementation of the ClassIDInterface. + * + * Usage: + * To enable class IDs for a custom class, simply inherite this class giving + * itself as the template argument. + * + * Example: + * \code + * class SomeClass: + * public ClassID + * { ... }; + * \endcode + * + * @tparam T + * The deriving class + */ + template < class T > + class ClassID + : virtual public ClassIDInterface + { + public: + + /** + * Query the object ID. + * + * @return + * The object ID + */ + static size_t const id() + { + static size_t my_id = BaseIDGenerator::generate_id(); + return my_id; + } + + /** + */ + virtual size_t const class_id() const + { + return id(); + } + }; + + /** Helper class for solving ambiguousity between class IDs. + * This class enables inheritance from another class, which already have a class ID. + * + * Problem: + * \code + * class SomeBase: + * public ClassID + * { ... }; + * class SomeClass: + * public SomeBase + * , public ClassID + * { ... }; // Error: id() and class_id() will be ambiguous + * \endcode + * + * Solution: + * \code + * class SomeClass: + * public ClassIDCompositor + * { ... }; + * \endcode + * + * @tparam Base + * Some base class, which will also be inherited + * @tparam Self + * The deriving class + */ + template < class Base, class Self > + class ClassIDCompositor + : public Base + , public ClassID + { + public: + + /** + */ + static size_t const id() + { + return ClassID::id(); + } + + /** + */ + virtual size_t const class_id() const + { + return ClassID::id(); + } + }; + + } // namespace utility +} // namespace OpenTissue +// OPENTISSUE_UTILITY_UTILITY_CLASS_ID_H +#endif + diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_empty_traits.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_empty_traits.h new file mode 100644 index 000000000..bc0c4506a --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_empty_traits.h @@ -0,0 +1,30 @@ +#ifndef OPENTISSUE_UTILITY_UTILITY_EMPTY_TRAITS_H +#define OPENTISSUE_UTILITY_UTILITY_EMPTY_TRAITS_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// + +namespace OpenTissue +{ + namespace utility + { + /** + * Empty Traits Class. + * This is basically an empty class it usage + * is intended as a default argument for the + * many algorithms and data structures where + * the default traits is simply the ``empty'' + * traits. + * + */ + class EmptyTraits { }; + + } +} + +// OPENTISSUE_UTILITY_UTILITY_EMPTY_TRAITS_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_fps_counter.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_fps_counter.h new file mode 100644 index 000000000..29a8caa1b --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_fps_counter.h @@ -0,0 +1,73 @@ +#ifndef OPENTISSUE_UTILITY_UTILITY_FPS_COUNTER_H +#define OPENTISSUE_UTILITY_UTILITY_FPS_COUNTER_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace utility + { + /* + * Frame Per Second (FPS) Counter + */ + template + class FPSCounter + { + public: + FPSCounter() + : m_fps(0) + , m_fpscnt(0) + , m_time(0.) + { + m_hrc.start(); + } + + /* + * /return the last known fps + */ + unsigned long operator()() const + { + return m_fps; + + } + + /* + * /return true if 1 sec has passed + */ + bool frame() + { + m_hrc.stop(); + m_time += m_hrc(); + m_fpscnt++; + if (m_time >= 1.) { + // m_time -= 1.; + m_time = 0.; + m_fps = m_fpscnt; + m_fpscnt = 0; + } + m_hrc.start(); + return 0 == m_fpscnt; + } + + private: + + Timer m_hrc; + unsigned long m_fps; + unsigned long m_fpscnt; + double m_time; + }; + + } //End of namespace utility + +} //End of namespace OpenTissue + +// OPENTISSUE_UTILITY_UTILITY_FPS_COUNTER_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_get_environment_variable.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_get_environment_variable.h new file mode 100644 index 000000000..3159fe267 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_get_environment_variable.h @@ -0,0 +1,44 @@ +#ifndef OPENTISSUE_UTILITY_UTILITY_GET_ENVIRONMENT_VARIABLE_H +#define OPENTISSUE_UTILITY_UTILITY_GET_ENVIRONMENT_VARIABLE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +#include //--- For getenv +#include +#include + +namespace OpenTissue +{ + namespace utility + { + + /** + * Retrieve Value of Environment Variable. + * If environment variable can not be found then a std::logic_error warning is thrown. + * + * @param A string containing the name of the environment + * variable. Example: "OPENTISSUE". + * @return A string containing the value. + */ + inline std::string get_environment_variable(std::string const & name) + { + char * value = getenv( name.c_str() ); + if(value) + return std::string( value ); + throw std::logic_error("get_environment_variable(): the specified environment variable was not set"); + } + + } // namespace utility + +} // namespace OpenTissue + +//OPENTISSUE_UTILITY_UTILITY_GET_ENVIRONMENT_VARIABLE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_get_system_memory_info.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_get_system_memory_info.h new file mode 100644 index 000000000..41609942e --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_get_system_memory_info.h @@ -0,0 +1,108 @@ +#ifndef OPENTISSUE_UTILITY_UTILITY_GET_SYSTEM_MEMORY_INFO_H +#define OPENTISSUE_UTILITY_UTILITY_GET_SYSTEM_MEMORY_INFO_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#ifdef WIN32 +# define NOMINMAX +# define WIN32_LEAN_AND_MEAN +# include +# undef WIN32_LEAN_AND_MEAN +# undef NOMINMAX +#endif + +namespace OpenTissue +{ + namespace utility + { + +#ifdef WIN32 + + double get_memory_usage() + { + typedef double real_type; + MEMORYSTATUSEX statex; + statex.dwLength = sizeof (statex); + GlobalMemoryStatusEx (&statex); + real_type memory_usage = statex.dwMemoryLoad; + return memory_usage; + } + + unsigned int get_total_physical_memory_in_bytes() + { + MEMORYSTATUSEX statex; + statex.dwLength = sizeof (statex); + GlobalMemoryStatusEx (&statex); + unsigned int total_physical_memory_in_bytes = statex.ullTotalPhys; + return total_physical_memory_in_bytes; + } + + unsigned int get_free_physical_memory_in_bytes() + { + MEMORYSTATUSEX statex; + statex.dwLength = sizeof (statex); + GlobalMemoryStatusEx (&statex); + unsigned int free_physical_memory_in_bytes = statex.ullAvailPhys; + return free_physical_memory_in_bytes; + } + + unsigned int get_total_page_file_in_bytes() + { + MEMORYSTATUSEX statex; + statex.dwLength = sizeof (statex); + GlobalMemoryStatusEx (&statex); + unsigned int total_page_file_in_bytes = statex.ullTotalPageFile; + return total_page_file_in_bytes; + } + + unsigned int get_free_page_file_in_bytes() + { + MEMORYSTATUSEX statex; + statex.dwLength = sizeof (statex); + GlobalMemoryStatusEx (&statex); + unsigned int free_page_file_in_bytes = statex.ullAvailPageFile; + return free_page_file_in_bytes; + } + + unsigned int get_total_virtual_memory_in_bytes() + { + MEMORYSTATUSEX statex; + statex.dwLength = sizeof (statex); + GlobalMemoryStatusEx (&statex); + unsigned int total_virtual_memory = statex.ullTotalVirtual; + return total_virtual_memory; + } + + unsigned int get_free_virtual_memory_in_bytes() + { + MEMORYSTATUSEX statex; + statex.dwLength = sizeof (statex); + GlobalMemoryStatusEx (&statex); + unsigned int free_virtual_memory_in_bytes = statex.ullAvailVirtual; + return free_virtual_memory; + } + + unsigned int get_free_extended_memory_in_bytes() + { + MEMORYSTATUSEX statex; + statex.dwLength = sizeof (statex); + GlobalMemoryStatusEx (&statex); + unsigned int free_extended_memory_in_bytes = statex.ullAvailExtendedVirtual; + return free_extended_memory_in_bytes; + } + + +#endif + + } //End of namespace utility + +} //End of namespace OpenTissue + +// OPENTISSUE_UTILITY_UTILITY_GET_SYSTEM_MEMORY_INFO_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_greater_ptr.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_greater_ptr.h new file mode 100644 index 000000000..9b7c6c405 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_greater_ptr.h @@ -0,0 +1,33 @@ +#ifndef OPENTISSUE_UTILITY_UTILITY_GREATER_PTR_H +#define OPENTISSUE_UTILITY_UTILITY_GREATER_PTR_H +// +// OpenTissue, A toolbox for physical based simulation and animation. +// Copyright (C) 2007 Department of Computer Science, University of Copenhagen +// +#include + +#include + +namespace OpenTissue +{ + namespace utility + { + + /** + * Greater Than Test. + * This is used for doing a greater than test on data + * structures containing pointers to data and not instances. + */ + template + struct greater_ptr + : public std::binary_function + { + bool operator()(const T& x, const T& y) const { return (*x)>(*y); } + }; + + } //End of namespace utility + +} //End of namespace OpenTissue + +// OPENTISSUE_UTILITY_UTILITY_GREATER_PTR_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_identifier.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_identifier.h new file mode 100644 index 000000000..b1ebbc75a --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_identifier.h @@ -0,0 +1,100 @@ +#ifndef OPENTISSUE_UTILITY_UTILITY_IDENTIFIER_H +#define OPENTISSUE_UTILITY_UTILITY_IDENTIFIER_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#include + +namespace OpenTissue +{ + namespace utility + { + + /** + * Identifier Class. + * This is an utility class that is supposed to make it easier to create + * new objects with unique identifiers. The idea is to simply inherit the + * new object class from the identifier class. + * + * At construction-time the class tries to generate a new unique + * identifier. Hereafter it is the users responsibility not to mess + * up the id-name string. However, end-users can always be sure that + * the index value provided by the identifier class is unique. + */ + class Identifier + { + protected: + + std::string m_ID; ///< An Identifier string. + size_t m_index; ///< Unique object index. + + public: + + Identifier() + { + generate_new_index(); + m_ID = "ID" + m_index; + } + + virtual ~Identifier(){} + + public: + + Identifier(Identifier const & id) + { + m_ID = id.m_ID; + m_index = id.m_index; + } + + Identifier & operator=(Identifier const & id) + { + m_ID = id.m_ID; + m_index = id.m_index; + return *this; + } + + bool operator==(Identifier const & id) const { return (m_index == id.m_index); } + + bool operator!=(Identifier const & id) const { return !(*this == id); } + + public: + + void set_id(std::string const & id) { m_ID = id; } + + std::string const & get_id() const { return m_ID; } + + size_t get_index() const {return m_index; } + + protected: + + /** + * Static Member Not Initialized in Header Trick (smnih) + * + * This way we avoid having a + * + * static size_t m_next_idx + * + * member variable which must be initialized in a + * source file. + */ + void generate_new_index() + { + static size_t next_index = 0; + m_index = next_index; + ++next_index; + } + + }; + + } // namespace utility + +} // namespace OpenTissue + +// OPENTISSUE_UTILITY_UTILITY_IDENTIFIER_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_less_ptr.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_less_ptr.h new file mode 100644 index 000000000..eab2753b9 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_less_ptr.h @@ -0,0 +1,33 @@ +#ifndef OPENTISSUE_UTILITY_UTILITY_LESS_PTR_H +#define OPENTISSUE_UTILITY_UTILITY_LESS_PTR_H +// +// OpenTissue, A toolbox for physical based simulation and animation. +// Copyright (C) 2007 Department of Computer Science, University of Copenhagen +// +#include + +#include + +namespace OpenTissue +{ + namespace utility + { + + /** + * Less Than Test. + * This is used for doing a less than test on data + * structures containing pointers to data and not instances. + */ + template + struct less_ptr + : public std::binary_function + { + bool operator()(const T& x, const T& y) const { return (*x)<(*y); } + }; + + } //End of namespace utility + +} //End of namespace OpenTissue + +// OPENTISSUE_UTILITY_UTILITY_LESS_PTR_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_material.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_material.h new file mode 100644 index 000000000..b1ad90852 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_material.h @@ -0,0 +1,108 @@ +#ifndef OPENTISSUE_UTILITY_UTILITY_MATERIAL_H +#define OPENTISSUE_UTILITY_UTILITY_MATERIAL_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace utility + { + + /** + * A Material. + * This class encapsulates openGL material parameters. + */ + class Material + { + protected: + + float m_ambient[ 4 ]; ///< The ambient color rgba. + float m_diffuse[ 4 ]; ///< The diffuse color rgba. + float m_specular[ 4 ]; ///< The specular color rgba. + float m_shininess; ///< The shininess of this material 0..180. + + public: + + Material() + : m_shininess(128) + { + set_default(); + } + + void ambient( float const & red, float const & green, float const & blue, float const & alpha = 1.0f ) + { + m_ambient[ 0 ] = red; + m_ambient[ 1 ] = green; + m_ambient[ 2 ] = blue; + m_ambient[ 3 ] = alpha; + } + + void diffuse( float const & red, float const & green, float const & blue, float const & alpha = 1.0f ) + { + m_diffuse[ 0 ] = red; + m_diffuse[ 1 ] = green; + m_diffuse[ 2 ] = blue; + m_diffuse[ 3 ] = alpha; + } + + void specular( float const & red, float const & green, float const & blue, float const & alpha = 1.0f ) + { + m_specular[ 0 ] = red; + m_specular[ 1 ] = green; + m_specular[ 2 ] = blue; + m_specular[ 3 ] = alpha; + } + + void shininess( float const & value ) + { + m_shininess = value; + } + + /* + void use() + { + if ( glIsEnabled( GL_COLOR_MATERIAL ) || !glIsEnabled( GL_LIGHTING ) ) + { + glColor4f( m_diffuse[ 0 ], m_diffuse[ 1 ], m_diffuse[ 2 ], m_diffuse[ 3 ] ); + } + else + { + glMaterialfv( GL_FRONT_AND_BACK, GL_AMBIENT, m_ambient ); + glMaterialfv( GL_FRONT_AND_BACK, GL_DIFFUSE, m_diffuse ); + glMaterialfv( GL_FRONT_AND_BACK, GL_SPECULAR, m_specular ); + glMaterialfv( GL_FRONT_AND_BACK, GL_SHININESS, &m_shininess ); + } + } + */ + + void set_default() + { + m_ambient[ 0 ] = 0.1f; + m_ambient[ 1 ] = 0.1f; + m_ambient[ 2 ] = 0.0f; + m_ambient[ 3 ] = 1.0f; + m_diffuse[ 0 ] = 0.6f; + m_diffuse[ 1 ] = 0.6f; + m_diffuse[ 2 ] = 0.1f; + m_diffuse[ 3 ] = 1.0f; + m_specular[ 0 ] = 1.0f; + m_specular[ 1 ] = 1.0f; + m_specular[ 2 ] = 0.75f; + m_specular[ 3 ] = 1.0f; + m_shininess = 128; + } + + }; + + } // namespace utility + +} // namespace OpenTissue + +//OPENTISSUE_UTILITY_UTILITY_MATERIAL_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_pop_boost_filter.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_pop_boost_filter.h new file mode 100644 index 000000000..74df18a09 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_pop_boost_filter.h @@ -0,0 +1,10 @@ +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#if (_MSC_VER >= 1200) +# pragma warning(pop) +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_push_boost_filter.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_push_boost_filter.h new file mode 100644 index 000000000..ba9a26e42 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_push_boost_filter.h @@ -0,0 +1,23 @@ +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#if (_MSC_VER >= 1200) +# pragma warning(push) +//# pragma warning(disable: 99) // 'identifier' : type name first seen using 'objecttype1' now seen using 'objecttype2' +//# pragma warning(disable: 100) // 'identifier' : unreferenced formal parameter +//# pragma warning(disable: 127) // conditional expression is constant +//# pragma warning(disable: 217) // 'operator' : member template functions cannot be used for copy-assignment or copy-construction +# pragma warning(disable: 265) // 'class' : class has virtual functions, but destructor is not virtual +//# pragma warning(disable: 267) // 'var' : conversion from 'size_t' to 'type', possible loss of data +//# pragma warning(disable: 347) // behavior change: 'function template' is called instead of 'function' +//# pragma warning(disable: 390) // ';' : empty controlled statement found; is this the intent? +//# pragma warning(disable: 503) // 'identifier' : decorated name length exceeded, name was truncated +//# pragma warning(disable: 511) // 'class' : copy constructor could not be generated +//# pragma warning(disable: 512) // 'class' : assignment operator could not be generated +//# pragma warning(disable: 701) // Potentially uninitialized local variable 'name' used +//# pragma warning(disable: 4355) // 'this' : used in base member initializer list +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_qhull.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_qhull.h new file mode 100644 index 000000000..f48f343a3 --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_qhull.h @@ -0,0 +1,45 @@ +#ifndef OPENTISSUE_UTILITY_UTILITY_QHULL_H +#define OPENTISSUE_UTILITY_UTILITY_QHULL_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + + ////////////////////////////////////////////////////////////////// + // + // This is a bit tricky, I cant just include qhull_a.h as + // QHull advices, because MVC complains about math.h which + // is included "indirectly by qhull_a.h + // + // Hopefully furture releases of QHull will allow me to + // just write: + // + // extern "C" + // { + // #include + // } + // +#if defined(__cplusplus) + extern "C" + { +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if defined(__cplusplus) + } +#endif + +//OPENTISSUE_UTILITY_UTILITY_QHULL_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_runtime_type.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_runtime_type.h new file mode 100644 index 000000000..7dfe41c2c --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_runtime_type.h @@ -0,0 +1,37 @@ +#ifndef OPENTISSUE_UTILITY_UTILITY_RUNTIME_TYPE_H +#define OPENTISSUE_UTILITY_UTILITY_RUNTIME_TYPE_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +namespace OpenTissue +{ + namespace utility + { + /** + * Runtime type. + * great for passing non-integral values as template arguments. + */ + template < typename Type > + struct RuntimeType + { + typedef Type type; + RuntimeType(){} + RuntimeType(const RuntimeType& rhs):m_value(rhs.m_value){} + RuntimeType& operator = (const type& rhs) {m_value=rhs; return *this;} + operator const type& () const {return m_value;} + protected: + type m_value; + }; + + } // namespace utility + +} // namespace OpenTissue + +// OPENTISSUE_UTILITY_UTILITY_RUNTIME_TYPE_H +#endif diff --git a/Demos3/FiniteElementMethod/OpenTissue/utility/utility_timer.h b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_timer.h new file mode 100644 index 000000000..69b07c1cc --- /dev/null +++ b/Demos3/FiniteElementMethod/OpenTissue/utility/utility_timer.h @@ -0,0 +1,102 @@ +#ifndef OPENTISSUE_UTILITY_UTILITY_TIMER_H +#define OPENTISSUE_UTILITY_UTILITY_TIMER_H +// +// OpenTissue Template Library +// - A generic toolbox for physics-based modeling and simulation. +// Copyright (C) 2008 Department of Computer Science, University of Copenhagen. +// +// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php +// +#include + +#ifdef WIN32 +# define NOMINMAX +# define WIN32_LEAN_AND_MEAN +# include +# undef WIN32_LEAN_AND_MEAN +# undef NOMINMAX +#else +# include +#endif + +#include + +namespace OpenTissue +{ + namespace utility + { + + /** + * High Resoultion Timer. + * Based on http://www-106.ibm.com/developerworks/library/l-rt1/ + * + * RunTime: High-performance programming techniques on Linux and Windows 2000 + * Setting up timing routines + * by + * Edward G. Bradford (egb@us.ibm.com) + * Senior Programmer, IBM + * 01 Apr 2001 + * + * Example usage (We recommand doubles): + * + * Timer timer; + * + * timer.start() + * ... + * timer.stop() + * std::cout << "It took " << timer() << " seconds to do it" << std::endl; + */ + template + class Timer + { +#ifdef WIN32 + private: + LARGE_INTEGER m_start; ///< + LARGE_INTEGER m_end; ///< + LARGE_INTEGER m_freq; ///< + bool m_first; ///< + public: + Timer():m_first(true){} + public: + void start() + { + if(m_first) + { + QueryPerformanceFrequency(&m_freq); + m_first = false; + } + QueryPerformanceCounter(&m_start); + } + void stop() + { + QueryPerformanceCounter(&m_end); + } + real_type operator()()const + { + real_type end = static_cast(m_end.QuadPart); + real_type start = static_cast(m_start.QuadPart); + real_type freq = static_cast(m_freq.QuadPart); + return (end - start)/ freq; + } +#else + private: + struct timeval m_start; ///< + struct timeval m_end; ///< + struct timezone m_tz; ///< + public: + void start() { gettimeofday(&m_start, &m_tz); } + void stop() { gettimeofday(&m_end,&m_tz); } + real_type operator()()const + { + real_type t1 = static_cast(m_start.tv_sec) + static_cast(m_start.tv_usec)/(1000*1000); + real_type t2 = static_cast(m_end.tv_sec) + static_cast(m_end.tv_usec)/(1000*1000); + return t2-t1; + } +#endif + }; + + } //End of namespace utility +} //End of namespace OpenTissue + +// OPENTISSUE_UTILITY_UTILITY_TIMER_H +#endif diff --git a/Demos3/ImportColladaDemo/ImportColladaSetup.cpp b/Demos3/ImportColladaDemo/ImportColladaSetup.cpp index 14862cc19..342883860 100644 --- a/Demos3/ImportColladaDemo/ImportColladaSetup.cpp +++ b/Demos3/ImportColladaDemo/ImportColladaSetup.cpp @@ -70,13 +70,12 @@ void ImportColladaSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) btVector3 scaling(1,1,1); // int index=10; - fileIndex++; - if (fileIndex>=numFiles) - { - fileIndex = 0; - } + + + + { btAlignedObjectArray visualShapes; @@ -86,8 +85,36 @@ void ImportColladaSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) btTransform upAxisTrans; upAxisTrans.setIdentity(); + btVector3 color(0,0,1); +#ifdef COMPARE_WITH_ASSIMP + static int useAssimp = 0; + if (useAssimp) + { + + LoadMeshFromColladaAssimp(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling); + fileIndex++; + if (fileIndex>=numFiles) + { + fileIndex = 0; + } + color.setValue(1,0,0); + } + else + { + LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling); + + } + useAssimp=1-useAssimp; +#else + fileIndex++; + if (fileIndex>=numFiles) + { + fileIndex = 0; + } LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling); +#endif// COMPARE_WITH_ASSIMP + //at the moment our graphics engine requires instances that share the same visual shape to be added right after registering the shape //so perform a sort, just to be sure @@ -111,7 +138,7 @@ void ImportColladaSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) //trans.setIdentity(); //trans.setRotation(btQuaternion(btVector3(1,0,0),SIMD_HALF_PI)); - btVector3 color(0,0,1); + b3AlignedObjectArray verts; verts.resize(gfxShape->m_vertices->size()); diff --git a/Demos3/ImportColladaDemo/LoadMeshFromCollada.cpp b/Demos3/ImportColladaDemo/LoadMeshFromCollada.cpp index 5991c95b7..864e88ee1 100644 --- a/Demos3/ImportColladaDemo/LoadMeshFromCollada.cpp +++ b/Demos3/ImportColladaDemo/LoadMeshFromCollada.cpp @@ -28,6 +28,8 @@ subject to the following restrictions: #include "btMatrix4x4.h" + + struct VertexSource { std::string m_positionArrayId; @@ -266,9 +268,16 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArraynormalIndex)) + { + vertexNormals.push_back(btVector3(normalFloatArray[normalIndex*3+0], normalFloatArray[normalIndex*3+1], normalFloatArray[normalIndex*3+2])); + } else + { + //add a dummy normal of length zero, so it is easy to detect that it is an invalid normal + vertexNormals.push_back(btVector3(0,0,0)); + } } int curNumIndices = indices.size(); indices.resize(curNumIndices+numIndices); @@ -545,3 +554,159 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray +#include +#include +#include + + +# include "assimp/ColladaLoader.h" +//# include "STLLoader.h" +# include "assimp/SortByPTypeProcess.h" +# include "assimp/LimitBoneWeightsProcess.h" +# include "assimp/TriangulateProcess.h" +# include "assimp/JoinVerticesProcess.h" +# include "assimp/RemoveVCProcess.h" + + +namespace Assimp { + // ------------------------------------------------------------------------------------------------ +void GetImporterInstanceList(std::vector< BaseImporter* >& out) + { + out.push_back( new ColladaLoader()); + } + // ------------------------------------------------------------------------------------------------ +void GetPostProcessingStepInstanceList(std::vector< BaseProcess* >& out) + { + out.push_back( new SortByPTypeProcess()); + out.push_back( new LimitBoneWeightsProcess()); + out.push_back( new TriangulateProcess()); + out.push_back( new JoinVerticesProcess()); + //out.push_back( new RemoveVCProcess()); + } + +} + +static void addMeshParts(const aiScene* scene, const aiNode* node, GLInstanceGraphicsShape* outverts, const aiMatrix4x4& parentTr) +{ + aiMatrix4x4 const& nodeTrans(node->mTransformation); + + aiMatrix4x4 trans; + trans = parentTr * nodeTrans; + + for (size_t i = 0; i < node->mNumMeshes; ++i) + { + aiMesh const* mesh = scene->mMeshes[node->mMeshes[i]]; + size_t num_vertices = mesh->mNumVertices; + if (mesh->mPrimitiveTypes==aiPrimitiveType_TRIANGLE) + { + int curVertexBase = outverts->m_vertices->size(); + + for (int v=0;vmNumVertices;v++) + { + GLInstanceVertex vtx; + aiVector3D vWorld = trans*mesh->mVertices[v]; + vtx.xyzw[0] = vWorld.x; + vtx.xyzw[1] = vWorld.y; + vtx.xyzw[2] = vWorld.z; + vtx.xyzw[3] = 1; + if (mesh->HasNormals()) + { + vtx.normal[0] = mesh->mNormals[v].x; + vtx.normal[1] = mesh->mNormals[v].y; + vtx.normal[2] = mesh->mNormals[v].z; + } else + { + vtx.normal[0] = 0; + vtx.normal[1] = 0; + vtx.normal[2] = 1; + } + if (mesh->HasTextureCoords(0)) + { + vtx.uv[0] = mesh->mTextureCoords[0][v].x; + vtx.uv[1] = mesh->mTextureCoords[0][v].y; + } else + { + vtx.uv[0]=0.5f; + vtx.uv[1]=0.5f; + } + outverts->m_vertices->push_back(vtx); + } + for (int f=0;fmNumFaces;f++) + { + b3Assert(mesh->mFaces[f].mNumIndices == 3); + int i0 = mesh->mFaces[f].mIndices[0]; + int i1 = mesh->mFaces[f].mIndices[1]; + int i2 = mesh->mFaces[f].mIndices[2]; + outverts->m_indices->push_back(i0+curVertexBase); + outverts->m_indices->push_back(i1+curVertexBase); + outverts->m_indices->push_back(i2+curVertexBase); + } + } + } + for (size_t i=0 ; imNumChildren ; ++i) { + addMeshParts(scene,node->mChildren[i], outverts, trans); + } +} + + +void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArray& visualShapes, btAlignedObjectArray& visualShapeInstances,btTransform& upAxisTrans, float& unitMeterScaling) +{ + upAxisTrans.setIdentity(); + unitMeterScaling=1; + + GLInstanceGraphicsShape* shape = 0; + + + FILE* file = fopen(relativeFileName,"rb"); + if (file) + { + int size=0; + if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET)) + { + printf("Error: Cannot access file to determine size of %s\n", relativeFileName); + } else + { + if (size) + { + printf("Open DAE file of %d bytes\n",size); + + Assimp::Importer importer; + //importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, aiComponent_NORMALS | aiComponent_COLORS); + importer.SetPropertyInteger(AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_LINE | aiPrimitiveType_POINT); + // importer.SetPropertyInteger(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, 1); + aiScene const* scene = importer.ReadFile(relativeFileName, + aiProcess_JoinIdenticalVertices | + //aiProcess_RemoveComponent | + aiProcess_SortByPType | + aiProcess_Triangulate); + if (scene) + { + shape = &visualShapes.expand(); + shape->m_scaling[0] = 1; + shape->m_scaling[1] = 1; + shape->m_scaling[2] = 1; + shape->m_scaling[3] = 1; + int index = 0; + shape->m_indices = new b3AlignedObjectArray(); + shape->m_vertices = new b3AlignedObjectArray(); + + aiMatrix4x4 ident; + addMeshParts(scene, scene->mRootNode, shape, ident); + shape->m_numIndices = shape->m_indices->size(); + shape->m_numvertices = shape->m_vertices->size(); + ColladaGraphicsInstance& instance = visualShapeInstances.expand(); + instance.m_shapeIndex = visualShapes.size()-1; + } + } + } + + } + +} + +#endif //COMPARE_WITH_ASSIMP \ No newline at end of file diff --git a/Demos3/ImportColladaDemo/LoadMeshFromCollada.h b/Demos3/ImportColladaDemo/LoadMeshFromCollada.h index 86225f6a2..153fe12e1 100644 --- a/Demos3/ImportColladaDemo/LoadMeshFromCollada.h +++ b/Demos3/ImportColladaDemo/LoadMeshFromCollada.h @@ -31,5 +31,14 @@ void LoadMeshFromCollada(const char* relativeFileName, btTransform& upAxisTrans, float& unitMeterScaling); +//#define COMPARE_WITH_ASSIMP +#ifdef COMPARE_WITH_ASSIMP +void LoadMeshFromColladaAssimp(const char* relativeFileName, + btAlignedObjectArray& visualShapes, + btAlignedObjectArray& visualShapeInstances, + btTransform& upAxisTrans, + float& unitMeterScaling + ); +#endif //COMPARE_WITH_ASSIMP #endif //LOAD_MESH_FROM_COLLADA_H diff --git a/Demos3/ImportSTLDemo/LoadMeshFromSTL.h b/Demos3/ImportSTLDemo/LoadMeshFromSTL.h index f0fe15954..d6f7fa01e 100644 --- a/Demos3/ImportSTLDemo/LoadMeshFromSTL.h +++ b/Demos3/ImportSTLDemo/LoadMeshFromSTL.h @@ -41,7 +41,15 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName) if (numTriangles) { - + { + //perform a sanity check instead of crashing on invalid triangles/STL files + int expectedBinaryFileSize = numTriangles* 50 + 84; + if (expectedBinaryFileSize != size) + { + return 0; + } + + } shape = new GLInstanceGraphicsShape; // b3AlignedObjectArray* m_vertices; // int m_numvertices; diff --git a/Demos3/ImportURDFDemo/ConvertRigidBodies2MultiBody.h b/Demos3/ImportURDFDemo/ConvertRigidBodies2MultiBody.h new file mode 100644 index 000000000..895a8717f --- /dev/null +++ b/Demos3/ImportURDFDemo/ConvertRigidBodies2MultiBody.h @@ -0,0 +1,14 @@ +#ifndef CONVERT_RIGIDBODIES_2_MULTIBODY_H +#define CONVERT_RIGIDBODIES_2_MULTIBODY_H + +struct ConvertRigidBodies2MultiBody +{ + btAlignedObjectArray m_bodies; + btAlignedObjectArray m_constraints; + + virtual void addRigidBody(btRigidBody* body); + virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); + virtual btMultiBody* convertToMultiBody(); + +}; +#endif //CONVERT_RIGIDBODIES_2_MULTIBODY_H diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp index dc9931e4a..85a7518e4 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp @@ -3,7 +3,9 @@ #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" #include "Bullet3Common/b3FileUtils.h" #include "../ImportSTLDemo/LoadMeshFromSTL.h" +#include "../ImportColladaDemo/LoadMeshFromCollada.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "Bullet3Common/b3FileUtils.h" static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter; static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter); @@ -20,6 +22,24 @@ ImportUrdfDemo::~ImportUrdfDemo() } +static btVector4 colors[4] = +{ + btVector4(1,0,0,1), + btVector4(0,1,0,1), + btVector4(0,1,1,1), + btVector4(1,1,0,1), +}; + + +btVector3 selectColor() +{ + + static int curColor = 0; + btVector4 color = colors[curColor]; + curColor++; + curColor&=3; + return color; +} void ImportUrdfDemo::setFileName(const char* urdfFileName) { @@ -100,8 +120,13 @@ struct URDF2BulletMappings btAlignedObjectArray m_jointTypeArray; }; +enum MyFileType +{ + FILE_STL=1, + FILE_COLLADA=2 +}; -btCollisionShape* convertVisualToCollisionShape(const Visual* visual, const char* pathPrefix) +btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const char* pathPrefix) { btCollisionShape* shape = 0; @@ -169,12 +194,118 @@ btCollisionShape* convertVisualToCollisionShape(const Visual* visual, const char const char* filename = mesh->filename.c_str(); printf("mesh->filename=%s\n",filename); char fullPath[1024]; + int fileType = 0; + sprintf(fullPath,"%s%s",pathPrefix,filename); + b3FileUtils::toLower(fullPath); + if (strstr(fullPath,".dae")) + { + fileType = FILE_COLLADA; + } + if (strstr(fullPath,".stl")) + { + fileType = FILE_STL; + } + + sprintf(fullPath,"%s%s",pathPrefix,filename); FILE* f = fopen(fullPath,"rb"); if (f) { fclose(f); - GLInstanceGraphicsShape* glmesh = LoadMeshFromSTL(fullPath); + GLInstanceGraphicsShape* glmesh = 0; + + + switch (fileType) + { + case FILE_STL: + { + glmesh = LoadMeshFromSTL(fullPath); + break; + } + case FILE_COLLADA: + { + + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans;upAxisTrans.setIdentity(); + float unitMeterScaling=1; + + LoadMeshFromCollada(fullPath, + visualShapes, + visualShapeInstances, + upAxisTrans, + unitMeterScaling); + + glmesh = new GLInstanceGraphicsShape; + int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i=0;im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i=0;im_vertices->size();i++) + { + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; + + } + + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices+additionalIndices); + for (int k=0;km_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex; + } + + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; + upAxisMat.setPureRotation(upAxisTrans.getRotation()); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices+additionalVertices); + + for(int v=0;vm_vertices->push_back(verts[v]); + } + } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(fullPath); + + break; + } + default: + { + } + } + + if (glmesh && (glmesh->m_numvertices>0)) { printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath); @@ -380,9 +511,9 @@ btMultiBody* URDF2BulletMultiBody(my_shared_ptr link, GraphicsPhysic //btCompoundShape* compoundShape = new btCompoundShape(); btCollisionShape* shape = 0; - for (int v=0;v<(int)link->visual_array.size();v++) + for (int v=0;v<(int)link->collision_array.size();v++) { - const Visual* visual = link->visual_array[v].get(); + const Collision* visual = link->collision_array[v].get(); shape = convertVisualToCollisionShape(visual,pathPrefix); @@ -390,12 +521,13 @@ btMultiBody* URDF2BulletMultiBody(my_shared_ptr link, GraphicsPhysic { gfxBridge.createCollisionShapeGraphicsObject(shape);//childShape); - btVector3 color(0,0,1); + btVector3 color = selectColor(); + /* if (visual->material.get()) { color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); } - + */ btVector3 localInertia(0,0,0); if (mass) { @@ -466,7 +598,7 @@ btMultiBody* URDF2BulletMultiBody(my_shared_ptr link, GraphicsPhysic return mb; } -void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world, URDF2BulletMappings& mappings, const char* pathPrefix) +void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix) { btCollisionShape* shape = 0; @@ -517,9 +649,9 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy { printf("converting visuals of link %s",link->name.c_str()); - for (int v=0;v<(int)link->visual_array.size();v++) + for (int v=0;v<(int)link->collision_array.size();v++) { - const Visual* visual = link->visual_array[v].get(); + const Collision* visual = link->collision_array[v].get(); shape = convertVisualToCollisionShape(visual,pathPrefix); @@ -527,12 +659,12 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy { gfxBridge.createCollisionShapeGraphicsObject(shape); - btVector3 color(0,0,1); - if (visual->material.get()) + btVector3 color = selectColor(); +/* if (visual->material.get()) { color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); } - + */ btVector3 localInertia(0,0,0); if (mass) { @@ -553,7 +685,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy btRigidBody* body = new btRigidBody(rbci); - world->addRigidBody(body,bodyCollisionFilterGroup,bodyCollisionFilterMask); + world1->addRigidBody(body,bodyCollisionFilterGroup,bodyCollisionFilterMask); // body->setFriction(0); gfxBridge.createRigidBodyGraphicsObject(body,color); @@ -566,7 +698,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy mappings.m_link2rigidbody.insert(p, linkInfo); //create a joint if necessary - if ((*link).parent_joint) + if ((*link).parent_joint && pp) { btAssert(pp); @@ -595,7 +727,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy dof6->setAngularUpperLimit(btVector3(0,0,0)); if (enableConstraints) - world->addConstraint(dof6,true); + world1->addConstraint(dof6,true); // btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB); // world->addConstraint(fixed,true); @@ -613,7 +745,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy dof6->setAngularUpperLimit(btVector3(0,0,-1000)); if (enableConstraints) - world->addConstraint(dof6,true); + world1->addConstraint(dof6,true); printf("Revolute/Continuous joint\n"); break; @@ -629,7 +761,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy dof6->setAngularUpperLimit(btVector3(0,0,0)); if (enableConstraints) - world->addConstraint(dof6,true); + world1->addConstraint(dof6,true); printf("Prismatic\n"); break; @@ -649,7 +781,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy { if (*child) { - URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world,mappings,pathPrefix); + URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world1,mappings,pathPrefix); } else @@ -733,15 +865,15 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) // print entire tree printTree(root_link); - btTransform worldTrans; - worldTrans.setIdentity(); + btTransform identityTrans; + identityTrans.setIdentity(); int numJoints = (*robot).m_numJoints; if (1) { URDF2BulletMappings mappings; - URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings,pathPrefix); + URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix); } //the btMultiBody support is work-in-progress :-) @@ -749,7 +881,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) { URDF2BulletMappings mappings; - btMultiBody* mb = URDF2BulletMultiBody(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings,pathPrefix, 0,numJoints); + btMultiBody* mb = URDF2BulletMultiBody(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix, 0,numJoints); mb->setHasSelfCollision(false); mb->finalizeMultiDof(); diff --git a/btgui/Bullet3AppSupport/MyDebugDrawer.h b/btgui/Bullet3AppSupport/MyDebugDrawer.h index 26ed7d905..ee9b3a04d 100644 --- a/btgui/Bullet3AppSupport/MyDebugDrawer.h +++ b/btgui/Bullet3AppSupport/MyDebugDrawer.h @@ -89,16 +89,16 @@ public: int sz = m_linePoints.size(); if (sz) { - float debugColor[4]; - debugColor[0] = m_currentLineColor.x(); - debugColor[1] = m_currentLineColor.y(); - debugColor[2] = m_currentLineColor.z(); - debugColor[3] = 1.f; - m_glApp->m_renderer->drawLines(&m_linePoints[0].x,debugColor, - m_linePoints.size(),sizeof(MyDebugVec3), - &m_lineIndices[0], - m_lineIndices.size(), - 1); + float debugColor[4]; + debugColor[0] = m_currentLineColor.x(); + debugColor[1] = m_currentLineColor.y(); + debugColor[2] = m_currentLineColor.z(); + debugColor[3] = 1.f; + m_glApp->m_renderer->drawLines(&m_linePoints[0].x,debugColor, + m_linePoints.size(),sizeof(MyDebugVec3), + &m_lineIndices[0], + m_lineIndices.size(), + 1); m_linePoints.clear(); m_lineIndices.clear(); } diff --git a/btgui/Gwen/premake4.lua b/btgui/Gwen/premake4.lua index 10607d52b..74d16aac9 100644 --- a/btgui/Gwen/premake4.lua +++ b/btgui/Gwen/premake4.lua @@ -2,14 +2,14 @@ kind "StaticLib" - flags {"Unicode"} + --flags {"Unicode"} initOpenGL() initGlew() if os.is("Linux") then initX11() end - defines { "GWEN_COMPILE_STATIC" , "_HAS_EXCEPTIONS=0", "_STATIC_CPPLIB" } + defines { "GWEN_COMPILE_STATIC" } defines { "DONT_USE_GLUT"} includedirs { ".",".." diff --git a/src/Bullet3Common/b3FileUtils.h b/src/Bullet3Common/b3FileUtils.h index 348b34ffd..5d2efe0cd 100644 --- a/src/Bullet3Common/b3FileUtils.h +++ b/src/Bullet3Common/b3FileUtils.h @@ -70,8 +70,9 @@ struct b3FileUtils return oriptr; } + - void extractPath(const char* fileName, char* path, int maxPathLength) + static void extractPath(const char* fileName, char* path, int maxPathLength) { const char* stripped = strip2(fileName, "/"); stripped = strip2(stripped, "\\"); @@ -97,6 +98,25 @@ struct b3FileUtils } } + static char toLowerChar(const char t) + { + if (t>=(char)'A' && t<=(char)'Z') + return t + ((char)'a' - (char)'A'); + else + return t; + } + + + static void toLower(char* str) + { + int len=strlen(str); + for (int i=0;i