| //##################################################################### |
| // Copyright 2002-2004, Zhaosheng Bao, Robert Bridson, Ronald Fedkiw, Eran Guendelman, Geoffrey Irving, Sergey Koltakov, Frank Losasso, Neil Molino, Andrew Selle, Rachel Weinstein. |
| // This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt. |
| //##################################################################### |
| // Class RIGID_BODY_3D |
| //##################################################################### |
| #ifndef __RIGID_BODY_3D__ |
| #define __RIGID_BODY_3D__ |
| |
| #include <string.h> |
| #include <stdlib.h> |
| #include "../Arrays/LIST_ARRAY.h" |
| #include "../Grids/SEGMENT_MESH.h" |
| #include "../Matrices_And_Vectors/VECTOR_3D.h" |
| #include "../Matrices_And_Vectors/DIAGONAL_MATRIX_3X3.h" |
| #include "../Matrices_And_Vectors/SYMMETRIC_MATRIX_3X3.h" |
| #include "../Matrices_And_Vectors/MATRIX_3X3.h" |
| #include "../Matrices_And_Vectors/MATRIX_4X4.h" |
| #include "../Matrices_And_Vectors/QUATERNION.h" |
| #include "../Matrices_And_Vectors/FRAME.h" |
| #include "../Forces_And_Torques/EXTERNAL_FORCES_AND_VELOCITIES.h" |
| #include "../Geometry/IMPLICIT_SURFACE.h" |
| #include "../Geometry/TRIANGULATED_SURFACE.h" |
| #include "../Geometry/TETRAHEDRALIZED_VOLUME.h" |
| #include "../Geometry/EMBEDDED_TETRAHEDRALIZED_VOLUME.h" |
| #include "../Geometry/ORIENTED_BOX_3D.h" |
| #include "../Read_Write/FILE_UTILITIES.h" |
| #include "../Collisions_And_Interactions/COLLISION_BODY_3D.h" |
| #include "RIGID_BODY_STATE_3D.h" |
| #include "MASS_PROPERTIES_3D.h" |
| namespace PhysBAM |
| { |
| template<class T> class RIGID_BODY_FORCES_3D; |
| template<class T> class RIGID_BODY_IMPULSE_ACCUMULATOR_3D; |
| |
| template<class T> |
| class RIGID_BODY_3D: public COLLISION_BODY_3D<T> |
| { |
| public: |
| using COLLISION_BODY_3D<T>::body_type; |
| using COLLISION_BODY_3D<T>::Set_Collision_Thickness; |
| |
| T mass; // center of mass is (0,0,0) in object space |
| DIAGONAL_MATRIX_3X3<T> inertia_tensor; // diagonal inertia tensor in object space, already scaled by the mass of the object |
| T surface_roughness; // small number indicating errors in the geometry |
| VECTOR_3D<T> position; // center of mass in world space |
| QUATERNION<T> orientation; // with respect to the center of mass |
| VECTOR_3D<T> velocity; // of the center of mass |
| VECTOR_3D<T> angular_momentum; // with respect to the center of mass |
| VECTOR_3D<T> angular_velocity; // with respect to the center of mass - needs to be computed! |
| LIST_ARRAY<RIGID_BODY_FORCES_3D<T>*> body_forces; |
| EXTERNAL_FORCES_AND_VELOCITIES<T, VECTOR_3D<T> >* external_forces_and_velocities, external_forces_and_velocities_default; |
| RIGID_BODY_IMPULSE_ACCUMULATOR_3D<T>* impulse_accumulator; |
| TRIANGULATED_SURFACE<T>* triangulated_surface; // discrete representation of the geometry |
| IMPLICIT_SURFACE<T>* implicit_surface; // implicit representation of geometry |
| TETRAHEDRALIZED_VOLUME<T>* tetrahedralized_volume; |
| ORIENTED_BOX_3D<T> oriented_box; |
| BOX_3D<T> axis_aligned_bounding_box; |
| bool bounding_box_up_to_date; |
| T coefficient_of_restitution; // not saved to file |
| T coefficient_of_friction; //not saved to file |
| T coefficient_of_rolling_friction; // not saved to file |
| std::string name; // not saved to file - for convenience and debugging. |
| bool is_static; // not saved to file - indicates whether this object is static in the scene |
| bool is_kinematic; |
| bool is_temporarily_static; // not saved to file |
| bool add_to_spatial_partition; // not saved to file - only false for (some) static objects |
| LIST_ARRAY<RIGID_BODY_STATE_3D<T>*> saved_states; |
| bool thin_shell; |
| bool CFL_initialized; // true if precalculation is done, false if out of date |
| T bounding_box_radius; // needed for CFL calculation |
| int id_number; |
| |
| RIGID_BODY_3D() |
| : external_forces_and_velocities (&external_forces_and_velocities_default), impulse_accumulator (0), triangulated_surface (0), implicit_surface (0), tetrahedralized_volume (0), bounding_box_up_to_date (false), |
| is_static (false), is_kinematic (false), is_temporarily_static (false), add_to_spatial_partition (true), thin_shell (false), |
| CFL_initialized (false), id_number (0) |
| { |
| body_type = COLLISION_BODY_3D<T>::RIGID_BODY_TYPE; |
| mass = 1; |
| inertia_tensor = DIAGONAL_MATRIX_3X3<T>::Identity_Matrix(); |
| Set_Surface_Roughness(); |
| Set_Coefficient_Of_Restitution(); |
| Set_Coefficient_Of_Friction(); |
| Set_Coefficient_Of_Rolling_Friction(); |
| Set_Collision_Thickness(); |
| } |
| |
| ~RIGID_BODY_3D() |
| { |
| for (int i = 1; i <= saved_states.m; i++) delete saved_states (i); |
| } |
| |
| void Set_Name (const std::string& name_input) |
| { |
| name = name_input; |
| } |
| |
| void Set_Id_Number (const int id_number_input) |
| { |
| id_number = id_number_input; |
| } |
| |
| static void Print_Names (const LIST_ARRAY<RIGID_BODY_3D<T>*>& rigid_bodies, const LIST_ARRAY<int>& indices) |
| { |
| std::cout << "{"; |
| |
| for (int i = 1; i <= indices.m; i++) |
| { |
| std::cout << "\"" << rigid_bodies (indices (i))->name << "\""; |
| |
| if (i < indices.m) std::cout << ", "; |
| } |
| |
| std::cout << "}"; |
| } |
| |
| static void Print_Pairs (const LIST_ARRAY<RIGID_BODY_3D<T>*>& rigid_bodies, const LIST_ARRAYS<int>& pairs) |
| { |
| std::cout << "{"; |
| |
| for (int i = 1; i <= pairs.m; i++) |
| { |
| std::cout << "(\"" << rigid_bodies (pairs (1, i))->name << "\", \"" << rigid_bodies (pairs (2, i))->name << "\")"; |
| |
| if (i < pairs.m) std::cout << ", "; |
| } |
| |
| std::cout << "}"; |
| } |
| |
| void Set_Mass (const T mass_input) // rescales moments of inertia |
| { |
| inertia_tensor *= mass_input / mass; |
| mass = mass_input; |
| } |
| |
| void Set_Inertia_Tensor (const DIAGONAL_MATRIX_3X3<T>& inertia_tensor_input) // already scaled by the mass of the object |
| { |
| inertia_tensor = inertia_tensor_input; |
| } |
| |
| void Rescale (const T scaling_factor, const bool rescale_mass = true) |
| { |
| NOT_IMPLEMENTED(); |
| } |
| |
| void Set_Surface_Roughness (const T surface_roughness_input = 1e-6) |
| { |
| surface_roughness = surface_roughness_input; |
| } |
| |
| void Set_External_Forces_And_Velocities (EXTERNAL_FORCES_AND_VELOCITIES<T, VECTOR_3D<T> >& external_forces_and_velocities_input) |
| { |
| external_forces_and_velocities = &external_forces_and_velocities_input; |
| } |
| |
| void Set_Coefficient_Of_Restitution (const T coefficient_input = .5) |
| { |
| coefficient_of_restitution = coefficient_input; |
| } |
| |
| static T Coefficient_Of_Restitution (const RIGID_BODY_3D<T>& body1, const RIGID_BODY_3D<T>& body2) |
| { |
| return min (body1.coefficient_of_restitution, body2.coefficient_of_restitution); |
| } |
| |
| void Set_Coefficient_Of_Friction (const T coefficient_input = .5) |
| { |
| coefficient_of_friction = coefficient_input; |
| } |
| |
| static T Coefficient_Of_Friction (const RIGID_BODY_3D<T>& body1, const RIGID_BODY_3D<T>& body2) |
| { |
| return min (body1.coefficient_of_friction, body2.coefficient_of_friction); |
| } |
| |
| void Set_Coefficient_Of_Rolling_Friction (const T coefficient_input = .5) |
| { |
| coefficient_of_rolling_friction = coefficient_input; |
| } |
| |
| static T Coefficient_Of_Rolling_Friction (const RIGID_BODY_3D<T>& body1, const RIGID_BODY_3D<T>& body2) |
| { |
| return min (body1.coefficient_of_rolling_friction, body2.coefficient_of_rolling_friction); |
| } |
| |
| bool Has_Infinite_Inertia() const |
| { |
| return is_static || is_temporarily_static || is_kinematic; |
| } |
| |
| bool Is_Simulated() const |
| { |
| return !is_static && !is_kinematic; |
| } |
| |
| void Transform (const MATRIX_4X4<T>& A) |
| { |
| position = A * position; |
| orientation *= QUATERNION<T> (A); |
| } |
| |
| void Set_State (const FRAME<T>& frame) |
| { |
| position = frame.t; |
| orientation = frame.r; |
| } |
| |
| void Set_State (const RIGID_BODY_3D<T>& rigid_body, const FRAME<T>& frame = FRAME<T>()) |
| { |
| position = rigid_body.position + frame.t; |
| orientation = rigid_body.orientation * frame.r; |
| } |
| |
| FRAME<T> Frame() const |
| { |
| return FRAME<T> (position, orientation); |
| } |
| |
| RAY_3D<T> Object_Space_Ray (const RAY_3D<T>& world_space_ray) const |
| { |
| RAY_3D<T> transformed_ray (Object_Space_Point (world_space_ray.endpoint), Object_Space_Vector (world_space_ray.direction)); |
| transformed_ray.semi_infinite = world_space_ray.semi_infinite; |
| transformed_ray.t_max = world_space_ray.t_max; |
| transformed_ray.aggregate_id = world_space_ray.aggregate_id; |
| return transformed_ray; |
| } |
| |
| VECTOR_3D<T> Object_Space_Point (const VECTOR_3D<T>& world_space_point) const |
| { |
| return orientation.Inverse_Rotate (world_space_point - position); |
| } |
| |
| VECTOR_3D<T> Object_Space_Point (const VECTOR_3D<T>& world_space_point, const RIGID_BODY_STATE_3D<T>& state) const |
| { |
| return state.orientation.Inverse_Rotate (world_space_point - state.position); |
| } |
| |
| VECTOR_3D<T> Object_Space_Vector (const VECTOR_3D<T>& world_space_vector) const |
| { |
| return orientation.Inverse_Rotate (world_space_vector); |
| } |
| |
| VECTOR_3D<T> Object_Space_Vector (const VECTOR_3D<T>& world_space_vector, const RIGID_BODY_STATE_3D<T>& state) const |
| { |
| return state.orientation.Inverse_Rotate (world_space_vector); |
| } |
| |
| VECTOR_3D<T> World_Space_Point (const VECTOR_3D<T>& object_space_point) const |
| { |
| return orientation.Rotate (object_space_point) + position; |
| } |
| |
| static VECTOR_3D<T> World_Space_Point (const VECTOR_3D<T>& object_space_point, const RIGID_BODY_STATE_3D<T>& state) |
| { |
| return state.orientation.Rotate (object_space_point) + state.position; |
| } |
| |
| VECTOR_3D<T> World_Space_Vector (const VECTOR_3D<T>& object_space_vector) const |
| { |
| return orientation.Rotate (object_space_vector); |
| } |
| |
| static VECTOR_3D<T> World_Space_Vector (const VECTOR_3D<T>& object_space_vector, const RIGID_BODY_STATE_3D<T>& state) |
| { |
| return state.orientation.Rotate (object_space_vector); |
| } |
| |
| SYMMETRIC_MATRIX_3X3<T> World_Space_Inertia_Tensor() const // relative to the center of mass |
| { |
| MATRIX_3X3<T> object_to_world_transformation = orientation.Matrix_3X3(); |
| return SYMMETRIC_MATRIX_3X3<T>::Conjugate (object_to_world_transformation, inertia_tensor); |
| } |
| |
| SYMMETRIC_MATRIX_3X3<T> World_Space_Inertia_Tensor_Inverse() const // relative to the center of mass |
| { |
| MATRIX_3X3<T> object_to_world_transformation = orientation.Matrix_3X3(); |
| return SYMMETRIC_MATRIX_3X3<T>::Conjugate (object_to_world_transformation, inertia_tensor.Inverse()); |
| } |
| |
| SYMMETRIC_MATRIX_3X3<T> World_Space_Inertia_Tensor (const VECTOR_3D<T>& reference_point) const // relative to a reference point |
| { |
| VECTOR_3D<T> dx = position - reference_point; |
| SYMMETRIC_MATRIX_3X3<T> new_term = mass * (VECTOR_3D<T>::Dot_Product (dx, dx) - SYMMETRIC_MATRIX_3X3<T>::Outer_Product (dx)); |
| return World_Space_Inertia_Tensor() + new_term; |
| } |
| |
| void Update_Angular_Velocity() // needs to be called to keep the angular velocity valid |
| { |
| angular_velocity = World_Space_Vector (inertia_tensor.Inverse_Times (Object_Space_Vector (angular_momentum))); |
| } |
| |
| void Update_Angular_Velocity (RIGID_BODY_STATE_3D<T>& state) const // needs to be called to keep the angular velocity valid |
| { |
| state.angular_velocity = World_Space_Vector (inertia_tensor.Inverse_Times (Object_Space_Vector (state.angular_momentum, state)), state); |
| } |
| |
| void Update_Angular_Momentum() // assumes a valid angular_velocity |
| { |
| angular_momentum = World_Space_Vector (inertia_tensor * Object_Space_Vector (angular_velocity)); |
| } |
| |
| void Update_Angular_Momentum (RIGID_BODY_STATE_3D<T>& state) const // assumes a valid angular_velocity |
| { |
| state.angular_momentum = World_Space_Vector (inertia_tensor * Object_Space_Vector (state.angular_velocity, state), state); |
| } |
| |
| VECTOR_3D<T> Pointwise_Object_Velocity (const VECTOR_3D<T>& world_space_point) const |
| { |
| return velocity + VECTOR_3D<T>::Cross_Product (angular_velocity, world_space_point - position); |
| } |
| |
| // overridden in DEFORMABLE_OBJECT_RIGID_BODY_WRAPPER_3D where particle_index is used instead of world_space_point |
| virtual VECTOR_3D<T> Pointwise_Object_Velocity_At_Particle (const VECTOR_3D<T>& world_space_point, const int particle_index) const |
| { |
| return velocity + VECTOR_3D<T>::Cross_Product (angular_velocity, world_space_point - position); |
| } |
| |
| VECTOR_3D<T> Pointwise_Object_Velocity (const int triangle_id, const VECTOR_3D<T>& world_space_point) const // extra triangle_id is not used, but for a virtual function in COLLISION BODY |
| { |
| return velocity + VECTOR_3D<T>::Cross_Product (angular_velocity, world_space_point - position); |
| } |
| |
| VECTOR_3D<T> Pointwise_Object_Pseudo_Velocity (const int triangle_id, const VECTOR_3D<T>& world_space_point, const int state1, const int state2) const |
| { |
| VECTOR_3D<T> object_space_point = Object_Space_Point (world_space_point); |
| return (World_Space_Point (object_space_point, *saved_states (state2)) - World_Space_Point (object_space_point, *saved_states (state1))) / (saved_states (state2)->time - saved_states (state1)->time); |
| } |
| |
| static VECTOR_3D<T> Relative_Velocity (const RIGID_BODY_3D<T>& body1, const RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& world_point) |
| { |
| return body1.Pointwise_Object_Velocity (world_point) - body2.Pointwise_Object_Velocity (world_point); |
| } |
| |
| static VECTOR_3D<T> Relative_Velocity_At_Body1_Particle (const RIGID_BODY_3D<T>& body1, const RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& world_point, const int particle_index) |
| { |
| return body1.Pointwise_Object_Velocity_At_Particle (world_point, particle_index) - body2.Pointwise_Object_Velocity (world_point); |
| } |
| |
| static VECTOR_3D<T> Relative_Angular_Velocity (const RIGID_BODY_3D<T>& body1, const RIGID_BODY_3D<T>& body2) // make sure the angular velocities are updated before calling this! |
| { |
| return body1.angular_velocity - body2.angular_velocity; |
| } |
| |
| T Kinetic_Energy() const |
| { |
| return Translational_Kinetic_Energy() + Rotational_Kinetic_Energy(); |
| } |
| |
| T Translational_Kinetic_Energy() const |
| { |
| return (T).5 * mass * velocity.Magnitude_Squared(); |
| } |
| |
| T Rotational_Kinetic_Energy() const |
| { |
| VECTOR_3D<T> object_space_angular_momentum = Object_Space_Vector (angular_momentum); |
| return (T).5 * inertia_tensor.Inverse_Inner_Product (object_space_angular_momentum, object_space_angular_momentum); |
| } |
| |
| SYMMETRIC_MATRIX_3X3<T> Impulse_Factor (const VECTOR_3D<T>& location) const |
| { |
| if (Has_Infinite_Inertia()) return SYMMETRIC_MATRIX_3X3<T>(); // return zero matrix |
| |
| MATRIX_3X3<T> moment_arm = MATRIX_3X3<T>::Cross_Product_Matrix (location - position); |
| return SYMMETRIC_MATRIX_3X3<T>::Conjugate (moment_arm, World_Space_Inertia_Tensor_Inverse()) + (1 / mass); |
| } |
| |
| static SYMMETRIC_MATRIX_3X3<T> Impulse_Factor (const RIGID_BODY_3D<T>& body1, const RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& location) |
| { |
| assert (!body1.Has_Infinite_Inertia() || !body2.Has_Infinite_Inertia()); |
| return body1.Impulse_Factor (location) + body2.Impulse_Factor (location); |
| } |
| |
| void Initialize_Triangulated_Surface (TRIANGULATED_SURFACE<T>& triangulated_surface_input) // set up acceleration stuctures too |
| { |
| triangulated_surface = &triangulated_surface_input; |
| |
| if (!triangulated_surface->triangle_mesh.segment_mesh) triangulated_surface->triangle_mesh.Initialize_Segment_Mesh(); |
| |
| if (!triangulated_surface->triangle_mesh.triangle_edges) triangulated_surface->triangle_mesh.Initialize_Triangle_Edges(); |
| |
| if (!triangulated_surface->triangle_mesh.segment_mesh->incident_segments) triangulated_surface->triangle_mesh.segment_mesh->Initialize_Incident_Segments(); |
| |
| if (!triangulated_surface->segment_lengths) triangulated_surface->Initialize_Segment_Lengths(); |
| |
| if (!triangulated_surface->triangle_list) triangulated_surface->Update_Triangle_List(); |
| |
| if (!triangulated_surface->bounding_box) triangulated_surface->Update_Bounding_Box(); |
| } |
| |
| bool Triangulated_Surface_Intersection (RAY_3D<T>& ray) const |
| { |
| RAY_3D<T> object_space_ray = Object_Space_Ray (ray); |
| |
| if (triangulated_surface->Intersection (object_space_ray, surface_roughness)) |
| { |
| ray.semi_infinite = false; |
| ray.t_max = object_space_ray.t_max; |
| ray.aggregate_id = object_space_ray.aggregate_id; |
| return true; |
| } |
| else return false; |
| } |
| |
| bool Triangulated_Surface_Closest_Non_Intersecting_Point (RAY_3D<T>& ray) const |
| { |
| RAY_3D<T> object_space_ray = Object_Space_Ray (ray); |
| |
| if (triangulated_surface->Closest_Non_Intersecting_Point (object_space_ray, surface_roughness)) |
| { |
| ray.Restore_Intersection_Information (object_space_ray); |
| return true; |
| } |
| else return false; |
| } |
| |
| VECTOR_3D<T> Triangulated_Surface_Normal (const VECTOR_3D<T>& location, const int aggregate = 0) const |
| { |
| return World_Space_Vector (triangulated_surface->Normal (Object_Space_Point (location), aggregate)); |
| } |
| |
| bool Triangulated_Surface_Inside (const VECTOR_3D<T>& location) const |
| { |
| return triangulated_surface->Inside (Object_Space_Point (location), surface_roughness); |
| } |
| |
| bool Triangulated_Surface_Outside (const VECTOR_3D<T>& location) const |
| { |
| return triangulated_surface->Outside (Object_Space_Point (location), surface_roughness); |
| } |
| |
| bool Triangulated_Surface_Boundary (const VECTOR_3D<T>& location) const |
| { |
| return triangulated_surface->Boundary (Object_Space_Point (location), surface_roughness); |
| } |
| |
| bool Triangulated_Surface_Inside_Any_Triangle (const VECTOR_3D<T>& location, int& triangle_id) const |
| { |
| return triangulated_surface->Inside_Any_Triangle (Object_Space_Point (location), triangle_id, surface_roughness); |
| } |
| |
| VECTOR_3D<T> Triangulated_Surface_Surface (const VECTOR_3D<T>& location, const T max_distance, int* triangle_id = 0, T* distance = 0) const |
| { |
| return World_Space_Point (triangulated_surface->Surface (Object_Space_Point (location), max_distance, surface_roughness, triangle_id, distance)); |
| } |
| |
| TRIANGLE_3D<T> Triangulated_Surface_World_Space_Triangle (const int triangle_id, const bool use_saved_state = false) const |
| { |
| if (use_saved_state) |
| { |
| assert (saved_states (1)); |
| return Triangulated_Surface_World_Space_Triangle (triangle_id, *saved_states (1)); |
| } |
| else return TRIANGLE_3D<T> (World_Space_Point (triangulated_surface->particles.X (triangulated_surface->triangle_mesh.triangles (1, triangle_id))), |
| World_Space_Point (triangulated_surface->particles.X (triangulated_surface->triangle_mesh.triangles (2, triangle_id))), |
| World_Space_Point (triangulated_surface->particles.X (triangulated_surface->triangle_mesh.triangles (3, triangle_id)))); |
| } |
| |
| TRIANGLE_3D<T> Triangulated_Surface_World_Space_Triangle (const int triangle_id, const RIGID_BODY_STATE_3D<T>& state) const |
| { |
| return TRIANGLE_3D<T> (World_Space_Point (triangulated_surface->particles.X (triangulated_surface->triangle_mesh.triangles (1, triangle_id)), state), |
| World_Space_Point (triangulated_surface->particles.X (triangulated_surface->triangle_mesh.triangles (2, triangle_id)), state), |
| World_Space_Point (triangulated_surface->particles.X (triangulated_surface->triangle_mesh.triangles (3, triangle_id)), state)); |
| } |
| |
| VECTOR_3D<T> Triangulated_Surface_World_Space_Point_From_Barycentric_Coordinates (const int triangle_id, const VECTOR_3D<T>& weights) const |
| { |
| return TRIANGLE_3D<T>::Point_From_Barycentric_Coordinates (weights, |
| World_Space_Point (triangulated_surface->particles.X (triangulated_surface->triangle_mesh.triangles (1, triangle_id))), |
| World_Space_Point (triangulated_surface->particles.X (triangulated_surface->triangle_mesh.triangles (2, triangle_id))), |
| World_Space_Point (triangulated_surface->particles.X (triangulated_surface->triangle_mesh.triangles (3, triangle_id)))); |
| } |
| |
| VECTOR_3D<T> Triangulated_Surface_World_Space_Point_From_Barycentric_Coordinates (const int triangle_id, const VECTOR_3D<T>& weights, const RIGID_BODY_STATE_3D<T>& state) const |
| { |
| return TRIANGLE_3D<T>::Point_From_Barycentric_Coordinates (weights, |
| World_Space_Point (triangulated_surface->particles.X (triangulated_surface->triangle_mesh.triangles (1, triangle_id)), state), |
| World_Space_Point (triangulated_surface->particles.X (triangulated_surface->triangle_mesh.triangles (2, triangle_id)), state), |
| World_Space_Point (triangulated_surface->particles.X (triangulated_surface->triangle_mesh.triangles (3, triangle_id)), state)); |
| } |
| |
| void Triangulated_Surface_Translate (const VECTOR_3D<T>& translation) |
| { |
| for (int i = 1; i <= triangulated_surface->particles.number; i++) triangulated_surface->particles.X (i) += translation; |
| |
| triangulated_surface->Update_Triangle_List(); |
| triangulated_surface->Update_Bounding_Box(); |
| } |
| |
| void Initialize_Implicit_Surface (IMPLICIT_SURFACE<T>& implicit_surface_input) |
| { |
| implicit_surface = &implicit_surface_input; // don't recompute cell min/max if already computed |
| implicit_surface->Update_Box(); |
| implicit_surface->Compute_Cell_Minimum_And_Maximum (false); |
| } |
| |
| void Initialize_Tetrahedralized_Volume (TETRAHEDRALIZED_VOLUME<T>& tetrahedralized_volume_input) |
| { |
| tetrahedralized_volume = &tetrahedralized_volume_input; |
| |
| if (!tetrahedralized_volume->bounding_box) tetrahedralized_volume->Update_Bounding_Box(); |
| |
| if (!tetrahedralized_volume->tetrahedron_list) tetrahedralized_volume->Update_Tetrahedron_List(); |
| |
| if (!tetrahedralized_volume->tetrahedron_hierarchy) |
| { |
| tetrahedralized_volume->Initialize_Tetrahedron_Hierarchy(); |
| tetrahedralized_volume->tetrahedron_hierarchy->Update_Box_Radii(); |
| } |
| } |
| |
| bool Implicit_Surface_Intersection (RAY_3D<T>& ray) const |
| { |
| RAY_3D<T> object_space_ray = Object_Space_Ray (ray); |
| |
| if (implicit_surface->Intersection (object_space_ray, surface_roughness)) |
| { |
| ray.semi_infinite = false; |
| ray.t_max = object_space_ray.t_max; |
| ray.aggregate_id = object_space_ray.aggregate_id; |
| return true; |
| } |
| else return false; |
| } |
| |
| virtual VECTOR_3D<T> Implicit_Surface_Normal (const VECTOR_3D<T>& location, const int aggregate = -1) const |
| { |
| return World_Space_Vector (implicit_surface->Normal (Object_Space_Point (location), aggregate)); |
| } |
| |
| VECTOR_3D<T> Implicit_Surface_Extended_Normal (const VECTOR_3D<T>& location, T& phi_value, const int aggregate = -1, const int location_particle_index = 0) const |
| { |
| phi_value = implicit_surface->Extended_Phi (Object_Space_Point (location)); |
| return World_Space_Vector (implicit_surface->Extended_Normal (Object_Space_Point (location), aggregate)); |
| } |
| |
| VECTOR_3D<T> Implicit_Surface_Normal (const VECTOR_3D<T>& location, T& phi_value, const int aggregate = -1, const int location_particle_index = 0) const |
| { |
| phi_value = (*implicit_surface) (Object_Space_Point (location)); |
| return World_Space_Vector (implicit_surface->Normal (Object_Space_Point (location), aggregate)); |
| } |
| |
| VECTOR_3D<T> Implicit_Surface_Extended_Normal (const VECTOR_3D<T>& location, const int aggregate = -1) const |
| { |
| return World_Space_Vector (implicit_surface->Extended_Normal (Object_Space_Point (location), aggregate)); |
| } |
| |
| VECTOR_3D<T> Implicit_Surface_Normal_In_Object_Space (const VECTOR_3D<T>& object_space_location, const int aggregate = -1) const |
| { |
| return World_Space_Vector (implicit_surface->Normal (object_space_location, aggregate)); |
| } |
| |
| VECTOR_3D<T> Implicit_Surface_Extended_Normal_In_Object_Space (const VECTOR_3D<T>& object_space_location, const int aggregate = -1) const |
| { |
| return World_Space_Vector (implicit_surface->Extended_Normal (object_space_location, aggregate)); |
| } |
| |
| bool Implicit_Surface_Inside (const VECTOR_3D<T>& location) const |
| { |
| return implicit_surface->Inside (Object_Space_Point (location), surface_roughness); |
| } |
| |
| bool Implicit_Surface_Inside_In_Object_Space (const VECTOR_3D<T>& object_space_location) const |
| { |
| return implicit_surface->Inside (object_space_location, surface_roughness); |
| } |
| |
| virtual bool Implicit_Surface_Lazy_Inside (const VECTOR_3D<T>& location, T contour_value = 0) const |
| { |
| return implicit_surface->Lazy_Inside (Object_Space_Point (location), contour_value); |
| } |
| |
| bool Implicit_Surface_Lazy_Inside_In_Object_Space (const VECTOR_3D<T>& object_space_location, T contour_value = 0) const |
| { |
| return implicit_surface->Lazy_Inside (object_space_location, contour_value); |
| } |
| |
| virtual bool Implicit_Surface_Lazy_Inside_And_Value (const VECTOR_3D<T>& location, T& phi, T contour_value = 0) const |
| |
| { |
| return implicit_surface->Lazy_Inside_And_Value (Object_Space_Point (location), phi, contour_value); |
| } |
| |
| bool Implicit_Surface_Lazy_Inside_And_Value_In_Object_Space (const VECTOR_3D<T>& location, T& phi, T contour_value = 0) const |
| { |
| return implicit_surface->Lazy_Inside_And_Value (location, phi, contour_value); |
| } |
| |
| bool Implicit_Surface_Lazy_Inside_Extended_Levelset_And_Value (const VECTOR_3D<T>& location, T& phi_value, T contour_value = 0) const |
| { |
| return implicit_surface->Lazy_Inside_Extended_Levelset_And_Value (Object_Space_Point (location), phi_value, contour_value); |
| } |
| |
| bool Implicit_Surface_Outside (const VECTOR_3D<T>& location) const |
| { |
| return implicit_surface->Outside (Object_Space_Point (location), surface_roughness); |
| } |
| |
| bool Implicit_Surface_Lazy_Outside (const VECTOR_3D<T>& location) const |
| { |
| return implicit_surface->Lazy_Outside (Object_Space_Point (location)); |
| } |
| |
| bool Implicit_Surface_Lazy_Outside_Extended_Levelset (const VECTOR_3D<T>& location, T contour_value = 0) const |
| { |
| return implicit_surface->Lazy_Outside_Extended_Levelset (Object_Space_Point (location), contour_value); |
| } |
| |
| bool Implicit_Surface_Lazy_Outside_Extended_Levelset_And_Value (const VECTOR_3D<T>& location, T& phi_value, T contour_value = 0) const |
| { |
| return implicit_surface->Lazy_Outside_Extended_Levelset_And_Value (Object_Space_Point (location), phi_value, contour_value); |
| } |
| |
| bool Implicit_Surface_Boundary (const VECTOR_3D<T>& location) const |
| { |
| return implicit_surface->Boundary (Object_Space_Point (location), surface_roughness); |
| } |
| |
| VECTOR_3D<T> Implicit_Surface_Surface (const VECTOR_3D<T>& location, const int max_iterations = 1) const |
| { |
| return World_Space_Point (implicit_surface->Surface (Object_Space_Point (location), surface_roughness, max_iterations)); |
| } |
| |
| T Implicit_Surface_Value (const VECTOR_3D<T>& location) const |
| { |
| return (*implicit_surface) (Object_Space_Point (location)); |
| } |
| |
| T Implicit_Surface_Value_In_Object_Space (const VECTOR_3D<T>& object_space_location) const |
| { |
| return (*implicit_surface) (object_space_location); |
| } |
| |
| void Update_Bounding_Box() |
| { |
| assert (triangulated_surface && triangulated_surface->bounding_box); |
| bounding_box_up_to_date = true; |
| oriented_box = ORIENTED_BOX_3D<T> (*triangulated_surface->bounding_box, orientation, World_Space_Point (triangulated_surface->bounding_box->Minimum_Corner())); |
| axis_aligned_bounding_box = oriented_box.Axis_Aligned_Bounding_Box(); |
| } |
| |
| void Update_Bounding_Box_From_Implicit_Surface() |
| { |
| assert (implicit_surface); |
| bounding_box_up_to_date = true; |
| oriented_box = ORIENTED_BOX_3D<T> (implicit_surface->box, orientation, World_Space_Point (implicit_surface->box.Minimum_Corner())); |
| axis_aligned_bounding_box = oriented_box.Axis_Aligned_Bounding_Box(); |
| } |
| |
| const ORIENTED_BOX_3D<T>& Oriented_Bounding_Box() const |
| { |
| assert (bounding_box_up_to_date); |
| return oriented_box; |
| } |
| |
| const BOX_3D<T>& Axis_Aligned_Bounding_Box() const |
| { |
| assert (bounding_box_up_to_date); |
| return axis_aligned_bounding_box; |
| } |
| |
| void Save_State (RIGID_BODY_STATE_3D<T>& state, const T time = 0) const |
| { |
| state.time = time; |
| state.position = position; |
| state.orientation = orientation; |
| state.velocity = velocity; |
| state.angular_momentum = angular_momentum; |
| state.angular_velocity = angular_velocity; |
| } |
| |
| void Restore_State (const RIGID_BODY_STATE_3D<T>& state) |
| { |
| position = state.position; |
| orientation = state.orientation; |
| velocity = state.velocity; |
| angular_momentum = state.angular_momentum; |
| angular_velocity = state.angular_velocity; |
| } |
| |
| void Save_State (const int state_index, const T time = 0) |
| { |
| if (saved_states.m < state_index) saved_states.Resize_Array (state_index); |
| |
| if (!saved_states (state_index)) saved_states (state_index) = new RIGID_BODY_STATE_3D<T>; |
| |
| Save_State (*saved_states (state_index), time); |
| } |
| |
| void Restore_State (const int state_index) |
| { |
| assert (saved_states (state_index)); |
| Restore_State (*saved_states (state_index)); |
| } |
| |
| void Delete_State (const int state_index) |
| { |
| delete saved_states (state_index); |
| saved_states (state_index) = 0; |
| } |
| |
| template<class RW> void Read_State (std::istream& input_stream, const int state_index) |
| { |
| if (saved_states.m < state_index) saved_states.Resize_Array (state_index); |
| |
| if (!saved_states (state_index)) saved_states (state_index) = new RIGID_BODY_STATE_3D<T>; |
| |
| saved_states (state_index)->template Read<RW> (input_stream); |
| } |
| |
| template<class RW> void Write_State (std::ostream& output_stream, const int state_index) |
| { |
| assert (saved_states (state_index)); |
| saved_states (state_index)->template Write<RW> (output_stream); |
| } |
| |
| T Thin_Shell_Density() const |
| { |
| return triangulated_surface->density; |
| } |
| |
| template<class RW> |
| void Read (std::istream& input_stream) |
| { |
| Read_Binary<RW> (input_stream, mass, inertia_tensor, surface_roughness, position, orientation, velocity, angular_momentum); |
| Update_Angular_Velocity(); |
| } |
| |
| template<class RW> |
| void Write (std::ostream& output_stream) const |
| { |
| Write_Binary<RW> (output_stream, mass, inertia_tensor, surface_roughness, position, orientation, velocity, angular_momentum); |
| } |
| |
| //##################################################################### |
| void Euler_Step (const T dt, const T time = 0, const T small_number = 1e-7); |
| void Euler_Step_Position (const T dt, const T time = 0, const T small_number = 1e-7); |
| void Euler_Step_Velocity (const T dt, const T time = 0); |
| void Add_Velocity_Independent_Forces (VECTOR_3D<T>& F, VECTOR_3D<T>& torque) const; |
| void Add_Velocity_Dependent_Forces (VECTOR_3D<T>& F, VECTOR_3D<T>& torque) const; // can depend on position too |
| void Initialize_CFL(); |
| T CFL (const T max_distance_per_time_step, const T max_rotation_per_time_step, const bool verbose = false); |
| void Interpolate_Between_States (const RIGID_BODY_STATE_3D<T>& state1, const RIGID_BODY_STATE_3D<T>& state2, const T time, RIGID_BODY_STATE_3D<T>& interpolated_state); |
| void Compute_Velocity_Between_States (const RIGID_BODY_STATE_3D<T>& state1, const RIGID_BODY_STATE_3D<T>& state2, RIGID_BODY_STATE_3D<T>& result_state); |
| static void Apply_Impulse (RIGID_BODY_3D<T>& body1, RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& location, const VECTOR_3D<T>& impulse); |
| static void Apply_Impulse_And_Angular_Impulse (RIGID_BODY_3D<T>& body1, RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& impulse, const VECTOR_3D<T>& angular_impulse); |
| static void Apply_Angular_Impulse (RIGID_BODY_3D<T>& body1, RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& angular_impulse); |
| static void Apply_Collision_Impulse (RIGID_BODY_3D<T>& body1, RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& location, const VECTOR_3D<T>& normal, const T coefficient_of_restitution, |
| const T coefficient_of_friction = 0, const bool clamp_friction_magnitude = true, const bool rolling_friction = false); |
| static void Apply_Collision_Impulse (RIGID_BODY_3D<T>& body1, RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& location, const VECTOR_3D<T>& normal, |
| const VECTOR_3D<T>& relative_velocity, const T coefficient_of_restitution, const T coefficient_of_friction = 0, const bool clamp_friction_magnitude = true, const bool rolling_friction = false); |
| static void Apply_Sticking_Impulse (RIGID_BODY_3D<T>& body1, RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& location); |
| static void Apply_Angular_Sticking_Impulse (RIGID_BODY_3D<T>& body1, RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& location); |
| static void Apply_Sticking_And_Angular_Sticking_Impulse (RIGID_BODY_3D<T>& body1, RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& location, |
| const VECTOR_3D<T>& delta_relative_angular_velocity, const VECTOR_3D<T>& delta_relative_linear_velocity, const T epsilon_scale = 1); |
| static void Apply_Rolling_Friction (RIGID_BODY_3D<T>& body1, RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& location, const VECTOR_3D<T>& normal, const T normal_impulse); |
| static void Find_Impulse_And_Angular_Impulse (const RIGID_BODY_3D<T>& body1, const RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& location, |
| const VECTOR_3D<T>& delta_rel_velocity_at_location, const VECTOR_3D<T>& delta_rel_angular_velocity, VECTOR_3D<T>& impulse, VECTOR_3D<T>& angular_impulse); |
| static void Apply_Push (RIGID_BODY_3D<T>& body1, RIGID_BODY_3D<T>& body2, const VECTOR_3D<T>& location, const VECTOR_3D<T>& normal, const T distance); |
| virtual void Adjust_Point_For_Collision (VECTOR_3D<T>& X, VECTOR_3D<T>& V, const T point_mass, const T penetration_depth, const T one_over_dt); |
| virtual void Adjust_Point_For_Collision (VECTOR_3D<T>& X, const T penetration_depth); |
| bool Earliest_Triangle_Crossover (const VECTOR_3D<T>& start_X, const VECTOR_3D<T>& end_X, const T dt, T& hit_time, VECTOR_3D<T>& normal, VECTOR_3D<T>& weights, int& triangle_id) const; |
| bool Latest_Triangle_Crossover (const VECTOR_3D<T>& start_X, const VECTOR_3D<T>& end_X, const T dt, T& hit_time, VECTOR_3D<T>& normal, VECTOR_3D<T>& weights, int& triangle_id, |
| typename TRIANGLE_3D<T>::POINT_TRIANGLE_COLLISION_TYPE& returned_collision_type) const; |
| bool Any_Triangle_Crossover (const VECTOR_3D<T>& start_X, const VECTOR_3D<T>& end_X, const T dt) const; |
| void Compute_Occupied_Cells (const GRID_3D<T>& grid, ARRAYS_3D<bool>& occupied, const bool with_body_motion, const T extra_thickness, const T body_thickness_factor, |
| const bool reset_occupied_to_false = true) const; |
| void Get_Triangle_Bounding_Boxes (LIST_ARRAY<BOX_3D<T> >& bounding_boxes, const bool with_body_motion, const T extra_thickness, const T body_thickness_factor) const; |
| void Update_Intersection_Acceleration_Structures (const bool use_swept_triangle_hierarchy, const int state1 = 0, const int state2 = 0); |
| //##################################################################### |
| }; |
| } |
| #endif |