1 /* ----------------------------------------------------------------------
2 This is the
3
4 ██╗ ██╗ ██████╗ ██████╗ ██████╗ ██╗ ██╗████████╗███████╗
5 ██║ ██║██╔════╝ ██╔════╝ ██╔════╝ ██║ ██║╚══██╔══╝██╔════╝
6 ██║ ██║██║ ███╗██║ ███╗██║ ███╗███████║ ██║ ███████╗
7 ██║ ██║██║ ██║██║ ██║██║ ██║██╔══██║ ██║ ╚════██║
8 ███████╗██║╚██████╔╝╚██████╔╝╚██████╔╝██║ ██║ ██║ ███████║
9 ╚══════╝╚═╝ ╚═════╝ ╚═════╝ ╚═════╝ ╚═╝ ╚═╝ ╚═╝ ╚══════╝®
10
11 DEM simulation engine, released by
12 DCS Computing Gmbh, Linz, Austria
13 http://www.dcs-computing.com, office@dcs-computing.com
14
15 LIGGGHTS® is part of CFDEM®project:
16 http://www.liggghts.com | http://www.cfdem.com
17
18 Core developer and main author:
19 Christoph Kloss, christoph.kloss@dcs-computing.com
20
21 LIGGGHTS® is open-source, distributed under the terms of the GNU Public
22 License, version 2 or later. It is distributed in the hope that it will
23 be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
24 of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. You should have
25 received a copy of the GNU General Public License along with LIGGGHTS®.
26 If not, see http://www.gnu.org/licenses . See also top-level README
27 and LICENSE files.
28
29 LIGGGHTS® and CFDEM® are registered trade marks of DCS Computing GmbH,
30 the producer of the LIGGGHTS® software and the CFDEM®coupling software
31 See http://www.cfdem.com/terms-trademark-policy for details.
32
33 -------------------------------------------------------------------------
34 Contributing author and copyright for this file:
35 Alexander Podlozhnyuk (DCS Computing GmbH, Linz)
36
37 Copyright 2015- DCS Computing GmbH, Linz
38 ------------------------------------------------------------------------- */
39
40 #include "math_extra_liggghts_nonspherical.h"
41
42 #include <map>
43 #include "math_const.h"
44
45 namespace MathExtraLiggghtsNonspherical {
46
47 //project point onto the surface defined by normal vector and some point on the surface
point_wall_projection(const double * normal_vector,const double * point_on_wall,const double * input_point,double * output_point)48 double point_wall_projection(const double *normal_vector, const double *point_on_wall, const double *input_point, double *output_point) {
49 const double x0 = point_on_wall[0];
50 const double y0 = point_on_wall[1];
51 const double z0 = point_on_wall[2];
52
53 const double x = input_point[0];
54 const double y = input_point[1];
55 const double z = input_point[2];
56
57 const double A = normal_vector[0];
58 const double B = normal_vector[1];
59 const double C = normal_vector[2];
60
61 const double alpha = -(A*(x - x0) + B*(y - y0) + C*(z - z0)) / (A*A + B*B + C*C);
62 output_point[0] = x + alpha * A;
63 output_point[1] = y + alpha * B;
64 output_point[2] = z + alpha * C;
65 return fabs((A*(x - x0) + B*(y - y0) + C*(z - z0)) / sqrt(A*A + B*B + C*C));
66 }
67
68 //calculate right hand side term in dynamic euler equation: dw/dt = f(w), w - angular velocity
dynamic_euler_right_term(const double * omega,const double * torque,const double * inertia,double * result)69 void dynamic_euler_right_term(const double *omega, const double *torque, const double *inertia, double *result)
70 {
71 const double wx = omega[0];
72 const double wy = omega[1];
73 const double wz = omega[2];
74
75 const double Ix = inertia[0];
76 const double Iy = inertia[1];
77 const double Iz = inertia[2];
78
79 result[0] = (wy*wz*(Iy - Iz) + torque[0]) / Ix;
80 result[1] = (wz*wx*(Iz - Ix) + torque[1]) / Iy;
81 result[2] = (wx*wy*(Ix - Iy) + torque[2]) / Iz;
82 }
83
84 //integrate angular velocity
integrate_omega(double * quat,double * torque,double * inertia,double * omega,double dt)85 void integrate_omega(double *quat, double *torque, double *inertia, double *omega, double dt)
86 {
87 double torque_body[3], omega_body[3];
88
89 MathExtraLiggghtsNonspherical::rotate_global2local(quat, omega, omega_body);
90 MathExtraLiggghtsNonspherical::rotate_global2local(quat, torque, torque_body);
91
92 double f[3];
93 dynamic_euler_right_term(omega_body, torque_body, inertia, f);
94 for(int i = 0; i < 3; i++)
95 omega_body[i] += dt*f[i];
96 MathExtraLiggghtsNonspherical::rotate_local2global(quat, omega_body, omega);
97 }
98
99 //distance between point and line
point_line_distance(double * pointA,double * pointB,double * inputPoint,double * closestPoint,double * delta)100 double point_line_distance(double *pointA, double *pointB, double *inputPoint, double *closestPoint, double *delta)
101 {
102 const double x0 = inputPoint[0];
103 const double y0 = inputPoint[1];
104 const double z0 = inputPoint[2];
105
106 const double nx = pointB[0] - pointA[0];
107 const double ny = pointB[1] - pointA[1];
108 const double nz = pointB[2] - pointA[2];
109
110 const double xA = pointA[0];
111 const double yA = pointA[1];
112 const double zA = pointA[2];
113
114 double alpha = ((x0 - xA)*nx + (y0 - yA)*ny + (z0 - zA)*nz) / (nx*nx + ny*ny + nz*nz);
115 if(alpha > 1.0)
116 alpha = 1.0;
117 if(alpha < 0.0)
118 alpha = 0.0;
119 closestPoint[0] = xA + alpha*nx;
120 closestPoint[1] = yA + alpha*ny;
121 closestPoint[2] = zA + alpha*nz;
122
123 LAMMPS_NS::vectorSubtract3D(closestPoint,inputPoint,delta);
124 return LAMMPS_NS::vectorMag3D(delta);
125 }
126
127 // integrate quaternion
integrate_quat(double * quat,double * omega,double dt)128 void integrate_quat(double *quat, double *omega, double dt)
129 {
130 double omega_mag = LAMMPS_NS::vectorMag3D(omega);
131 double deltaq[4];
132 if(omega_mag > 1e-8) {
133 deltaq[0] = cos(0.5*omega_mag*dt);
134 deltaq[1] = sin(0.5*omega_mag*dt) * omega[0]/omega_mag;
135 deltaq[2] = sin(0.5*omega_mag*dt) * omega[1]/omega_mag,
136 deltaq[3] = sin(0.5*omega_mag*dt) * omega[2]/omega_mag;
137 } else {
138 deltaq[0] = 1.0;
139 deltaq[1] = 0.5 * dt * omega[0];
140 deltaq[2] = 0.5 * dt * omega[1];
141 deltaq[3] = 0.5 * dt * omega[2];
142 }
143 double quat_temp[4];
144 MathExtra::quatquat(deltaq, quat, quat_temp);
145 MathExtra::qnormalize(quat_temp);
146 LAMMPS_NS::vectorCopy4D(quat_temp, quat);
147
148 /*double wq[4];
149 MathExtra::vecquat(omega, quat, wq);
150 for(int i = 0; i < 4; i++)
151 quat[i] += 0.5*dt * wq[i];
152 MathExtra::qnormalize(quat);*/
153 }
154
155 //calculate angular velocity from angular moment
angmom_to_omega(const double * quat,const double * angmom,const double * inertia,double * omega)156 void angmom_to_omega(const double *quat, const double *angmom, const double *inertia, double *omega)
157 {
158 double omega_body[3], angmom_body[3];
159 rotate_global2local(quat, angmom, angmom_body);
160 for(int k = 0; k < 3; k++)
161 omega_body[k] = angmom_body[k] / inertia[k];
162 rotate_local2global(quat, omega_body, omega);
163 }
164
165 //calculate angular moment from angular velocity
omega_to_angmom(const double * quat,const double * omega,const double * inertia,double * angmom)166 void omega_to_angmom(const double *quat, const double *omega, const double *inertia, double *angmom)
167 {
168 double omega_body[3], angmom_body[3];
169 rotate_global2local(quat, omega, omega_body);
170 for(int k = 0; k < 3; k++)
171 angmom_body[k] = omega_body[k] * inertia[k];
172 rotate_local2global(quat, angmom_body, angmom);
173 }
174
no_squish_rotate(int k,double * p,double * q,double * inertia,double dt)175 void no_squish_rotate(int k, double *p, double *q, double *inertia, double dt)
176 {
177 double phi,c_phi,s_phi,kp[4]={},kq[4]={};
178
179 // apply permuation operator on p and q, get kp and kq
180
181 if (k == 1) {
182 kq[0] = -q[1]; kp[0] = -p[1];
183 kq[1] = q[0]; kp[1] = p[0];
184 kq[2] = q[3]; kp[2] = p[3];
185 kq[3] = -q[2]; kp[3] = -p[2];
186 } else if (k == 2) {
187 kq[0] = -q[2]; kp[0] = -p[2];
188 kq[1] = -q[3]; kp[1] = -p[3];
189 kq[2] = q[0]; kp[2] = p[0];
190 kq[3] = q[1]; kp[3] = p[1];
191 } else if (k == 3) {
192 kq[0] = -q[3]; kp[0] = -p[3];
193 kq[1] = q[2]; kp[1] = p[2];
194 kq[2] = -q[1]; kp[2] = -p[1];
195 kq[3] = q[0]; kp[3] = p[0];
196 }
197
198 // obtain phi, cosines and sines
199
200 phi = p[0]*kq[0] + p[1]*kq[1] + p[2]*kq[2] + p[3]*kq[3];
201 phi /= 4.0 * inertia[k-1];
202 c_phi = cos(dt * phi);
203 s_phi = sin(dt * phi);
204
205 // advance p and q
206
207 p[0] = c_phi*p[0] + s_phi*kp[0];
208 p[1] = c_phi*p[1] + s_phi*kp[1];
209 p[2] = c_phi*p[2] + s_phi*kp[2];
210 p[3] = c_phi*p[3] + s_phi*kp[3];
211
212 q[0] = c_phi*q[0] + s_phi*kq[0];
213 q[1] = c_phi*q[1] + s_phi*kq[1];
214 q[2] = c_phi*q[2] + s_phi*kq[2];
215 q[3] = c_phi*q[3] + s_phi*kq[3];
216 }
217
calc_conjqm(double * quat,double * angmom_body,double * conjqm)218 void calc_conjqm(double *quat, double *angmom_body, double *conjqm)
219 {
220 MathExtra::quatvec(quat,angmom_body,conjqm);
221 conjqm[0] *= 2.0;
222 conjqm[1] *= 2.0;
223 conjqm[2] *= 2.0;
224 conjqm[3] *= 2.0;
225 }
226
clamp(double val,double min,double max)227 double clamp(double val, double min, double max)
228 {
229 if(val < min) return min;
230 if(val > max) return max;
231 return val;
232 }
233
line_segments_distance(double * P1,double * Q1,double * P2,double * Q2,double * C1,double * C2)234 void line_segments_distance(double *P1, double *Q1, double *P2, double *Q2, double *C1, double *C2)
235 {
236 double tol = 1e-14;
237 double d1[3], d2[3], r[3];
238 LAMMPS_NS::vectorSubtract3D(Q1, P1, d1);
239 LAMMPS_NS::vectorSubtract3D(Q2, P2, d2);
240 LAMMPS_NS::vectorSubtract3D(P1, P2, r);
241
242 const double a = LAMMPS_NS::vectorDot3D(d1, d1); //always >= 0
243 const double e = LAMMPS_NS::vectorDot3D(d2, d2); //always >= 0
244 const double f = LAMMPS_NS::vectorDot3D(d2, r);
245
246 double s, t;
247
248 if(a <= tol && e <= tol) { //line segments with zero length (points)
249 s = t = 0.0;
250 LAMMPS_NS::vectorCopy3D(P1, C1);
251 LAMMPS_NS::vectorCopy3D(P2, C2);
252 return;
253 }
254 if(a <= tol) {
255 s = 0.0;
256 t = clamp(f / e, 0.0, 1.0);
257 } else {
258 const double c = LAMMPS_NS::vectorDot3D(d1, r);
259 if(e <= tol) {
260 t = 0.0;
261 s = clamp(-c / a, 0.0, 1.0);
262 } else {
263 const double b = LAMMPS_NS::vectorDot3D(d1, d2);
264 const double determinant = a*e - b*b; //always non-negative
265 if(determinant >= tol*tol) { //segments non parallel
266 s = clamp( (b*f - c*e)/determinant, 0.0, 1.0);
267 t = (b*s + f) / e;
268 if(t < 0.0) {
269 t = 0.0;
270 s = clamp(-c / a, 0.0, 1.0);
271 } else if(t > 1.0) {
272 t = 1.0;
273 s = clamp((b-c) / a, 0.0, 1.0);
274 }
275 } else { //segments are parallel
276 double s1 = 0.0;
277 double s2 = 1.0;
278 double t1 = (f + s1*b) / e;
279 double t2 = (f + s2*b) / e;
280 if(t1 < 0.0) {
281 t1 = 0.0;
282 s1 = clamp(-c / a, 0.0, 1.0);
283 } else if(t1 > 1.0) {
284 t1 = 1.0;
285 s1 = clamp((b-c) / a, 0.0, 1.0);
286 }
287 if(t2 < 0.0) {
288 t2 = 0.0;
289 s2 = clamp(-c / a, 0.0, 1.0);
290 } else if(t2 > 1.0) {
291 t2 = 1.0;
292 s2 = clamp((b-c) / a, 0.0, 1.0);
293 }
294 s = 0.5*(s1 + s2);
295 t = 0.5*(t1 + t2);
296 }
297 }
298 }
299 for(int i = 0; i < 3; i++) {
300 C1[i] = P1[i] + d1[i]*s;
301 C2[i] = P2[i] + d2[i]*t;
302 }
303 }
304
305 //effective curvature radius of a pair of particles at the contact point
get_effective_radius(SurfacesIntersectData & sidata,double * blockiness_i,double * blockiness_j,double koefi,double koefj,double curvatureLimitFactor,LAMMPS_NS::Error * error)306 double get_effective_radius(SurfacesIntersectData & sidata, double *blockiness_i, double *blockiness_j, double koefi, double koefj, double curvatureLimitFactor, LAMMPS_NS::Error *error)
307 {
308 #ifdef SUPERQUADRIC_ACTIVE_FLAG
309 if(blockiness_i == NULL)
310 error->one(FLERR,"blockiness_i array in get_effective_radius() is NULL");
311 if(blockiness_j == NULL)
312 error->one(FLERR,"blockiness_j array in get_effective_radius() is NULL");
313
314 double keff = koefi + koefj;
315 //if(blockiness_i[0] == 2.0 and blockiness_i[1] == 2.0 and blockiness_j[0] == 2.0 and blockiness_j[1] == 2.0)
316 // return 1.0 / keff;
317 double rmax = sidata.reff * curvatureLimitFactor;
318 return (keff < 1.0/rmax)? rmax: 1.0/keff;
319 #else
320 return 0.;
321 #endif
322 }
323
get_effective_radius_wall(SurfacesIntersectData & sidata,double * blockiness_i,double koefi,double curvatureLimitFactor,LAMMPS_NS::Error * error)324 double get_effective_radius_wall(SurfacesIntersectData & sidata, double *blockiness_i, double koefi, double curvatureLimitFactor, LAMMPS_NS::Error *error)
325 {
326 #ifdef SUPERQUADRIC_ACTIVE_FLAG
327
328 double keff = koefi;
329 if(blockiness_i == NULL)
330 error->one(FLERR,"blockiness_i array in get_effective_radius_wall() is NULL");
331 //if(blockiness_i[0] == 2.0 and blockiness_i[1] == 2.0)
332 // return 1.0 / keff;
333 double rmax = sidata.radi * curvatureLimitFactor;
334 return (keff < 1.0/rmax)? rmax: 1.0/keff;
335 #else
336 return 0.;
337 #endif
338 }
339
340 //distance between two points in N-dimensional space
dist(const double * v1,const double * v2,int len)341 inline double dist(const double *v1, const double *v2, int len)
342 {
343 return sqrt(distsq(v1, v2, len));
344 }
345
346 }
347