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) |
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.
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. |
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.
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. |
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;
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;
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;
void OpenTissue::mbd::compute_min_max_distance | ( | configuration_type & | configuration, | |
real_type2 & | min_value, | |||
real_type2 & | max_value | |||
) |
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.
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 |
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 | |||
) |
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.
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. |
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.
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. |
void OpenTissue::mbd::compute_scripted_motions | ( | group_type const & | group, | |
real_type const & | time | |||
) |
void OpenTissue::mbd::draw_body | ( | body_type const & | body, | |
bool | wireframe | |||
) |
void OpenTissue::mbd::draw_contacts | ( | configuration_type & | configuration | ) |
void OpenTissue::mbd::draw_joint | ( | joint_type const & | joint | ) |
void OpenTissue::mbd::draw_penetrations | ( | configuration_type & | configuration | ) |
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).
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. |
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.
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. |
void OpenTissue::mbd::get_external_force_vector | ( | group_type const & | group, | |
vector_type & | f_ext, | |||
bool | compute_velocity_forces | |||
) |
void OpenTissue::mbd::get_inverse_mass_matrix | ( | indirect_body_iterator | begin, | |
indirect_body_iterator | end, | |||
matrix_type & | invM | |||
) |
Extract Generalized Inverse Mass Matrix.
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. |
void OpenTissue::mbd::get_inverse_mass_matrix | ( | group_type const & | group, | |
matrix_type & | invM | |||
) |
void OpenTissue::mbd::get_mass_matrix | ( | group_type const & | group, | |
matrix_type & | M | |||
) |
void OpenTissue::mbd::get_mass_matrix | ( | indirect_body_iterator | begin, | |
indirect_body_iterator | end, | |||
matrix_type & | M | |||
) |
Extract Generalized Mass Matrix.
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. |
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 | |||
) |
void OpenTissue::mbd::get_position_vector | ( | group_type const & | group, | |
vector_type & | s | |||
) |
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.
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. |
void OpenTissue::mbd::get_velocity_vector | ( | group_type const & | group, | |
vector_type & | u | |||
) |
void OpenTissue::mbd::get_velocity_vector | ( | indirect_body_iterator | begin, | |
indirect_body_iterator | end, | |||
vector_type & | u | |||
) |
Extract Generalized Velocity Vector.
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. |
bool OpenTissue::mbd::is_all_bodies_sleepy | ( | indirect_body_iterator | begin, | |
indirect_body_iterator | end | |||
) |
bool OpenTissue::mbd::is_all_bodies_sleepy | ( | group_type const & | group | ) |
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
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).
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. |
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.
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. |
void OpenTissue::mbd::set_position_vector | ( | group_type const & | group, | |
vector_type & | s | |||
) |
void OpenTissue::mbd::set_velocity_vector | ( | group_type const & | group, | |
vector_type & | u | |||
) |
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.
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. |
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.
simulator | A reference to the simulator where the dispatcher belongs to. |
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.
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. |