@@ -44,85 +44,64 @@ namespace SPH
4444 // =================================================================================================//
4545 Vec2d getVectorChangeRateAfterThinStructureRotation (const Vec2d &initial_vector, const Vec2d &rotation_angles, const Vec2d &angular_vel)
4646 {
47- /* *The derivative of the rotation matrix. */
48- Real sin_angle = sin (rotation_angles[0 ]);
49- Real cos_angle = cos (rotation_angles[0 ]);
50- Mat2d drotation_matrix_dt{
51- {-sin_angle * angular_vel[0 ], cos_angle * angular_vel[0 ]},
52- {-cos_angle * angular_vel[0 ], -sin_angle * angular_vel[0 ]},
53- };
54-
55- return drotation_matrix_dt * initial_vector;
47+ return Vec2d (cos (rotation_angles[0 ]) * angular_vel[0 ], -sin (rotation_angles[0 ]) * angular_vel[0 ]);
5648 }
5749 // =================================================================================================//
5850 Vec3d getVectorChangeRateAfterThinStructureRotation (const Vec3d &initial_vector, const Vec3d &rotation_angles, const Vec3d &angular_vel)
5951 {
60- /* *The rotation matrix about the X-axis. */
61- Real sin_angle_x = sin (rotation_angles[0 ]);
62- Real cos_angle_x = cos (rotation_angles[0 ]);
63-
64- Real sin_angle_y = sin (rotation_angles[1 ]);
65- Real cos_angle_y = cos (rotation_angles[1 ]);
52+ Real sin_rotation_0 = sin (rotation_angles[0 ]);
53+ Real cos_rotation_0 = cos (rotation_angles[0 ]);
6654
67- Real angular_vel_x = angular_vel[ 0 ] ;
68- Real angular_vel_y = angular_vel [1 ];
55+ Real sin_rotation_1 = sin (rotation_angles[ 1 ]) ;
56+ Real cos_rotation_1 = cos (rotation_angles [1 ]) ;
6957
70- Mat3d rotation_matrix_x{
71- {1.0 , 0.0 , 0.0 },
72- {0.0 , cos_angle_x, -sin_angle_x},
73- {0.0 , sin_angle_x, cos_angle_x},
74- };
75- /* *The rotation matrix about the Y-axis. */
76- Mat3d rotation_matrix_y{
77- {cos_angle_y, 0.0 , sin_angle_y},
78- {0.0 , 1.0 , 0.0 },
79- {-sin_angle_y, 0.0 , cos_angle_y},
80- };
81- /* *The derivative of the rotation matrix of the X-axis. */
82- Mat3d drotation_matrix_x_dt{
83- {0.0 , 0.0 , 0.0 },
84- {0.0 ,-sin_angle_x * angular_vel_x, -cos_angle_x * angular_vel_x},
85- {0.0 , cos_angle_x * angular_vel_x, -sin_angle_x * angular_vel_x},
86- };
87- /* *The derivative of the rotation matrix of the Y-axis. */
88- Mat3d drotation_matrix_y_dt{
89- {-sin_angle_y * angular_vel_y, 0.0 , cos_angle_y * angular_vel_y},
90- {0.0 , 0.0 , 0.0 },
91- {-cos_angle_y * angular_vel_y, 0.0 ,-sin_angle_y * angular_vel_y},
92- };
58+ Real dpseudo_n_dt_0 = -sin_rotation_0 * sin_rotation_1 * angular_vel[0 ] + cos_rotation_0 * cos_rotation_1 * angular_vel[1 ];
59+ Real dpseudo_n_dt_1 = -cos_rotation_0 * angular_vel[0 ];
60+ Real dpseudo_n_dt_2 = -sin_rotation_0 * cos_rotation_1 * angular_vel[0 ] - cos_rotation_0 * sin_rotation_1 * angular_vel[1 ];
9361
94- return (drotation_matrix_y_dt * rotation_matrix_x + rotation_matrix_y * drotation_matrix_x_dt)* initial_vector ;
62+ return Vec3d (dpseudo_n_dt_0, dpseudo_n_dt_1, dpseudo_n_dt_2) ;
9563 }
9664 // =================================================================================================//
9765 Vec2d getRotationFromPseudoNormalForFiniteDeformation (const Vec2d &dpseudo_n_d2t, const Vec2d &rotation, const Vec2d &angular_vel, Real dt)
98- {
99- Vec2d dangular_vel_dt = Vec2d::Zero ();
100- dangular_vel_dt[0 ] = -(dpseudo_n_d2t[0 ] + sin (rotation[0 ]) * pow (angular_vel[0 ], 2 ))
101- / (2 * sin (rotation[0 ]) * angular_vel[0 ] * dt - cos (rotation[0 ]));
102- return dangular_vel_dt;
66+ {
67+ Real cos_rotation_0 = cos (rotation[0 ]);
68+ Real sin_rotation_0 = sin (rotation[0 ]);
69+
70+ Real angle_vel_dt_0 = cos_rotation_0 * (dpseudo_n_d2t[0 ] + sin_rotation_0 * angular_vel[0 ] * angular_vel[0 ])
71+ - sin_rotation_0 * (dpseudo_n_d2t[1 ] + cos_rotation_0 * angular_vel[0 ] * angular_vel[0 ]);
72+
73+ return Vec2d (angle_vel_dt_0, 0.0 );
10374 }
10475 // =================================================================================================//
10576 Vec3d getRotationFromPseudoNormalForFiniteDeformation (const Vec3d &dpseudo_n_d2t, const Vec3d &rotation, const Vec3d &angular_vel, Real dt)
10677 {
107- Real sin_rotation_x = sin (rotation[0 ]);
108- Real cos_rotation_x = cos (rotation[0 ]);
109- Real sin_rotation_y = sin (rotation[1 ]);
110- Real cos_rotation_y = cos (rotation[1 ]);
78+ Real sin_rotation_0 = sin (rotation[0 ]);
79+ Real cos_rotation_0 = cos (rotation[0 ]);
80+ Real sin_rotation_1 = sin (rotation[1 ]);
81+ Real cos_rotation_1 = cos (rotation[1 ]);
11182
112- Real angle_vel_dt_x = (dpseudo_n_d2t[1 ] - sin_rotation_x * pow (angular_vel[0 ], 2 ))
113- / (2 * sin_rotation_x * angular_vel[0 ] * dt - cos_rotation_x);
83+ Real rotation_0_a = -(dpseudo_n_d2t[2 ] * cos_rotation_1 + dpseudo_n_d2t[0 ] * sin_rotation_1
84+ + angular_vel[1 ] * angular_vel[1 ] * cos_rotation_0
85+ + angular_vel[0 ] * angular_vel[0 ] * cos_rotation_0);
86+ Real rotation_0_b = sin_rotation_0 * angular_vel[0 ] * angular_vel[0 ] - dpseudo_n_d2t[1 ];
87+ Real angle_vel_dt_0 = sin_rotation_0 * rotation_0_a + cos_rotation_0 * rotation_0_b;
11488
115- Real angle_vel_dt_y = (dpseudo_n_d2t[0 ] + cos_rotation_x * sin_rotation_y
116- * (pow (angular_vel[0 ], 2 ) + pow (angular_vel[1 ], 2 ))
117- + 2 * sin_rotation_x * cos_rotation_y * angular_vel[0 ] * angular_vel[1 ]
118- + (2 * cos_rotation_x * sin_rotation_y * angular_vel[0 ] * dt
119- + 2 * sin_rotation_x * cos_rotation_y * angular_vel[1 ] * dt
120- + sin_rotation_x * cos_rotation_y) * angle_vel_dt_x)
121- / (-2 * sin_rotation_x * cos_rotation_y * angular_vel[0 ] * dt
122- - 2 * cos_rotation_x * sin_rotation_y * angular_vel[1 ] * dt
123- + cos_rotation_x * cos_rotation_y);
124-
125- return Vec3d (angle_vel_dt_x, angle_vel_dt_y, 0.0 );
89+ Real rotation_1_a = dpseudo_n_d2t[0 ] * cos_rotation_1 - dpseudo_n_d2t[2 ] * sin_rotation_1
90+ + 2.0 * angular_vel[1 ] * angular_vel[0 ] * sin_rotation_0;
91+ Real rotation_1_b1 = dpseudo_n_d2t[0 ] * cos_rotation_0
92+ + angular_vel[1 ] * angular_vel[1 ] * cos_rotation_0 * cos_rotation_0 * sin_rotation_1
93+ + angular_vel[0 ] * angular_vel[0 ] * sin_rotation_1
94+ - dpseudo_n_d2t[1 ] * sin_rotation_1 * sin_rotation_0
95+ + 2.0 * angular_vel[1 ] * angular_vel[0 ] * cos_rotation_1 * cos_rotation_0 * sin_rotation_0;
96+ Real rotation_1_b2 = -(dpseudo_n_d2t[2 ] * cos_rotation_0
97+ + angular_vel[1 ] * angular_vel[1 ] * cos_rotation_1 * cos_rotation_0 * cos_rotation_0
98+ + angular_vel[0 ] * angular_vel[0 ] * cos_rotation_1
99+ - dpseudo_n_d2t[1 ] * cos_rotation_1 * sin_rotation_0
100+ - 2.0 * angular_vel[1 ] * angular_vel[0 ] * cos_rotation_0 * sin_rotation_1 * sin_rotation_0);
101+ Real angle_vel_dt_1 = rotation_1_a * rotation_1_a * (rotation_1_b1 * cos_rotation_1 + rotation_1_b2 * sin_rotation_1)
102+ / (rotation_1_b1 * rotation_1_b1 + rotation_1_b2 * rotation_1_b2 + Eps);
103+
104+ return Vec3d (angle_vel_dt_0, angle_vel_dt_1, 0.0 );
126105 }
127106 // =================================================================================================//
128107 Vec2d getRotationFromPseudoNormalForSmallDeformation (const Vec2d &dpseudo_n_d2t, const Vec2d &rotation, const Vec2d &angular_vel, Real dt)
0 commit comments