| //##################################################################### |
| // Copyright 2004, Igor Neverov, Eftychios Sifakis. |
| // This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt. |
| //##################################################################### |
| // Class GLOBAL_FRAME_CONTROL_SET |
| //##################################################################### |
| #ifndef __GLOBAL_FRAME_CONTROL_SET__ |
| #define __GLOBAL_FRAME_CONTROL_SET__ |
| |
| #include "FACE_CONTROL_SET.h" |
| #include "QUASI_RIGID_TRANSFORM_3D.h" |
| |
| namespace PhysBAM |
| { |
| |
| template <class T> |
| class GLOBAL_FRAME_CONTROL_SET: public FACE_CONTROL_SET<T> |
| { |
| public: |
| QUASI_RIGID_TRANSFORM_3D<T> global_transform, global_transform_save; |
| ARRAY<bool> coefficient_active; |
| ARRAY<VECTOR_3D<T> >& X; |
| ARRAY<VECTOR_3D<T> > X_save; |
| LIST_ARRAY<LIST_ARRAY<int> >& attached_nodes; |
| T rigidity_penalty_coefficient; |
| |
| GLOBAL_FRAME_CONTROL_SET (ARRAY<VECTOR_3D<T> >& X_input, LIST_ARRAY<LIST_ARRAY<int> >& attached_nodes_input) |
| : coefficient_active (12), X (X_input), attached_nodes (attached_nodes_input), rigidity_penalty_coefficient ( (T) 1000) |
| { |
| Save_Controls(); |
| ARRAY<bool>::copy (true, coefficient_active); |
| X_save = X; |
| } |
| |
| int Size() const |
| { |
| return 12; |
| } |
| |
| T operator() (const int control_id) const |
| { |
| return global_transform (control_id); |
| } |
| |
| T& operator() (const int control_id) |
| { |
| return global_transform (control_id); |
| } |
| |
| bool Control_Active (const int control_id) const |
| { |
| return coefficient_active (control_id); |
| } |
| |
| bool& Control_Active (const int control_id) |
| { |
| return coefficient_active (control_id); |
| } |
| |
| bool Positions_Determined_Kinematically (const int control_id) const |
| { |
| return true; |
| } |
| |
| void Position_Derivative (ARRAY<VECTOR_3D<T> >& dXdl, const int control_id) const |
| { |
| assert (1 <= control_id && control_id <= 12); |
| MATRIX_3X3<T> affine_differential, affine_differential_transformed; |
| VECTOR_3D<T> translation_differential, translation_differential_transformed; |
| |
| if (control_id <= 9) affine_differential.x[control_id - 1] = (T) 1; |
| else translation_differential[control_id - 9] = (T) 1; |
| |
| affine_differential_transformed = affine_differential * global_transform.affine_transform.Inverse(); |
| translation_differential_transformed = translation_differential - affine_differential_transformed * global_transform.translation; |
| |
| for (int i = 1; i <= dXdl.m; i++) dXdl (i) = affine_differential_transformed * X (i) + translation_differential_transformed; |
| |
| for (int i = 1; i <= attached_nodes.m; i++) for (int j = 1; j <= attached_nodes (i).m; j++) dXdl (attached_nodes (i) (j)) = affine_differential * X_save (attached_nodes (i) (j)) + translation_differential; |
| } |
| |
| void Set_Attachment_Positions (ARRAY<VECTOR_3D<T> >&X) const |
| { |
| for (int i = 1; i <= attached_nodes.m; i++) for (int j = 1; j <= attached_nodes (i).m; j++) X (attached_nodes (i) (j)) = global_transform.affine_transform * X_save (attached_nodes (i) (j)) + global_transform.translation; |
| } |
| |
| void Save_Controls() |
| { |
| global_transform_save = global_transform; |
| } |
| |
| void Kinematically_Update_Positions (ARRAY<VECTOR_3D<T> >&X) const |
| { |
| QUASI_RIGID_TRANSFORM_3D<T> global_transform_incremental = QUASI_RIGID_TRANSFORM_3D<T>::Incremental_Transform (global_transform, global_transform_save); |
| |
| for (int i = 1; i <= X.m; i++) X (i) = global_transform_incremental.affine_transform * X (i) + global_transform_incremental.translation; |
| } |
| |
| void Kinematically_Update_Jacobian (ARRAY<VECTOR_3D<T> >&dX) const |
| { |
| QUASI_RIGID_TRANSFORM_3D<T> global_transform_incremental = QUASI_RIGID_TRANSFORM_3D<T>::Incremental_Transform (global_transform, global_transform_save); |
| |
| for (int i = 1; i <= X.m; i++) dX (i) = global_transform_incremental.affine_transform * dX (i); |
| } |
| |
| T Penalty() const |
| { |
| return rigidity_penalty_coefficient * global_transform.Rigidity_Penalty(); |
| } |
| |
| T Penalty_Gradient (const int control_id) const |
| { |
| return rigidity_penalty_coefficient * global_transform.Rigidity_Penalty_Gradient (control_id); |
| } |
| |
| T Penalty_Hessian (const int control_id1, const int control_id2) const |
| { |
| return rigidity_penalty_coefficient * global_transform.Ridigity_Penalty_Hessian_Definite_Part (control_id1, control_id2); |
| } |
| |
| void Print_Diagnostics (std::ostream& output = std::cout) const |
| { |
| global_transform.Print_Diagnostics (output); |
| } |
| |
| //##################################################################### |
| }; |
| } |
| #endif |
