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