-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
corotated_model_data.cc
37 lines (32 loc) · 1.23 KB
/
corotated_model_data.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
#include "drake/multibody/fem/corotated_model_data.h"
#include "drake/common/autodiff.h"
#include "drake/multibody/fem/matrix_utilities.h"
namespace drake {
namespace multibody {
namespace fem {
namespace internal {
template <typename T, int num_locations>
CorotatedModelData<T, num_locations>::CorotatedModelData() {
std::fill(R_.begin(), R_.end(), Matrix3<T>::Identity());
std::fill(S_.begin(), S_.end(), Matrix3<T>::Identity());
std::fill(Jm1_.begin(), Jm1_.end(), 0);
std::fill(JFinvT_.begin(), JFinvT_.end(), Matrix3<T>::Identity());
}
template <typename T, int num_locations>
void CorotatedModelData<T, num_locations>::UpdateFromDeformationGradient() {
const std::array<Matrix3<T>, num_locations>& F = this->deformation_gradient();
for (int i = 0; i < num_locations; ++i) {
Matrix3<T>& local_R = R_[i];
Matrix3<T>& local_S = S_[i];
Matrix3<T>& local_JFinvT = JFinvT_[i];
internal::PolarDecompose<T>(F[i], &local_R, &local_S);
Jm1_[i] = F[i].determinant() - 1.0;
internal::CalcCofactorMatrix<T>(F[i], &local_JFinvT);
}
}
template class CorotatedModelData<double, 1>;
template class CorotatedModelData<AutoDiffXd, 1>;
} // namespace internal
} // namespace fem
} // namespace multibody
} // namespace drake