-
Notifications
You must be signed in to change notification settings - Fork 211
/
thin_structure_math.cpp
247 lines (234 loc) · 12.1 KB
/
thin_structure_math.cpp
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
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
/**
* @file thin_structure_math.cpp
* @author Dong Wu, Chi ZHang and Xiangyu Hu
* @version 0.1
*/
#include "thin_structure_math.h"
using namespace SimTK;
namespace SPH
{
namespace thin_structure_dynamics
{
//=================================================================================================//
Vec2d getVectorAfterThinStructureRotation(const Vec2d &initial_vector, const Vec2d &rotation_angles)
{
/**The rotation matrix. */
Mat2d rotation_matrix(0.0);
rotation_matrix[0][0] = cos(rotation_angles[0]);
rotation_matrix[0][1] = sin(rotation_angles[0]);
rotation_matrix[1][0] = -rotation_matrix[0][1];
rotation_matrix[1][1] = rotation_matrix[0][0];
return rotation_matrix * initial_vector;
}
//=================================================================================================//
Vec3d getVectorAfterThinStructureRotation(const Vec3d &initial_vector, const Vec3d &rotation_angles)
{
/**The rotation matrix about the X-axis. */
Mat3d rotation_matrix_x(0.0);
rotation_matrix_x[0][0] = 1.0;
rotation_matrix_x[1][1] = cos(rotation_angles[0]);
rotation_matrix_x[1][2] = -sin(rotation_angles[0]);
rotation_matrix_x[2][1] = -rotation_matrix_x[1][2];
rotation_matrix_x[2][2] = rotation_matrix_x[1][1];
/**The rotation matrix about the Y-axis. */
Mat3d rotation_matrix_y(0.0);
rotation_matrix_y[0][0] = cos(rotation_angles[1]);
rotation_matrix_y[0][2] = sin(rotation_angles[1]);
rotation_matrix_y[1][1] = 1.0;
rotation_matrix_y[2][0] = -rotation_matrix_y[0][2];
rotation_matrix_y[2][2] = rotation_matrix_y[0][0];
return rotation_matrix_y * rotation_matrix_x * initial_vector;
}
//=================================================================================================//
Vec2d getVectorChangeRateAfterThinStructureRotation(const Vec2d &initial_vector, const Vec2d &rotation_angles, const Vec2d &angular_vel)
{
/**The derivative of the rotation matrix. */
Mat2d drotation_matrix_dt(0.0);
drotation_matrix_dt[0][0] = -sin(rotation_angles[0]) * angular_vel[0];
drotation_matrix_dt[0][1] = cos(rotation_angles[0]) * angular_vel[0];
drotation_matrix_dt[1][0] = -drotation_matrix_dt[0][1];
drotation_matrix_dt[1][1] = drotation_matrix_dt[0][0];
return drotation_matrix_dt * initial_vector;
}
//=================================================================================================//
Vec3d getVectorChangeRateAfterThinStructureRotation(const Vec3d &initial_vector, const Vec3d &rotation_angles, const Vec3d &angular_vel)
{
/**The rotation matrix about the X-axis. */
Mat3d rotation_matrix_x(0.0);
rotation_matrix_x[0][0] = 1.0;
rotation_matrix_x[1][1] = cos(rotation_angles[0]);
rotation_matrix_x[1][2] = -sin(rotation_angles[0]);
rotation_matrix_x[2][1] = -rotation_matrix_x[1][2];
rotation_matrix_x[2][2] = rotation_matrix_x[1][1];
/**The rotation matrix about the Y-axis. */
Mat3d rotation_matrix_y(0.0);
rotation_matrix_y[0][0] = cos(rotation_angles[1]);
rotation_matrix_y[0][2] = sin(rotation_angles[1]);
rotation_matrix_y[1][1] = 1.0;
rotation_matrix_y[2][0] = -rotation_matrix_y[0][2];
rotation_matrix_y[2][2] = rotation_matrix_y[0][0];
/**The derivative of the rotation matrix of the X-axis. */
Mat3d drotation_matrix_x_dt(0.0);
drotation_matrix_x_dt[1][1] = -sin(rotation_angles[0]) * angular_vel[0];
drotation_matrix_x_dt[1][2] = -cos(rotation_angles[0]) * angular_vel[0];
drotation_matrix_x_dt[2][1] = -drotation_matrix_x_dt[1][2];
drotation_matrix_x_dt[2][2] = drotation_matrix_x_dt[1][1];
/**The derivative of the rotation matrix of the Y-axis. */
Mat3d drotation_matrix_y_dt(0.0);
drotation_matrix_y_dt[0][0] = -sin(rotation_angles[1]) * angular_vel[1];
drotation_matrix_y_dt[0][2] = cos(rotation_angles[1]) * angular_vel[1];
drotation_matrix_y_dt[2][0] = -drotation_matrix_y_dt[0][2];
drotation_matrix_y_dt[2][2] = drotation_matrix_y_dt[0][0];
return (drotation_matrix_y_dt * rotation_matrix_x + rotation_matrix_y * drotation_matrix_x_dt)* initial_vector;
}
//=================================================================================================//
Vec2d getRotationFromPseudoNormalForFiniteDeformation(const Vec2d &dpseudo_n_d2t, const Vec2d &rotation, const Vec2d &angular_vel, Real dt)
{
Vec2d dangular_vel_dt(0.0);
dangular_vel_dt[0] = -(dpseudo_n_d2t[0] + sin(rotation[0]) * powerN(angular_vel[0], 2))
/ (2 * sin(rotation[0]) * angular_vel[0] * dt - cos(rotation[0]));
return dangular_vel_dt;
}
//=================================================================================================//
Vec3d getRotationFromPseudoNormalForFiniteDeformation(const Vec3d &dpseudo_n_d2t, const Vec3d &rotation, const Vec3d &angular_vel, Real dt)
{
Vec3d dangular_vel_dt(0.0);
dangular_vel_dt[0] = (dpseudo_n_d2t[1] - sin(rotation[0]) * powerN(angular_vel[0], 2))
/ (2 * sin(rotation[0]) * angular_vel[0] * dt - cos(rotation[0]));
dangular_vel_dt[1] = (dpseudo_n_d2t[0] + cos(rotation[0]) * sin(rotation[1])
* (powerN(angular_vel[0], 2) + powerN(angular_vel[1], 2))
+ 2 * sin(rotation[0]) * cos(rotation[1]) * angular_vel[0] * angular_vel[1]
+ (2 * cos(rotation[0]) * sin(rotation[1]) * angular_vel[0] * dt
+ 2 * sin(rotation[0]) * cos(rotation[1]) * angular_vel[1] * dt
+ sin(rotation[0]) * cos(rotation[1])) * dangular_vel_dt[0])
/ (-2 * sin(rotation[0]) * cos(rotation[1]) * angular_vel[0] * dt
- 2 * cos(rotation[0]) * sin(rotation[1]) * angular_vel[1] * dt
+ cos(rotation[0]) * cos(rotation[1]));
return dangular_vel_dt;
}
//=================================================================================================//
Vec2d getRotationFromPseudoNormalForSmallDeformation(const Vec2d &dpseudo_n_d2t, const Vec2d &rotation, const Vec2d &angular_vel, Real dt)
{
Vec2d dangular_vel_dt(0.0);
dangular_vel_dt[0] = dpseudo_n_d2t[0];
return dangular_vel_dt;
}
//=================================================================================================//
Vec3d getRotationFromPseudoNormalForSmallDeformation(const Vec3d &dpseudo_n_d2t, const Vec3d &rotation, const Vec3d &angular_vel, Real dt)
{
Vec3d dangular_vel_dt(0.0);
dangular_vel_dt[0] = -dpseudo_n_d2t[1];
dangular_vel_dt[1] = dpseudo_n_d2t[0];
return dangular_vel_dt;
}
//=================================================================================================//
Vec2d getNormalFromDeformationGradientTensor(const Mat2d &F)
{
Vec2d n = Vec2d(-F.col(0)[1], F.col(0)[0]);
n = n / (n.norm() + Eps);
return n;
}
//=================================================================================================//
Vec3d getNormalFromDeformationGradientTensor(const Mat3d &F)
{
Vec3d n = F.col(0) % F.col(1);
n = n / (n.norm() + Eps);
return n;
}
//=================================================================================================//
Vecd getLinearVariableJump(const Vecd &e_ij, const Real &r_ij, const Vecd &particle_i_value,
const Matd &gradient_particle_i_value, const Vecd &particle_j_value, const Matd &gradient_particle_j_value)
{
return particle_i_value - particle_j_value
- 0.5 * r_ij * (gradient_particle_i_value + gradient_particle_j_value) * e_ij;
}
//=================================================================================================//
Vecd getWENOVariableJump(const Vecd &e_ij, const Real &r_ij, const Vecd &particle_i_value,
const Matd &gradient_particle_i_value, const Vecd &particle_j_value, const Matd &gradient_particle_j_value)
{
return getWENOLeftState(e_ij, r_ij, particle_i_value,
gradient_particle_i_value, particle_j_value, gradient_particle_j_value)
- getWENORightState(e_ij, r_ij, particle_i_value,
gradient_particle_i_value, particle_j_value, gradient_particle_j_value);
}
//=================================================================================================//
Vecd getWENOStateWithStencilPoints(const Vecd &v1, const Vecd &v2, const Vecd &v3, const Vecd &v4)
{
Vecd f1 = 0.5 * v2 + 0.5 * v3;
Vecd f2 = -0.5 * v1 + 1.5 * v2;
Vecd f3 = v2 / 3.0 + 5.0 * v3 / 6.0 - v4 / 6.0;
Real epsilon = 1.0e-6;
Real s1 = dot(v2 - v3, v2 - v3) + epsilon;
Real s2 = dot(v2 - v1, v2 - v1) + epsilon;
Real s3 = dot(3.0 * v2 - 4.0 * v3 + v4, 3.0 * v2 - 4.0 * v3 + v4) / 4.0
+ 13.0 * dot(v2 - 2.0 * v3 + v4, v2 - 2.0 * v3 + v4) / 12.0 + epsilon;
Real s12 = 13.0 * dot(v1 - 2.0 * v2 + v3, v1 - 2.0 * v2 + v3) / 12.0
+ dot(v1 - v3, v1 - v3) / 4.0 + epsilon;
Real s4 = (dot(v1, 6649.0 * v1 - 30414.0 * v2 + 23094.0 * v3 - 5978.0 * v4)
+ 3.0 * dot(v2, 13667.0 * v2 - 23534.0 * v3 + 6338.0 * v4)
+ 3.0 * dot(v3, 11147.0 * v3 - 6458.0 * v4)
+ 3169.0 * dot(v4, v4)) / 2880.0;
Real tau_4 = s4 - 0.5 * (s1 + s2);
Real alpha_1 = (1.0 + (tau_4 / s1) * (tau_4 / s12)) / 3.0;
Real alpha_2 = (1.0 + (tau_4 / s2) * (tau_4 / s12)) / 6.0;
Real alpha_3 = (1.0 + tau_4 / s3) / 2.0;
Real w_1 = alpha_1 / (alpha_1 + alpha_2 + alpha_3);
Real w_2 = alpha_2 / (alpha_1 + alpha_2 + alpha_3);
Real w_3 = alpha_3 / (alpha_1 + alpha_2 + alpha_3);
return w_1 * f1 + w_2 * f2 + w_3 * f3;
}
//=================================================================================================//
Vecd getWENOLeftState(const Vecd &e_ij, const Real &r_ij, const Vecd &particle_i_value,
const Matd &gradient_particle_i_value, const Vecd &particle_j_value, const Matd &gradient_particle_j_value)
{
Vecd v1 = particle_i_value + gradient_particle_i_value * e_ij * r_ij;
Vecd v2 = particle_i_value;
Vecd v3 = particle_j_value;
Vecd v4 = particle_j_value - gradient_particle_j_value * e_ij * r_ij;
return getWENOStateWithStencilPoints(v1, v2, v3, v4);
}
//=================================================================================================//
Vecd getWENORightState(const Vecd &e_ij, const Real &r_ij, const Vecd &particle_i_value,
const Matd &gradient_particle_i_value, const Vecd &particle_j_value, const Matd &gradient_particle_j_value)
{
Vecd v1 = particle_j_value - gradient_particle_j_value * e_ij * r_ij;
Vecd v2 = particle_j_value;
Vecd v3 = particle_i_value;
Vecd v4 = particle_i_value + gradient_particle_i_value * e_ij * r_ij;
return getWENOStateWithStencilPoints(v1, v2, v3, v4);
}
//=================================================================================================//
Vec2d getRotationJump(const Vec2d &pseudo_n_jump, const Mat2d &transformation_matrix)
{
Vec2d local_rotation_jump(0.0);
Vec2d local_pseuodo_n_jump = transformation_matrix * pseudo_n_jump;
local_rotation_jump[0] = local_pseuodo_n_jump[0];
return ~transformation_matrix * local_rotation_jump;
}
//=================================================================================================//
Vec3d getRotationJump(const Vec3d &pseudo_n_jump, const Mat3d &transformation_matrix)
{
Vec3d local_rotation_jump(0.0);
Vec3d local_pseuodo_n_jump = transformation_matrix * pseudo_n_jump;
local_rotation_jump[0] = local_pseuodo_n_jump[0];
local_rotation_jump[1] = local_pseuodo_n_jump[1];
return ~transformation_matrix * local_rotation_jump;
}
//=================================================================================================//
Mat2d getCorrectedAlmansiStrain(const Mat2d ¤t_local_almansi_strain, const Real &nu_)
{
Mat2d corrected_almansi_strain = current_local_almansi_strain;
corrected_almansi_strain[1][1] = -nu_ * corrected_almansi_strain[0][0] / (1.0 - nu_);
return corrected_almansi_strain;
}
//=================================================================================================//
Mat3d getCorrectedAlmansiStrain(const Mat3d ¤t_local_almansi_strain, const Real &nu_)
{
Mat3d corrected_almansi_strain = current_local_almansi_strain;
corrected_almansi_strain[2][2]
= -nu_ * (corrected_almansi_strain[0][0] + corrected_almansi_strain[1][1]) / (1.0 - nu_);
return corrected_almansi_strain;
}
//=================================================================================================//
}
}