Namespaces | Classes | Functions

OpenTissue::mbd Namespace Reference

Namespaces

namespace  collision_detection
namespace  collision_laws
namespace  detail
namespace  math
namespace  mel

Classes

class  CachingContactGraphAnalysis
class  CollisionDetection
class  ExhaustiveSearch
class  GeometryDispatcher
class  SingleGroupAnalysis
class  SpatialHashing
class  IntervalEndpoint
class  AABB
class  SweepNPrune
class  IterateOnceCollisionResolver
class  SequentialCollisionResolver
class  SequentialTruncatingCollisionResolver
class  Damping
class  DrivingForce
class  Gravity
class  CollisionResolverInterface
class  ConstraintInterface
class  CoreConstraintInterface
class  ForceInterface
class  JointInterface
class  NCPSolverInterface
class  ScriptedMotionInterface
class  SimulatorInterface
class  StepperInterface
class  SubConstraintInterface
class  BallJoint
class  HingeJoint
class  SliderJoint
class  UniversalJoint
class  WheelJoint
class  AngularJointLimit
class  LinearJointLimit
class  ReachCone
class  default_ublas_math_policy
class  optimized_ublas_math_policy
class  Body
class  BodyGroup
class  CollisionInfo
class  Configuration
class  ContactPoint
class  Edge
class  JointSocket
class  KineticEnergySleepyPolicy
class  Material
class  MaterialLibrary
struct  NoSleepyPolicy
class  StackAnalysis
class  StackPropagation
class  Types
class  AngularJointMotor
class  LinearJointMotor
class  Oscillation
class  BisectionStepSimulator
class  ExplicitFixedStepSimulator
class  ExplicitSeparateErrorCorrectionFixedStepSimulator
class  FixPointStepSimulator
class  ImplicitFixedStepSimulator
class  SemiImplicitFixedStepSimulator
class  SeparatedCollisionContactFixedStepSimulator
class  ProjectedGaussSeidel
class  TwoPassShockPropagationStepper
class  ConstraintBasedShockPropagationStepper
class  DynamicsProjectionStepper
class  DynamicsStepper
class  FirstOrderStepper
class  DrawBodyFunctor
class  DrawBodyFunctor< true >
class  DrawJointFunctor

Functions

template<typename simulator_type >
void setup_default_geometry_dispatcher (simulator_type &simulator)
template<typename body_type >
void apply_impulse (body_type *body, typename body_type::vector3_type const &r, typename body_type::vector3_type const &J)
template<typename real_type , typename vector3_type , typename matrix3x3_type >
matrix3x3_type compute_collision_matrix (real_type const &m_a, matrix3x3_type const &I_a, vector3_type const &r_a, real_type const &m_b, matrix3x3_type const &I_b, vector3_type const &r_b)
template<typename indirect_body_iterator >
size_t compute_contact_count (indirect_body_iterator begin, indirect_body_iterator end)
template<typename indirect_body_iterator , typename matrix_type >
void compute_contact_count_matrix (indirect_body_iterator begin, indirect_body_iterator end, matrix_type &C)
template<typename indirect_body_iterator , typename vector_type >
void compute_kinetic_energy_vector (indirect_body_iterator begin, indirect_body_iterator end, vector_type &energy)
template<typename configuration_type , typename real_type2 >
void compute_min_max_distance (configuration_type &configuration, real_type2 &min_value, real_type2 &max_value)
template<typename indirect_body_iterator , typename vector_type , typename real_type >
void compute_position_update (indirect_body_iterator begin, indirect_body_iterator end, vector_type const &s, vector_type const &u, real_type const &h, vector_type &snew)
template<typename group_type , typename vector_type , typename real_type >
void compute_position_update (group_type const &group, vector_type const &s, vector_type const &u, real_type const &h, vector_type &snew)
template<typename vector3_type >
vector3_type compute_relative_contact_velocity (vector3_type const &v_a, vector3_type const &w_a, vector3_type const &r_a, vector3_type const &v_b, vector3_type const &w_b, vector3_type const &r_b)
template<typename indirect_body_iterator , typename real_type >
void compute_scripted_motions (indirect_body_iterator begin, indirect_body_iterator end, real_type const &time)
template<typename group_type , typename real_type >
void compute_scripted_motions (group_type const &group, real_type const &time)
template<typename group_type , typename vector_type >
void get_cached_solution_vector (group_type const &group, size_t const &m, vector_type &x)
template<typename indirect_body_iterator , typename vector_type >
void get_external_force_vector (indirect_body_iterator begin, indirect_body_iterator end, vector_type &f_ext, bool compute_velocity_forces)
template<typename group_type , typename vector_type >
void get_external_force_vector (group_type const &group, vector_type &f_ext, bool compute_velocity_forces)
template<typename indirect_body_iterator , typename matrix_type >
void get_inverse_mass_matrix (indirect_body_iterator begin, indirect_body_iterator end, matrix_type &invM)
template<typename group_type , typename matrix_type >
void get_inverse_mass_matrix (group_type const &group, matrix_type &invM)
template<typename indirect_body_iterator , typename matrix_type >
void get_mass_matrix (indirect_body_iterator begin, indirect_body_iterator end, matrix_type &M)
template<typename group_type , typename matrix_type >
void get_mass_matrix (group_type const &group, matrix_type &M)
template<typename group_type , typename real_type , typename matrix_type , typename vector_type , typename idx_vector_type >
void get_ncp_formulation (group_type &group, real_type const &fps, matrix_type &J, matrix_type &invM, vector_type &lo, vector_type &hi, idx_vector_type &pi, vector_type &mu, vector_type &gamma, vector_type &b, bool const &use_stabilization, bool const &use_friction, bool const &use_bounce, bool const &use_erp)
template<typename indirect_body_iterator , typename vector_type >
void get_position_vector (indirect_body_iterator begin, indirect_body_iterator end, vector_type &s)
template<typename group_type , typename vector_type >
void get_position_vector (group_type const &group, vector_type &s)
template<typename indirect_body_iterator , typename vector_type >
void get_velocity_vector (indirect_body_iterator begin, indirect_body_iterator end, vector_type &u)
template<typename group_type , typename vector_type >
void get_velocity_vector (group_type const &group, vector_type &u)
template<typename indirect_body_iterator >
bool is_all_bodies_sleepy (indirect_body_iterator begin, indirect_body_iterator end)
template<typename group_type >
bool is_all_bodies_sleepy (group_type const &group)
template<typename group_type , typename vector_type >
void set_cached_solution_vector (group_type &group, size_t const &m, vector_type &x)
template<typename indirect_body_iterator , typename vector_type >
void set_position_vector (indirect_body_iterator begin, indirect_body_iterator end, vector_type const &s)
template<typename group_type , typename vector_type >
void set_position_vector (group_type const &group, vector_type &s)
template<typename indirect_body_iterator , typename vector_type >
void set_velocity_vector (indirect_body_iterator begin, indirect_body_iterator end, vector_type const &u)
template<typename group_type , typename vector_type >
void set_velocity_vector (group_type const &group, vector_type &u)
template<typename matrix3x3_type >
void update_inertia_tensor (matrix3x3_type const &R, matrix3x3_type const &B, matrix3x3_type &W)
template<typename matrix_type , typename vector_type , typename math_policy >
vector_type::value_type merit (matrix_type const &A, vector_type const &x, vector_type const &b, vector_type const &lo, vector_type const &hi, math_policy const &)
template<typename body_type >
void draw_body (body_type const &body, bool wireframe)
template<typename configuration_type >
void draw_contacts (configuration_type &configuration)
template<typename joint_type >
void draw_joint (joint_type const &joint)
template<typename configuration_type >
void draw_penetrations (configuration_type &configuration)

Function Documentation

template<typename body_type >
void OpenTissue::mbd::apply_impulse ( body_type *  body,
typename body_type::vector3_type const &  r,
typename body_type::vector3_type const &  J 
)

Apply Impulse to body_type.

Parameters:
body The body where the impulse should be applied to.
r The arm from the center of mass of the body to the point of contact, where the impulse is applied.
J The impulse that should be applied.
template<typename real_type , typename vector3_type , typename matrix3x3_type >
matrix3x3_type OpenTissue::mbd::compute_collision_matrix ( real_type const &  m_a,
matrix3x3_type const &  I_a,
vector3_type const &  r_a,
real_type const &  m_b,
matrix3x3_type const &  I_b,
vector3_type const &  r_b 
)

Compute Collision Matrix. This method computes the collision matrix, K, which is part of the impulse-momemtum relation between two bodies in a single point collision.

K = (1/Ma + 1/Mb) Identity - ( star(ra) Ia^-1 star(ra) + star(rb) Ib^-1 star(rb))

Where star() is the cross product matrix and Identity is a diagonal matrix of ones.

The computations have been optimized to exploit symmetry and zero pattern's as much as possible. It is still possible to optimize further by exploiting common sub-terms in the computations.

Parameters:
m_a Inverse mass of object A.
I_a Inverse Intertia Tensor of object A.
r_a The arm from center of mas of object A to the point of contact.
m_b Inverse mass of object B.
I_b Inverse Intertia Tensor of object B.
r_b The arm from center of mas of object B to the point of contact.
Returns:
The collision matrix K.
template<typename indirect_body_iterator >
size_t OpenTissue::mbd::compute_contact_count ( indirect_body_iterator  begin,
indirect_body_iterator  end 
)

Compute Contact Count. This template function computes the number of contact points between a specified sequence of bodies. This is usefull for debugging or profiling information.

Example of usage:

std::cout << "|C| = " << mbd::compute_contact_count(configuration.body_begin(),configuration.body_end()) << std::endl;

template<typename indirect_body_iterator , typename matrix_type >
void OpenTissue::mbd::compute_contact_count_matrix ( indirect_body_iterator  begin,
indirect_body_iterator  end,
matrix_type C 
)

Compute Contact Count Matrix. This template function computes the number of contact points between pairs of bodies. Upon return the result is stored into a matrix.

This is usefull for debugging information. Example of usage:

matrix_type C; mbd::compute_contact_count_matrix(configuration.body_begin(),configuration.body_end(),C); std::cout << "C = " << C << ";" << std::endl;

template<typename indirect_body_iterator , typename vector_type >
void OpenTissue::mbd::compute_kinetic_energy_vector ( indirect_body_iterator  begin,
indirect_body_iterator  end,
vector_type energy 
)

Compute Kinetic Energy Vector. Retrieves the total kinetic energy of a sequence of bodies and stores these into a vector.

This template function is usefull for debugging. For instance to verify that kinetic energy is not increased during a simulation.

Example usage:

vector_type energy; mbd::compute_kinetic_energy_vector(configuration.body_begin(),configuration.body_end(),energy); std::cout << "E = " << energy << ";" << std:endl;

template<typename configuration_type , typename real_type2 >
void OpenTissue::mbd::compute_min_max_distance ( configuration_type &  configuration,
real_type2 &  min_value,
real_type2 &  max_value 
)
template<typename indirect_body_iterator , typename vector_type , typename real_type >
void OpenTissue::mbd::compute_position_update ( indirect_body_iterator  begin,
indirect_body_iterator  end,
vector_type const &  s,
vector_type const &  u,
real_type const &  h,
vector_type snew 
)

Position Update Method. This method implements a modified position update, ie.

$ s = s + t S u$

The modification lies in the ability to use finite rotatation updates. This allows an end user to model high-speed spinning objects to use finite rotation updates. Doing so will greatly improve upon the accuracy.

The drawback being that the finite update is much slower than the infinite update.

Parameters:
begin An iterator to the first body in the sequence.
end An iterator to the one past the last body in the sequence.
s Must have 7*n entries is the current position - probably rcm + quaternion
u Must have 6*n entries velocity - linear and angular
h Timestep size
snew Must be of same size as s - new position - we call with the same vector as s
template<typename group_type , typename vector_type , typename real_type >
void OpenTissue::mbd::compute_position_update ( group_type const &  group,
vector_type const &  s,
vector_type const &  u,
real_type const &  h,
vector_type snew 
)
template<typename vector3_type >
vector3_type OpenTissue::mbd::compute_relative_contact_velocity ( vector3_type const &  v_a,
vector3_type const &  w_a,
vector3_type const &  r_a,
vector3_type const &  v_b,
vector3_type const &  w_b,
vector3_type const &  r_b 
)

Compute relative Contact Velocity.

Parameters:
v_a The linear velocity of the center of mass of object A.
w_a The angular velocity around the center of mass of object A.
r_a The arm from center of mas of object A to the point of contact.
v_b The linear velocity of the center of mass of object B.
w_b The angular velocity around the center of mass of object B.
r_b The arm from center of mas of object B to the point of contact.
Returns:
The relative contact velocity of the two bodies.
template<typename indirect_body_iterator , typename real_type >
void OpenTissue::mbd::compute_scripted_motions ( indirect_body_iterator  begin,
indirect_body_iterator  end,
real_type const &  time 
)

Compute Scripted Motion. This method will update the scripted motion of all scripted bodies in the body sequence. That is the state of each scripted body is updated by invoking the run method on the attached scripted motion.

Parameters:
begin An iterator to the first body in the sequence.
end An iterator to the one past the last body in the sequence.
time The time at which the scripted motion should be evaluated.
template<typename group_type , typename real_type >
void OpenTissue::mbd::compute_scripted_motions ( group_type const &  group,
real_type const &  time 
)
template<typename body_type >
void OpenTissue::mbd::draw_body ( body_type const &  body,
bool  wireframe 
)
template<typename configuration_type >
void OpenTissue::mbd::draw_contacts ( configuration_type &  configuration  ) 
template<typename joint_type >
void OpenTissue::mbd::draw_joint ( joint_type const &  joint  ) 
template<typename configuration_type >
void OpenTissue::mbd::draw_penetrations ( configuration_type &  configuration  ) 
template<typename group_type , typename vector_type >
void OpenTissue::mbd::get_cached_solution_vector ( group_type const &  group,
size_t const &  m,
vector_type x 
)

Extract Cached Solution.

Further it is assumed that the vector library used provides a vector proxy function called subrange, which is capable of returning a vector range. (see for instance in Boost uBLAS for an example).

Parameters:
group The group corresponding to the A-matrix.
m The number of active constraints in the group (i.e. the number of rows in the Jacobian matrix).
x Upon return this vector holds the cached solution. Vector must have size m, where m is total number of jacobian rows.
template<typename indirect_body_iterator , typename vector_type >
void OpenTissue::mbd::get_external_force_vector ( indirect_body_iterator  begin,
indirect_body_iterator  end,
vector_type f_ext,
bool  compute_velocity_forces 
)

Extract external Forces and Torques. Incl. velocity dependent terms acting on bodies.

The method implicitly assume that caller has restored position and velocities at the point in time where he/she wants to compute the external forces and torques.

Parameters:
begin An iterator to the first body in the sequence.
end An iterator to the one past the last body in the sequence.
f_ext Upon return this array holds the extracted values.
compute_velocity_forces Boolean flag indicating wheter velocity dependent forces should be added to the extracted vector. Default value is true meaning that velocity forces is by default added to the external force vector.
template<typename group_type , typename vector_type >
void OpenTissue::mbd::get_external_force_vector ( group_type const &  group,
vector_type f_ext,
bool  compute_velocity_forces 
)
template<typename indirect_body_iterator , typename matrix_type >
void OpenTissue::mbd::get_inverse_mass_matrix ( indirect_body_iterator  begin,
indirect_body_iterator  end,
matrix_type invM 
)

Extract Generalized Inverse Mass Matrix.

Parameters:
begin An iterator to the first body in the sequence.
end An iterator to the one past the last body in the sequence.
M Upon return this arguement contains the generalized inverse mass matrix.
template<typename group_type , typename matrix_type >
void OpenTissue::mbd::get_inverse_mass_matrix ( group_type const &  group,
matrix_type invM 
)
template<typename group_type , typename matrix_type >
void OpenTissue::mbd::get_mass_matrix ( group_type const &  group,
matrix_type M 
)
template<typename indirect_body_iterator , typename matrix_type >
void OpenTissue::mbd::get_mass_matrix ( indirect_body_iterator  begin,
indirect_body_iterator  end,
matrix_type M 
)

Extract Generalized Mass Matrix.

Parameters:
begin An iterator to the first body in the sequence.
end An iterator to the one past the last body in the sequence.
M Upon return this arguement contains the generalized mass matrix.
template<typename group_type , typename real_type , typename matrix_type , typename vector_type , typename idx_vector_type >
void OpenTissue::mbd::get_ncp_formulation ( group_type &  group,
real_type const &  fps,
matrix_type J,
matrix_type invM,
vector_type lo,
vector_type hi,
idx_vector_type &  pi,
vector_type mu,
vector_type gamma,
vector_type b,
bool const &  use_stabilization,
bool const &  use_friction,
bool const &  use_bounce,
bool const &  use_erp 
)
template<typename group_type , typename vector_type >
void OpenTissue::mbd::get_position_vector ( group_type const &  group,
vector_type s 
)
template<typename indirect_body_iterator , typename vector_type >
void OpenTissue::mbd::get_position_vector ( indirect_body_iterator  begin,
indirect_body_iterator  end,
vector_type s 
)

Extract Generalized Position Vector. Note this call also affect the tag-member of all bodies in the group. Thus, in a parallel computation side-effects could occur if bodies are ``shared'' among different groups.

Parameters:
begin An iterator to the first body in the sequence.
end An iterator to the one past the last body in the sequence.
s Upon return this vector holds the extracted generalized position vector.
template<typename group_type , typename vector_type >
void OpenTissue::mbd::get_velocity_vector ( group_type const &  group,
vector_type u 
)
template<typename indirect_body_iterator , typename vector_type >
void OpenTissue::mbd::get_velocity_vector ( indirect_body_iterator  begin,
indirect_body_iterator  end,
vector_type u 
)

Extract Generalized Velocity Vector.

Parameters:
begin An iterator to the first body in the sequence.
end An iterator to the one past the last body in the sequence.
u Upon return this vector holds the extracted generalized velocity vector.
template<typename indirect_body_iterator >
bool OpenTissue::mbd::is_all_bodies_sleepy ( indirect_body_iterator  begin,
indirect_body_iterator  end 
)
template<typename group_type >
bool OpenTissue::mbd::is_all_bodies_sleepy ( group_type const &  group  ) 
template<typename matrix_type , typename vector_type , typename math_policy >
vector_type::value_type OpenTissue::mbd::merit ( matrix_type const &  A,
vector_type const &  x,
vector_type const &  b,
vector_type const &  lo,
vector_type const &  hi,
math_policy const &   
)

Compute merit function and use as a measure of convergence

y = A * x + b

H_i(x) = min(x_i - l_i, max(x_i - u_i, y_i)).

theta(x) = H(x)^T H(x)/2

template<typename group_type , typename vector_type >
void OpenTissue::mbd::set_cached_solution_vector ( group_type &  group,
size_t const &  m,
vector_type x 
)

Set Cached Solution.

Further it is assumed that the vector library used provides a vector proxy function called subrange, which is capable of returning a vector range. (see for instance in Boost uBLAS for an example).

Parameters:
group The group corresponding to the A-matrix.
m The number of active constraints in the group (i.e. the number of rows in the Jacobian matrix).
x This vector holds a solution that should be cached. Vector must have size m, where m is total number of jacobian rows.
template<typename indirect_body_iterator , typename vector_type >
void OpenTissue::mbd::set_position_vector ( indirect_body_iterator  begin,
indirect_body_iterator  end,
vector_type const &  s 
)

Set positions and orientations of a sequence of bodies.

Parameters:
begin An iterator to the first body in the sequence.
end An iterator to the one past the last body in the sequence.
s This vector holds the extracted generalized position vector.
template<typename group_type , typename vector_type >
void OpenTissue::mbd::set_position_vector ( group_type const &  group,
vector_type s 
)
template<typename group_type , typename vector_type >
void OpenTissue::mbd::set_velocity_vector ( group_type const &  group,
vector_type u 
)
template<typename indirect_body_iterator , typename vector_type >
void OpenTissue::mbd::set_velocity_vector ( indirect_body_iterator  begin,
indirect_body_iterator  end,
vector_type const &  u 
)

Set velocities and spins of a sequence of bodies.

Parameters:
begin An iterator to the first body in the sequence.
end An iterator to the one past the last body in the sequence.
u This vector holds the extracted generalized velocity vector.
template<typename simulator_type >
void OpenTissue::mbd::setup_default_geometry_dispatcher ( simulator_type &  simulator  )  [inline]

Setup Default Geometry Dispatcher. This function will initialize the collision matrix in the geometry dispatcher.

Parameters:
simulator A reference to the simulator where the dispatcher belongs to.
template<typename matrix3x3_type >
void OpenTissue::mbd::update_inertia_tensor ( matrix3x3_type const &  R,
matrix3x3_type const &  B,
matrix3x3_type &  W 
)

Inertia Update Method. Computes, W = R B R^T. This method have been optimized to avoid having to transpose the orientation matrix and exploit symmetry of the inertia tensor.

Note: Further optimization may be possible by exploiting common sub-terms.

Parameters:
R The orientation as a rotation matrix.
B An inertia tensor entity in body frame
W The inertia tensor entity in the corresponding world frame.