00001 #ifndef OPENTISSUE_DYNAMICS_PSYS_CONSTRAINTS_PSYS_STICK_H 00002 #define OPENTISSUE_DYNAMICS_PSYS_CONSTRAINTS_PSYS_STICK_H 00003 // 00004 // OpenTissue Template Library 00005 // - A generic toolbox for physics-based modeling and simulation. 00006 // Copyright (C) 2008 Department of Computer Science, University of Copenhagen. 00007 // 00008 // OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php 00009 // 00010 #include <OpenTissue/configuration.h> 00011 00012 #include <OpenTissue/dynamics/psys/psys_constraint.h> 00013 00014 #include <cassert> 00015 #include <cmath> 00016 00017 namespace OpenTissue 00018 { 00019 namespace psys 00020 { 00021 00022 template<typename types> 00023 class Stick 00024 : public Constraint< types > 00025 { 00026 public: 00027 00028 typedef typename types::math_types math_types; 00029 typedef typename math_types::real_type real_type; 00030 typedef typename math_types::vector3_type vector3_type; 00031 typedef typename types::particle_type particle_type; 00032 00033 protected: 00034 00035 particle_type * m_A; 00036 particle_type * m_B; 00037 real_type m_length; 00038 real_type m_length_sqr; 00039 unsigned int m_choice; 00040 00041 00042 public: 00043 00044 particle_type * A() { return m_A; } 00045 particle_type * B() { return m_B; } 00046 particle_type const * A() const { return m_A; } 00047 particle_type const * B() const { return m_B; } 00048 00049 real_type const & rest_length() const { return m_length; } 00050 unsigned int & choice() { return m_choice; } 00051 unsigned int const & choice() const { return m_choice; } 00052 00053 public: 00054 00055 Stick() 00056 : m_A(0) 00057 , m_B(0) 00058 , m_length(0) 00059 , m_length_sqr(0) 00060 , m_choice(2) 00061 { } 00062 00063 virtual ~Stick() { } 00064 00065 public: 00066 00073 void init(particle_type * A, particle_type * B) 00074 { 00075 assert(A!=B || !"Stick::set(): Particle A and B were the same"); 00076 assert(A || !"Stick::set(): Particle A was null"); 00077 assert(B || !"Stick::set(): Particle B was null"); 00078 00079 this->m_A = A; 00080 this->m_B = B; 00081 00082 set_rest_length( length(B->position()-A->position())); 00083 } 00084 00090 void set_rest_length(real_type l) 00091 { 00092 assert(l>=0 || !"Spring::set_length(): Spring rest length was negative"); 00093 this->m_length = l; 00094 this->m_length_sqr = l*l; 00095 } 00096 00097 public: 00098 00099 void satisfy() 00100 { 00101 switch(m_choice) 00102 { 00103 case 1: satisfy_type1(); break; 00104 case 2: satisfy_type2(); break; 00105 case 3: satisfy_type3(); break; 00106 }; 00107 } 00108 00109 public: 00110 00115 void satisfy_type1() 00116 { 00117 vector3_type delta = m_A->position() - m_B->position(); 00118 real_type delta_sqr = delta*delta; 00119 real_type delta_length = sqrt(delta_sqr); 00120 real_type diff = (delta_length - m_length)/delta_length; 00121 m_A->position() -= delta*0.5*diff; 00122 m_B->position() += delta*0.5*diff; 00123 } 00124 00129 void satisfy_type2() 00130 { 00131 vector3_type delta = m_A->position() - m_B->position(); 00132 real_type delta_sqr = delta * delta; 00133 real_type approx = m_length_sqr / (delta_sqr + m_length_sqr) - 0.5; 00134 delta *= approx; 00135 m_A->position() += delta; 00136 m_B->position() -= delta; 00137 } 00138 00143 void satisfy_type3() 00144 { 00145 vector3_type delta = m_A->position() - m_B->position(); 00146 real_type delta_length = std::sqrt(delta*delta); 00147 real_type diff = (delta_length - m_length)/(delta_length*(m_A->inv_mass()+m_B->inv_mass())); 00148 m_A->position() -= delta * (diff * m_A->inv_mass()); 00149 m_B->position() += delta * (diff * m_B->inv_mass()); 00150 } 00151 00152 public: 00153 00154 }; 00155 00156 } // namespace psys 00157 } // namespace OpenTissue 00158 00159 // OPENTISSUE_DYNAMICS_PSYS_CONSTRAINTS_PSYS_STICK_H 00160 #endif
1.7.1