Cleanup: remove redundant doxygen \file argument
[blender.git] / source / blender / physics / intern / BPH_mass_spring.cpp
1 /*
2  * This program is free software; you can redistribute it and/or
3  * modify it under the terms of the GNU General Public License
4  * as published by the Free Software Foundation; either version 2
5  * of the License, or (at your option) any later version.
6  *
7  * This program is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
10  * GNU General Public License for more details.
11  *
12  * You should have received a copy of the GNU General Public License
13  * along with this program; if not, write to the Free Software Foundation,
14  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15  *
16  * The Original Code is Copyright (C) Blender Foundation
17  * All rights reserved.
18  */
19
20 /** \file \ingroup bph
21  */
22
23 extern "C" {
24 #include "MEM_guardedalloc.h"
25
26 #include "DNA_cloth_types.h"
27 #include "DNA_scene_types.h"
28 #include "DNA_object_force_types.h"
29 #include "DNA_object_types.h"
30 #include "DNA_meshdata_types.h"
31 #include "DNA_modifier_types.h"
32
33 #include "BLI_math.h"
34 #include "BLI_linklist.h"
35 #include "BLI_utildefines.h"
36
37 #include "BKE_cloth.h"
38 #include "BKE_collision.h"
39 #include "BKE_effect.h"
40 }
41
42 #include "BPH_mass_spring.h"
43 #include "implicit.h"
44
45 #include "DEG_depsgraph.h"
46 #include "DEG_depsgraph_query.h"
47
48 static float I3[3][3] = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}};
49
50 /* Number of off-diagonal non-zero matrix blocks.
51  * Basically there is one of these for each vertex-vertex interaction.
52  */
53 static int cloth_count_nondiag_blocks(Cloth *cloth)
54 {
55         LinkNode *link;
56         int nondiag = 0;
57
58         for (link = cloth->springs; link; link = link->next) {
59                 ClothSpring *spring = (ClothSpring *)link->link;
60                 switch (spring->type) {
61                         case CLOTH_SPRING_TYPE_BENDING_HAIR:
62                                 /* angular bending combines 3 vertices */
63                                 nondiag += 3;
64                                 break;
65
66                         default:
67                                 /* all other springs depend on 2 vertices only */
68                                 nondiag += 1;
69                                 break;
70                 }
71         }
72
73         return nondiag;
74 }
75
76 int BPH_cloth_solver_init(Object *UNUSED(ob), ClothModifierData *clmd)
77 {
78         Cloth *cloth = clmd->clothObject;
79         ClothVertex *verts = cloth->verts;
80         const float ZERO[3] = {0.0f, 0.0f, 0.0f};
81         Implicit_Data *id;
82         unsigned int i, nondiag;
83
84         nondiag = cloth_count_nondiag_blocks(cloth);
85         cloth->implicit = id = BPH_mass_spring_solver_create(cloth->mvert_num, nondiag);
86
87         for (i = 0; i < cloth->mvert_num; i++) {
88                 BPH_mass_spring_set_vertex_mass(id, i, verts[i].mass);
89         }
90
91         for (i = 0; i < cloth->mvert_num; i++) {
92                 BPH_mass_spring_set_motion_state(id, i, verts[i].x, ZERO);
93         }
94
95         return 1;
96 }
97
98 void BPH_cloth_solver_free(ClothModifierData *clmd)
99 {
100         Cloth *cloth = clmd->clothObject;
101
102         if (cloth->implicit) {
103                 BPH_mass_spring_solver_free(cloth->implicit);
104                 cloth->implicit = NULL;
105         }
106 }
107
108 void BKE_cloth_solver_set_positions(ClothModifierData *clmd)
109 {
110         Cloth *cloth = clmd->clothObject;
111         ClothVertex *verts = cloth->verts;
112         unsigned int mvert_num = cloth->mvert_num, i;
113         ClothHairData *cloth_hairdata = clmd->hairdata;
114         Implicit_Data *id = cloth->implicit;
115
116         for (i = 0; i < mvert_num; i++) {
117                 if (cloth_hairdata) {
118                         ClothHairData *root = &cloth_hairdata[i];
119                         BPH_mass_spring_set_rest_transform(id, i, root->rot);
120                 }
121                 else
122                         BPH_mass_spring_set_rest_transform(id, i, I3);
123
124                 BPH_mass_spring_set_motion_state(id, i, verts[i].x, verts[i].v);
125         }
126 }
127
128 static bool collision_response(ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, float dt, float restitution, float r_impulse[3])
129 {
130         Cloth *cloth = clmd->clothObject;
131         int index = collpair->ap1;
132         bool result = false;
133
134         float v1[3], v2_old[3], v2_new[3], v_rel_old[3], v_rel_new[3];
135         float epsilon2 = BLI_bvhtree_get_epsilon(collmd->bvhtree);
136
137         float margin_distance = (float)collpair->distance - epsilon2;
138         float mag_v_rel;
139
140         zero_v3(r_impulse);
141
142         if (margin_distance > 0.0f)
143                 return false; /* XXX tested before already? */
144
145         /* only handle static collisions here */
146         if (collpair->flag & COLLISION_IN_FUTURE)
147                 return false;
148
149         /* velocity */
150         copy_v3_v3(v1, cloth->verts[index].v);
151         collision_get_collider_velocity(v2_old, v2_new, collmd, collpair);
152         /* relative velocity = velocity of the cloth point relative to the collider */
153         sub_v3_v3v3(v_rel_old, v1, v2_old);
154         sub_v3_v3v3(v_rel_new, v1, v2_new);
155         /* normal component of the relative velocity */
156         mag_v_rel = dot_v3v3(v_rel_old, collpair->normal);
157
158         /* only valid when moving toward the collider */
159         if (mag_v_rel < -ALMOST_ZERO) {
160                 float v_nor_old, v_nor_new;
161                 float v_tan_old[3], v_tan_new[3];
162                 float bounce, repulse;
163
164                 /* Collision response based on
165                  * "Simulating Complex Hair with Robust Collision Handling" (Choe, Choi, Ko, ACM SIGGRAPH 2005)
166                  * http://graphics.snu.ac.kr/publications/2005-choe-HairSim/Choe_2005_SCA.pdf
167                  */
168
169                 v_nor_old = mag_v_rel;
170                 v_nor_new = dot_v3v3(v_rel_new, collpair->normal);
171
172                 madd_v3_v3v3fl(v_tan_old, v_rel_old, collpair->normal, -v_nor_old);
173                 madd_v3_v3v3fl(v_tan_new, v_rel_new, collpair->normal, -v_nor_new);
174
175                 bounce = -v_nor_old * restitution;
176
177                 repulse = -margin_distance / dt; /* base repulsion velocity in normal direction */
178                 /* XXX this clamping factor is quite arbitrary ...
179                  * not sure if there is a more scientific approach, but seems to give good results
180                  */
181                 CLAMP(repulse, 0.0f, 4.0f * bounce);
182
183                 if (margin_distance < -epsilon2) {
184                         mul_v3_v3fl(r_impulse, collpair->normal, max_ff(repulse, bounce) - v_nor_new);
185                 }
186                 else {
187                         bounce = 0.0f;
188                         mul_v3_v3fl(r_impulse, collpair->normal, repulse - v_nor_new);
189                 }
190
191                 result = true;
192         }
193
194         return result;
195 }
196
197 /* Init constraint matrix
198  * This is part of the modified CG method suggested by Baraff/Witkin in
199  * "Large Steps in Cloth Simulation" (Siggraph 1998)
200  */
201 static void cloth_setup_constraints(ClothModifierData *clmd, ColliderContacts *contacts, int totcolliders, float dt)
202 {
203         Cloth *cloth = clmd->clothObject;
204         Implicit_Data *data = cloth->implicit;
205         ClothVertex *verts = cloth->verts;
206         int mvert_num = cloth->mvert_num;
207         int i, j, v;
208
209         const float ZERO[3] = {0.0f, 0.0f, 0.0f};
210
211         BPH_mass_spring_clear_constraints(data);
212
213         for (v = 0; v < mvert_num; v++) {
214                 if (verts[v].flags & CLOTH_VERT_FLAG_PINNED) {
215                         /* pinned vertex constraints */
216                         BPH_mass_spring_add_constraint_ndof0(data, v, ZERO); /* velocity is defined externally */
217                 }
218
219                 verts[v].impulse_count = 0;
220         }
221
222         for (i = 0; i < totcolliders; ++i) {
223                 ColliderContacts *ct = &contacts[i];
224                 for (j = 0; j < ct->totcollisions; ++j) {
225                         CollPair *collpair = &ct->collisions[j];
226 //                      float restitution = (1.0f - clmd->coll_parms->damping) * (1.0f - ct->ob->pd->pdef_sbdamp);
227                         float restitution = 0.0f;
228                         int v = collpair->face1;
229                         float impulse[3];
230
231                         /* pinned verts handled separately */
232                         if (verts[v].flags & CLOTH_VERT_FLAG_PINNED)
233                                 continue;
234
235                         /* XXX cheap way of avoiding instability from multiple collisions in the same step
236                          * this should eventually be supported ...
237                          */
238                         if (verts[v].impulse_count > 0)
239                                 continue;
240
241                         /* calculate collision response */
242                         if (!collision_response(clmd, ct->collmd, collpair, dt, restitution, impulse))
243                                 continue;
244
245                         BPH_mass_spring_add_constraint_ndof2(data, v, collpair->normal, impulse);
246                         ++verts[v].impulse_count;
247                 }
248         }
249 }
250
251 /* computes where the cloth would be if it were subject to perfectly stiff edges
252  * (edge distance constraints) in a lagrangian solver.  then add forces to help
253  * guide the implicit solver to that state.  this function is called after
254  * collisions*/
255 static int UNUSED_FUNCTION(cloth_calc_helper_forces)(Object *UNUSED(ob), ClothModifierData *clmd, float (*initial_cos)[3], float UNUSED(step), float dt)
256 {
257         Cloth *cloth = clmd->clothObject;
258         float(*cos)[3] = (float(*)[3])MEM_callocN(sizeof(float[3]) * cloth->mvert_num, "cos cloth_calc_helper_forces");
259         float *masses = (float *)MEM_callocN(sizeof(float) * cloth->mvert_num, "cos cloth_calc_helper_forces");
260         LinkNode *node;
261         ClothSpring *spring;
262         ClothVertex *cv;
263         int i, steps;
264
265         cv = cloth->verts;
266         for (i = 0; i < cloth->mvert_num; i++, cv++) {
267                 copy_v3_v3(cos[i], cv->tx);
268
269                 if (cv->goal == 1.0f || len_squared_v3v3(initial_cos[i], cv->tx) != 0.0f) {
270                         masses[i] = 1e+10;
271                 }
272                 else {
273                         masses[i] = cv->mass;
274                 }
275         }
276
277         steps = 55;
278         for (i = 0; i < steps; i++) {
279                 for (node = cloth->springs; node; node = node->next) {
280                         /* ClothVertex *cv1, *cv2; */ /* UNUSED */
281                         int v1, v2;
282                         float len, c, l, vec[3];
283
284                         spring = (ClothSpring *)node->link;
285                         if (spring->type != CLOTH_SPRING_TYPE_STRUCTURAL && spring->type != CLOTH_SPRING_TYPE_SHEAR)
286                                 continue;
287
288                         v1 = spring->ij; v2 = spring->kl;
289                         /* cv1 = cloth->verts + v1; */ /* UNUSED */
290                         /* cv2 = cloth->verts + v2; */ /* UNUSED */
291                         len = len_v3v3(cos[v1], cos[v2]);
292
293                         sub_v3_v3v3(vec, cos[v1], cos[v2]);
294                         normalize_v3(vec);
295
296                         c = (len - spring->restlen);
297                         if (c == 0.0f)
298                                 continue;
299
300                         l = c / ((1.0f / masses[v1]) + (1.0f / masses[v2]));
301
302                         mul_v3_fl(vec, -(1.0f / masses[v1]) * l);
303                         add_v3_v3(cos[v1], vec);
304
305                         sub_v3_v3v3(vec, cos[v2], cos[v1]);
306                         normalize_v3(vec);
307
308                         mul_v3_fl(vec, -(1.0f / masses[v2]) * l);
309                         add_v3_v3(cos[v2], vec);
310                 }
311         }
312
313         cv = cloth->verts;
314         for (i = 0; i < cloth->mvert_num; i++, cv++) {
315                 float vec[3];
316
317                 /*compute forces*/
318                 sub_v3_v3v3(vec, cos[i], cv->tx);
319                 mul_v3_fl(vec, cv->mass * dt * 20.0f);
320                 add_v3_v3(cv->tv, vec);
321                 //copy_v3_v3(cv->tx, cos[i]);
322         }
323
324         MEM_freeN(cos);
325         MEM_freeN(masses);
326
327         return 1;
328 }
329
330 BLI_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s)
331 {
332         Cloth *cloth = clmd->clothObject;
333         ClothSimSettings *parms = clmd->sim_parms;
334         Implicit_Data *data = cloth->implicit;
335         bool using_angular = parms->bending_model == CLOTH_BENDING_ANGULAR;
336         bool resist_compress = (parms->flags & CLOTH_SIMSETTINGS_FLAG_RESIST_SPRING_COMPRESS) && !using_angular;
337
338         s->flags &= ~CLOTH_SPRING_FLAG_NEEDED;
339
340         /* Calculate force of bending springs. */
341         if ((s->type & CLOTH_SPRING_TYPE_BENDING) && using_angular) {
342 #ifdef CLOTH_FORCE_SPRING_BEND
343                 float k, scaling;
344
345                 s->flags |= CLOTH_SPRING_FLAG_NEEDED;
346
347                 scaling = parms->bending + s->ang_stiffness * fabsf(parms->max_bend - parms->bending);
348                 k = scaling * s->restlen * 0.1f; /* Multiplying by 0.1, just to scale the forces to more reasonable values. */
349
350                 BPH_mass_spring_force_spring_angular(data, s->ij, s->kl, s->pa, s->pb, s->la, s->lb,
351                                                      s->restang, k, parms->bending_damping);
352 #endif
353         }
354
355         /* Calculate force of structural + shear springs. */
356         if (s->type & (CLOTH_SPRING_TYPE_STRUCTURAL | CLOTH_SPRING_TYPE_SEWING)) {
357 #ifdef CLOTH_FORCE_SPRING_STRUCTURAL
358                 float k_tension, scaling_tension;
359
360                 s->flags |= CLOTH_SPRING_FLAG_NEEDED;
361
362                 scaling_tension = parms->tension + s->lin_stiffness * fabsf(parms->max_tension - parms->tension);
363                 k_tension = scaling_tension / (parms->avg_spring_len + FLT_EPSILON);
364
365                 if (s->type & CLOTH_SPRING_TYPE_SEWING) {
366                         // TODO: verify, half verified (couldn't see error)
367                         // sewing springs usually have a large distance at first so clamp the force so we don't get tunnelling through colission objects
368                         BPH_mass_spring_force_spring_linear(data, s->ij, s->kl, s->restlen,
369                                                             k_tension, parms->tension_damp,
370                                                             0.0f, 0.0f, false, false, parms->max_sewing);
371                 }
372                 else {
373                         float k_compression, scaling_compression;
374                         scaling_compression = parms->compression + s->lin_stiffness * fabsf(parms->max_compression - parms->compression);
375                         k_compression = scaling_compression / (parms->avg_spring_len + FLT_EPSILON);
376
377                         BPH_mass_spring_force_spring_linear(data, s->ij, s->kl, s->restlen,
378                                                             k_tension, parms->tension_damp,
379                                                             k_compression, parms->compression_damp,
380                                                             resist_compress, using_angular, 0.0f);
381                 }
382 #endif
383         }
384         else if (s->type & CLOTH_SPRING_TYPE_SHEAR) {
385 #ifdef CLOTH_FORCE_SPRING_SHEAR
386                 float k, scaling;
387
388                 s->flags |= CLOTH_SPRING_FLAG_NEEDED;
389
390                 scaling = parms->shear + s->lin_stiffness * fabsf(parms->max_shear - parms->shear);
391                 k = scaling / (parms->avg_spring_len + FLT_EPSILON);
392
393                 BPH_mass_spring_force_spring_linear(data, s->ij, s->kl, s->restlen, k, parms->shear_damp,
394                                                     0.0f, 0.0f, resist_compress, false, 0.0f);
395 #endif
396         }
397         else if (s->type & CLOTH_SPRING_TYPE_BENDING) {  /* calculate force of bending springs */
398 #ifdef CLOTH_FORCE_SPRING_BEND
399                 float kb, cb, scaling;
400
401                 s->flags |= CLOTH_SPRING_FLAG_NEEDED;
402
403                 scaling = parms->bending + s->lin_stiffness * fabsf(parms->max_bend - parms->bending);
404                 kb = scaling / (20.0f * (parms->avg_spring_len + FLT_EPSILON));
405
406                 // Fix for [#45084] for cloth stiffness must have cb proportional to kb
407                 cb = kb * parms->bending_damping;
408
409                 BPH_mass_spring_force_spring_bending(data, s->ij, s->kl, s->restlen, kb, cb);
410 #endif
411         }
412         else if (s->type & CLOTH_SPRING_TYPE_BENDING_HAIR) {
413 #ifdef CLOTH_FORCE_SPRING_BEND
414                 float kb, cb, scaling;
415
416                 s->flags |= CLOTH_SPRING_FLAG_NEEDED;
417
418                 /* XXX WARNING: angular bending springs for hair apply stiffness factor as an overall factor, unlike cloth springs!
419                  * this is crap, but needed due to cloth/hair mixing ...
420                  * max_bend factor is not even used for hair, so ...
421                  */
422                 scaling = s->lin_stiffness * parms->bending;
423                 kb = scaling / (20.0f * (parms->avg_spring_len + FLT_EPSILON));
424
425                 // Fix for [#45084] for cloth stiffness must have cb proportional to kb
426                 cb = kb * parms->bending_damping;
427
428                 /* XXX assuming same restlen for ij and jk segments here, this can be done correctly for hair later */
429                 BPH_mass_spring_force_spring_bending_hair(data, s->ij, s->kl, s->mn, s->target, kb, cb);
430
431 #if 0
432                 {
433                         float x_kl[3], x_mn[3], v[3], d[3];
434
435                         BPH_mass_spring_get_motion_state(data, s->kl, x_kl, v);
436                         BPH_mass_spring_get_motion_state(data, s->mn, x_mn, v);
437
438                         BKE_sim_debug_data_add_dot(clmd->debug_data, x_kl, 0.9, 0.9, 0.9, "target", 7980, s->kl);
439                         BKE_sim_debug_data_add_line(clmd->debug_data, x_kl, x_mn, 0.8, 0.8, 0.8, "target", 7981, s->kl);
440
441                         copy_v3_v3(d, s->target);
442                         BKE_sim_debug_data_add_vector(clmd->debug_data, x_kl, d, 0.8, 0.8, 0.2, "target", 7982, s->kl);
443
444 //                      copy_v3_v3(d, s->target_ij);
445 //                      BKE_sim_debug_data_add_vector(clmd->debug_data, x, d, 1, 0.4, 0.4, "target", 7983, s->kl);
446                 }
447 #endif
448 #endif
449         }
450 }
451
452 static void hair_get_boundbox(ClothModifierData *clmd, float gmin[3], float gmax[3])
453 {
454         Cloth *cloth = clmd->clothObject;
455         Implicit_Data *data = cloth->implicit;
456         unsigned int mvert_num = cloth->mvert_num;
457         int i;
458
459         INIT_MINMAX(gmin, gmax);
460         for (i = 0; i < mvert_num; i++) {
461                 float x[3];
462                 BPH_mass_spring_get_motion_state(data, i, x, NULL);
463                 DO_MINMAX(x, gmin, gmax);
464         }
465 }
466
467 static void cloth_calc_force(Scene *scene, ClothModifierData *clmd, float UNUSED(frame), ListBase *effectors, float time)
468 {
469         /* Collect forces and derivatives:  F, dFdX, dFdV */
470         Cloth *cloth = clmd->clothObject;
471         Implicit_Data *data = cloth->implicit;
472         unsigned int i  = 0;
473         float drag    = clmd->sim_parms->Cvi * 0.01f;       /* viscosity of air scaled in percent */
474         float gravity[3] = {0.0f, 0.0f, 0.0f};
475         const MVertTri *tri     = cloth->tri;
476         unsigned int mvert_num = cloth->mvert_num;
477         ClothVertex *vert;
478
479 #ifdef CLOTH_FORCE_GRAVITY
480         /* global acceleration (gravitation) */
481         if (scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) {
482                 /* scale gravity force */
483                 mul_v3_v3fl(gravity, scene->physics_settings.gravity, 0.001f * clmd->sim_parms->effector_weights->global_gravity);
484         }
485
486         vert = cloth->verts;
487         for (i = 0; i < cloth->mvert_num; i++, vert++) {
488                 BPH_mass_spring_force_gravity(data, i, vert->mass, gravity);
489
490                 /* Vertex goal springs */
491                 if ((!(vert->flags & CLOTH_VERT_FLAG_PINNED)) && (vert->goal > FLT_EPSILON)) {
492                         float goal_x[3], goal_v[3];
493                         float k;
494
495                         /* divide by time_scale to prevent goal vertices' delta locations from being multiplied */
496                         interp_v3_v3v3(goal_x, vert->xold, vert->xconst, time / clmd->sim_parms->time_scale);
497                         sub_v3_v3v3(goal_v, vert->xconst, vert->xold); /* distance covered over dt==1 */
498
499                         k = vert->goal * clmd->sim_parms->goalspring / (clmd->sim_parms->avg_spring_len + FLT_EPSILON);
500
501                         BPH_mass_spring_force_spring_goal(data, i, goal_x, goal_v, k, clmd->sim_parms->goalfrict * 0.01f);
502                 }
503         }
504 #endif
505
506         /* cloth_calc_volume_force(clmd); */
507
508 #ifdef CLOTH_FORCE_DRAG
509         BPH_mass_spring_force_drag(data, drag);
510 #endif
511
512         /* handle external forces like wind */
513         if (effectors) {
514                 /* cache per-vertex forces to avoid redundant calculation */
515                 float(*winvec)[3] = (float(*)[3])MEM_callocN(sizeof(float[3]) * mvert_num, "effector forces");
516                 for (i = 0; i < cloth->mvert_num; i++) {
517                         float x[3], v[3];
518                         EffectedPoint epoint;
519
520                         BPH_mass_spring_get_motion_state(data, i, x, v);
521                         pd_point_from_loc(scene, x, v, i, &epoint);
522                         BKE_effectors_apply(effectors, NULL, clmd->sim_parms->effector_weights, &epoint, winvec[i], NULL);
523                 }
524
525                 for (i = 0; i < cloth->tri_num; i++) {
526                         const MVertTri *vt = &tri[i];
527                         BPH_mass_spring_force_face_wind(data, vt->tri[0], vt->tri[1], vt->tri[2], winvec);
528                 }
529
530                 /* Hair has only edges */
531                 if (cloth->tri_num == 0) {
532 #if 0
533                         ClothHairData *hairdata = clmd->hairdata;
534                         ClothHairData *hair_ij, *hair_kl;
535
536                         for (LinkNode *link = cloth->springs; link; link = link->next) {
537                                 ClothSpring *spring = (ClothSpring *)link->link;
538                                 if (spring->type == CLOTH_SPRING_TYPE_STRUCTURAL) {
539                                         if (hairdata) {
540                                                 hair_ij = &hairdata[spring->ij];
541                                                 hair_kl = &hairdata[spring->kl];
542                                                 BPH_mass_spring_force_edge_wind(data, spring->ij, spring->kl, hair_ij->radius, hair_kl->radius, winvec);
543                                         }
544                                         else
545                                                 BPH_mass_spring_force_edge_wind(data, spring->ij, spring->kl, 1.0f, 1.0f, winvec);
546                                 }
547                         }
548 #else
549                         ClothHairData *hairdata = clmd->hairdata;
550
551                         vert = cloth->verts;
552                         for (i = 0; i < cloth->mvert_num; i++, vert++) {
553                                 if (hairdata) {
554                                         ClothHairData *hair = &hairdata[i];
555                                         BPH_mass_spring_force_vertex_wind(data, i, hair->radius, winvec);
556                                 }
557                                 else
558                                         BPH_mass_spring_force_vertex_wind(data, i, 1.0f, winvec);
559                         }
560 #endif
561                 }
562
563                 MEM_freeN(winvec);
564         }
565
566         // calculate spring forces
567         for (LinkNode *link = cloth->springs; link; link = link->next) {
568                 ClothSpring *spring = (ClothSpring *)link->link;
569                 // only handle active springs
570                 if (!(spring->flags & CLOTH_SPRING_FLAG_DEACTIVATE)) {
571                         cloth_calc_spring_force(clmd, spring);
572                 }
573         }
574 }
575
576 /* returns vertexes' motion state */
577 BLI_INLINE void cloth_get_grid_location(Implicit_Data *data, float cell_scale, const float cell_offset[3],
578                                         int index, float x[3], float v[3])
579 {
580         BPH_mass_spring_get_position(data, index, x);
581         BPH_mass_spring_get_new_velocity(data, index, v);
582
583         mul_v3_fl(x, cell_scale);
584         add_v3_v3(x, cell_offset);
585 }
586
587 /* returns next spring forming a continuous hair sequence */
588 BLI_INLINE LinkNode *hair_spring_next(LinkNode *spring_link)
589 {
590         ClothSpring *spring = (ClothSpring *)spring_link->link;
591         LinkNode *next = spring_link->next;
592         if (next) {
593                 ClothSpring *next_spring = (ClothSpring *)next->link;
594                 if (next_spring->type == CLOTH_SPRING_TYPE_STRUCTURAL && next_spring->kl == spring->ij)
595                         return next;
596         }
597         return NULL;
598 }
599
600 /* XXX this is nasty: cloth meshes do not explicitly store
601  * the order of hair segments!
602  * We have to rely on the spring build function for now,
603  * which adds structural springs in reverse order:
604  *   (3,4), (2,3), (1,2)
605  * This is currently the only way to figure out hair geometry inside this code ...
606  */
607 static LinkNode *cloth_continuum_add_hair_segments(HairGrid *grid, const float cell_scale, const float cell_offset[3], Cloth *cloth, LinkNode *spring_link)
608 {
609         Implicit_Data *data = cloth->implicit;
610         LinkNode *next_spring_link = NULL;     /* return value */
611         ClothSpring *spring1, *spring2, *spring3;
612         // ClothVertex *verts = cloth->verts;
613         // ClothVertex *vert3, *vert4;
614         float x1[3], v1[3], x2[3], v2[3], x3[3], v3[3], x4[3], v4[3];
615         float dir1[3], dir2[3], dir3[3];
616
617         spring1 = NULL;
618         spring2 = NULL;
619         spring3 = (ClothSpring *)spring_link->link;
620
621         zero_v3(x1); zero_v3(v1);
622         zero_v3(dir1);
623         zero_v3(x2); zero_v3(v2);
624         zero_v3(dir2);
625
626         // vert3 = &verts[spring3->kl];
627         cloth_get_grid_location(data, cell_scale, cell_offset, spring3->kl, x3, v3);
628         // vert4 = &verts[spring3->ij];
629         cloth_get_grid_location(data, cell_scale, cell_offset, spring3->ij, x4, v4);
630         sub_v3_v3v3(dir3, x4, x3);
631         normalize_v3(dir3);
632
633         while (spring_link) {
634                 /* move on */
635                 spring1 = spring2;
636                 spring2 = spring3;
637
638                 // vert3 = vert4;
639
640                 copy_v3_v3(x1, x2); copy_v3_v3(v1, v2);
641                 copy_v3_v3(x2, x3); copy_v3_v3(v2, v3);
642                 copy_v3_v3(x3, x4); copy_v3_v3(v3, v4);
643
644                 copy_v3_v3(dir1, dir2);
645                 copy_v3_v3(dir2, dir3);
646
647                 /* read next segment */
648                 next_spring_link = spring_link->next;
649                 spring_link = hair_spring_next(spring_link);
650
651                 if (spring_link) {
652                         spring3 = (ClothSpring *)spring_link->link;
653                         // vert4 = &verts[spring3->ij];
654                         cloth_get_grid_location(data, cell_scale, cell_offset, spring3->ij, x4, v4);
655                         sub_v3_v3v3(dir3, x4, x3);
656                         normalize_v3(dir3);
657                 }
658                 else {
659                         spring3 = NULL;
660                         // vert4 = NULL;
661                         zero_v3(x4); zero_v3(v4);
662                         zero_v3(dir3);
663                 }
664
665                 BPH_hair_volume_add_segment(grid, x1, v1, x2, v2, x3, v3, x4, v4,
666                                             spring1 ? dir1 : NULL,
667                                             dir2,
668                                             spring3 ? dir3 : NULL);
669         }
670
671         return next_spring_link;
672 }
673
674 static void cloth_continuum_fill_grid(HairGrid *grid, Cloth *cloth)
675 {
676 #if 0
677         Implicit_Data *data = cloth->implicit;
678         int mvert_num = cloth->mvert_num;
679         ClothVertex *vert;
680         int i;
681
682         for (i = 0, vert = cloth->verts; i < mvert_num; i++, vert++) {
683                 float x[3], v[3];
684
685                 cloth_get_vertex_motion_state(data, vert, x, v);
686                 BPH_hair_volume_add_vertex(grid, x, v);
687         }
688 #else
689         LinkNode *link;
690         float cellsize, gmin[3], cell_scale, cell_offset[3];
691
692         /* scale and offset for transforming vertex locations into grid space
693          * (cell size is 0..1, gmin becomes origin)
694          */
695         BPH_hair_volume_grid_geometry(grid, &cellsize, NULL, gmin, NULL);
696         cell_scale = cellsize > 0.0f ? 1.0f / cellsize : 0.0f;
697         mul_v3_v3fl(cell_offset, gmin, cell_scale);
698         negate_v3(cell_offset);
699
700         link = cloth->springs;
701         while (link) {
702                 ClothSpring *spring = (ClothSpring *)link->link;
703                 if (spring->type == CLOTH_SPRING_TYPE_STRUCTURAL)
704                         link = cloth_continuum_add_hair_segments(grid, cell_scale, cell_offset, cloth, link);
705                 else
706                         link = link->next;
707         }
708 #endif
709         BPH_hair_volume_normalize_vertex_grid(grid);
710 }
711
712 static void cloth_continuum_step(ClothModifierData *clmd, float dt)
713 {
714         ClothSimSettings *parms = clmd->sim_parms;
715         Cloth *cloth = clmd->clothObject;
716         Implicit_Data *data = cloth->implicit;
717         int mvert_num = cloth->mvert_num;
718         ClothVertex *vert;
719
720         const float fluid_factor = 0.95f;     /* blend between PIC and FLIP methods */
721         float smoothfac = parms->velocity_smooth;
722         /* XXX FIXME arbitrary factor!!! this should be based on some intuitive value instead,
723          * like number of hairs per cell and time decay instead of "strength"
724          */
725         float density_target = parms->density_target;
726         float density_strength = parms->density_strength;
727         float gmin[3], gmax[3];
728         int i;
729
730         /* clear grid info */
731         zero_v3_int(clmd->hair_grid_res);
732         zero_v3(clmd->hair_grid_min);
733         zero_v3(clmd->hair_grid_max);
734         clmd->hair_grid_cellsize = 0.0f;
735
736         hair_get_boundbox(clmd, gmin, gmax);
737
738         /* gather velocities & density */
739         if (smoothfac > 0.0f || density_strength > 0.0f) {
740                 HairGrid *grid = BPH_hair_volume_create_vertex_grid(clmd->sim_parms->voxel_cell_size, gmin, gmax);
741
742                 cloth_continuum_fill_grid(grid, cloth);
743
744                 /* main hair continuum solver */
745                 BPH_hair_volume_solve_divergence(grid, dt, density_target, density_strength);
746
747                 for (i = 0, vert = cloth->verts; i < mvert_num; i++, vert++) {
748                         float x[3], v[3], nv[3];
749
750                         /* calculate volumetric velocity influence */
751                         BPH_mass_spring_get_position(data, i, x);
752                         BPH_mass_spring_get_new_velocity(data, i, v);
753
754                         BPH_hair_volume_grid_velocity(grid, x, v, fluid_factor, nv);
755
756                         interp_v3_v3v3(nv, v, nv, smoothfac);
757
758                         /* apply on hair data */
759                         BPH_mass_spring_set_new_velocity(data, i, nv);
760                 }
761
762                 /* store basic grid info in the modifier data */
763                 BPH_hair_volume_grid_geometry(grid, &clmd->hair_grid_cellsize, clmd->hair_grid_res, clmd->hair_grid_min, clmd->hair_grid_max);
764
765 #if 0 /* DEBUG hair velocity vector field */
766                 {
767                         const int size = 64;
768                         int i, j;
769                         float offset[3], a[3], b[3];
770                         const int axis = 0;
771                         const float shift = 0.0f;
772
773                         copy_v3_v3(offset, clmd->hair_grid_min);
774                         zero_v3(a);
775                         zero_v3(b);
776
777                         offset[axis] = shift * clmd->hair_grid_cellsize;
778                         a[(axis + 1) % 3] = clmd->hair_grid_max[(axis + 1) % 3] - clmd->hair_grid_min[(axis + 1) % 3];
779                         b[(axis + 2) % 3] = clmd->hair_grid_max[(axis + 2) % 3] - clmd->hair_grid_min[(axis + 2) % 3];
780
781                         BKE_sim_debug_data_clear_category(clmd->debug_data, "grid velocity");
782                         for (j = 0; j < size; ++j) {
783                                 for (i = 0; i < size; ++i) {
784                                         float x[3], v[3], gvel[3], gvel_smooth[3], gdensity;
785
786                                         madd_v3_v3v3fl(x, offset, a, (float)i / (float)(size - 1));
787                                         madd_v3_v3fl(x, b, (float)j / (float)(size - 1));
788                                         zero_v3(v);
789
790                                         BPH_hair_volume_grid_interpolate(grid, x, &gdensity, gvel, gvel_smooth, NULL, NULL);
791
792 //                                      BKE_sim_debug_data_add_circle(clmd->debug_data, x, gdensity, 0.7, 0.3, 1, "grid density", i, j, 3111);
793                                         if (!is_zero_v3(gvel) || !is_zero_v3(gvel_smooth)) {
794                                                 float dvel[3];
795                                                 sub_v3_v3v3(dvel, gvel_smooth, gvel);
796 //                                              BKE_sim_debug_data_add_vector(clmd->debug_data, x, gvel, 0.4, 0, 1, "grid velocity", i, j, 3112);
797 //                                              BKE_sim_debug_data_add_vector(clmd->debug_data, x, gvel_smooth, 0.6, 1, 1, "grid velocity", i, j, 3113);
798                                                 BKE_sim_debug_data_add_vector(clmd->debug_data, x, dvel, 0.4, 1, 0.7, "grid velocity", i, j, 3114);
799 #if 0
800                                                 if (gdensity > 0.0f) {
801                                                         float col0[3] = {0.0, 0.0, 0.0};
802                                                         float col1[3] = {0.0, 1.0, 0.0};
803                                                         float col[3];
804
805                                                         interp_v3_v3v3(col, col0, col1, CLAMPIS(gdensity * clmd->sim_parms->density_strength, 0.0, 1.0));
806 //                                                      BKE_sim_debug_data_add_circle(clmd->debug_data, x, gdensity * clmd->sim_parms->density_strength, 0, 1, 0.4, "grid velocity", i, j, 3115);
807 //                                                      BKE_sim_debug_data_add_dot(clmd->debug_data, x, col[0], col[1], col[2], "grid velocity", i, j, 3115);
808                                                         BKE_sim_debug_data_add_circle(clmd->debug_data, x, 0.01f, col[0], col[1], col[2], "grid velocity", i, j, 3115);
809                                                 }
810 #endif
811                                         }
812                                 }
813                         }
814                 }
815 #endif
816
817                 BPH_hair_volume_free_vertex_grid(grid);
818         }
819 }
820
821 #if 0
822 static void cloth_calc_volume_force(ClothModifierData *clmd)
823 {
824         ClothSimSettings *parms = clmd->sim_parms;
825         Cloth *cloth = clmd->clothObject;
826         Implicit_Data *data = cloth->implicit;
827         int mvert_num = cloth->mvert_num;
828         ClothVertex *vert;
829
830         /* 2.0f is an experimental value that seems to give good results */
831         float smoothfac = 2.0f * parms->velocity_smooth;
832         float collfac = 2.0f * parms->collider_friction;
833         float pressfac = parms->pressure;
834         float minpress = parms->pressure_threshold;
835         float gmin[3], gmax[3];
836         int i;
837
838         hair_get_boundbox(clmd, gmin, gmax);
839
840         /* gather velocities & density */
841         if (smoothfac > 0.0f || pressfac > 0.0f) {
842                 HairVertexGrid *vertex_grid = BPH_hair_volume_create_vertex_grid(clmd->sim_parms->voxel_res, gmin, gmax);
843
844                 vert = cloth->verts;
845                 for (i = 0; i < mvert_num; i++, vert++) {
846                         float x[3], v[3];
847
848                         if (vert->solver_index < 0) {
849                                 copy_v3_v3(x, vert->x);
850                                 copy_v3_v3(v, vert->v);
851                         }
852                         else {
853                                 BPH_mass_spring_get_motion_state(data, vert->solver_index, x, v);
854                         }
855                         BPH_hair_volume_add_vertex(vertex_grid, x, v);
856                 }
857                 BPH_hair_volume_normalize_vertex_grid(vertex_grid);
858
859                 vert = cloth->verts;
860                 for (i = 0; i < mvert_num; i++, vert++) {
861                         float x[3], v[3], f[3], dfdx[3][3], dfdv[3][3];
862
863                         if (vert->solver_index < 0)
864                                 continue;
865
866                         /* calculate volumetric forces */
867                         BPH_mass_spring_get_motion_state(data, vert->solver_index, x, v);
868                         BPH_hair_volume_vertex_grid_forces(vertex_grid, x, v, smoothfac, pressfac, minpress, f, dfdx, dfdv);
869                         /* apply on hair data */
870                         BPH_mass_spring_force_extern(data, vert->solver_index, f, dfdx, dfdv);
871                 }
872
873                 BPH_hair_volume_free_vertex_grid(vertex_grid);
874         }
875 }
876 #endif
877
878 static void cloth_solve_collisions(Depsgraph *depsgraph, Object *ob, ClothModifierData *clmd, float step, float dt)
879 {
880         Cloth *cloth = clmd->clothObject;
881         Implicit_Data *id = cloth->implicit;
882         ClothVertex *verts = cloth->verts;
883         int mvert_num = cloth->mvert_num;
884         const float time_multiplier = 1.0f / (clmd->sim_parms->dt * clmd->sim_parms->timescale);
885         int i;
886
887         if (!(clmd->coll_parms->flags & (CLOTH_COLLSETTINGS_FLAG_ENABLED | CLOTH_COLLSETTINGS_FLAG_SELF)))
888                 return;
889
890         if (!clmd->clothObject->bvhtree)
891                 return;
892
893         BPH_mass_spring_solve_positions(id, dt);
894
895         /* Update verts to current positions. */
896         for (i = 0; i < mvert_num; i++) {
897                 BPH_mass_spring_get_new_position(id, i, verts[i].tx);
898
899                 sub_v3_v3v3(verts[i].tv, verts[i].tx, verts[i].txold);
900                 zero_v3(verts[i].dcvel);
901         }
902
903         if (cloth_bvh_collision(depsgraph, ob, clmd, step / clmd->sim_parms->timescale, dt / clmd->sim_parms->timescale)) {
904                 for (i = 0; i < mvert_num; i++) {
905                         if ((clmd->sim_parms->vgroup_mass > 0) && (verts[i].flags & CLOTH_VERT_FLAG_PINNED))
906                                 continue;
907
908                         BPH_mass_spring_get_new_velocity(id, i, verts[i].tv);
909                         madd_v3_v3fl(verts[i].tv, verts[i].dcvel, time_multiplier);
910                         BPH_mass_spring_set_new_velocity(id, i, verts[i].tv);
911                 }
912         }
913 }
914
915 static void cloth_clear_result(ClothModifierData *clmd)
916 {
917         ClothSolverResult *sres = clmd->solver_result;
918
919         sres->status = 0;
920         sres->max_error = sres->min_error = sres->avg_error = 0.0f;
921         sres->max_iterations = sres->min_iterations = 0;
922         sres->avg_iterations = 0.0f;
923 }
924
925 static void cloth_record_result(ClothModifierData *clmd, ImplicitSolverResult *result, float dt)
926 {
927         ClothSolverResult *sres = clmd->solver_result;
928
929         if (sres->status) {     /* already initialized ? */
930                 /* error only makes sense for successful iterations */
931                 if (result->status == BPH_SOLVER_SUCCESS) {
932                         sres->min_error = min_ff(sres->min_error, result->error);
933                         sres->max_error = max_ff(sres->max_error, result->error);
934                         sres->avg_error += result->error * dt;
935                 }
936
937                 sres->min_iterations = min_ii(sres->min_iterations, result->iterations);
938                 sres->max_iterations = max_ii(sres->max_iterations, result->iterations);
939                 sres->avg_iterations += (float)result->iterations * dt;
940         }
941         else {
942                 /* error only makes sense for successful iterations */
943                 if (result->status == BPH_SOLVER_SUCCESS) {
944                         sres->min_error = sres->max_error = result->error;
945                         sres->avg_error += result->error * dt;
946                 }
947
948                 sres->min_iterations = sres->max_iterations  = result->iterations;
949                 sres->avg_iterations += (float)result->iterations * dt;
950         }
951
952         sres->status |= result->status;
953 }
954
955 int BPH_cloth_solve(Depsgraph *depsgraph, Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors)
956 {
957         /* Hair currently is a cloth sim in disguise ...
958          * Collision detection and volumetrics work differently then.
959          * Bad design, TODO
960          */
961         Scene *scene = DEG_get_evaluated_scene(depsgraph);
962         const bool is_hair = (clmd->hairdata != NULL);
963
964         unsigned int i = 0;
965         float step = 0.0f, tf = clmd->sim_parms->timescale;
966         Cloth *cloth = clmd->clothObject;
967         ClothVertex *verts = cloth->verts /*, *cv*/;
968         unsigned int mvert_num = cloth->mvert_num;
969         float dt = clmd->sim_parms->dt * clmd->sim_parms->timescale;
970         Implicit_Data *id = cloth->implicit;
971         ColliderContacts *contacts = NULL;
972         int totcolliders = 0;
973
974         BKE_sim_debug_data_clear_category("collision");
975
976         if (!clmd->solver_result)
977                 clmd->solver_result = (ClothSolverResult *)MEM_callocN(sizeof(ClothSolverResult), "cloth solver result");
978         cloth_clear_result(clmd);
979
980         if (clmd->sim_parms->vgroup_mass > 0) { /* Do goal stuff. */
981                 for (i = 0; i < mvert_num; i++) {
982                         // update velocities with constrained velocities from pinned verts
983                         if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) {
984                                 float v[3];
985                                 sub_v3_v3v3(v, verts[i].xconst, verts[i].xold);
986                                 // mul_v3_fl(v, clmd->sim_parms->stepsPerFrame);
987                                 /* divide by time_scale to prevent constrained velocities from being multiplied */
988                                 mul_v3_fl(v, 1.0f / clmd->sim_parms->time_scale);
989                                 BPH_mass_spring_set_velocity(id, i, v);
990                         }
991                 }
992         }
993
994         while (step < tf) {
995                 ImplicitSolverResult result;
996
997                 if (is_hair) {
998                         /* determine contact points */
999                         if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) {
1000                                 cloth_find_point_contacts(depsgraph, ob, clmd, 0.0f, tf, &contacts, &totcolliders);
1001                         }
1002
1003                         /* setup vertex constraints for pinned vertices and contacts */
1004                         cloth_setup_constraints(clmd, contacts, totcolliders, dt);
1005                 }
1006                 else {
1007                         /* setup vertex constraints for pinned vertices */
1008                         cloth_setup_constraints(clmd, NULL, 0, dt);
1009                 }
1010
1011                 /* initialize forces to zero */
1012                 BPH_mass_spring_clear_forces(id);
1013
1014                 // calculate forces
1015                 cloth_calc_force(scene, clmd, frame, effectors, step);
1016
1017                 // calculate new velocity and position
1018                 BPH_mass_spring_solve_velocities(id, dt, &result);
1019                 cloth_record_result(clmd, &result, dt);
1020
1021                 /* Calculate collision impulses. */
1022                 if (!is_hair) {
1023                         cloth_solve_collisions(depsgraph, ob, clmd, step, dt);
1024                 }
1025
1026                 if (is_hair) {
1027                         cloth_continuum_step(clmd, dt);
1028                 }
1029
1030                 BPH_mass_spring_solve_positions(id, dt);
1031                 BPH_mass_spring_apply_result(id);
1032
1033                 /* move pinned verts to correct position */
1034                 for (i = 0; i < mvert_num; i++) {
1035                         if (clmd->sim_parms->vgroup_mass > 0) {
1036                                 if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) {
1037                                         float x[3];
1038                                         /* divide by time_scale to prevent pinned vertices' delta locations from being multiplied */
1039                                         interp_v3_v3v3(x, verts[i].xold, verts[i].xconst, (step + dt) / clmd->sim_parms->time_scale);
1040                                         BPH_mass_spring_set_position(id, i, x);
1041                                 }
1042                         }
1043
1044                         BPH_mass_spring_get_motion_state(id, i, verts[i].txold, NULL);
1045                 }
1046
1047                 /* free contact points */
1048                 if (contacts) {
1049                         cloth_free_contacts(contacts, totcolliders);
1050                 }
1051
1052                 step += dt;
1053         }
1054
1055         /* copy results back to cloth data */
1056         for (i = 0; i < mvert_num; i++) {
1057                 BPH_mass_spring_get_motion_state(id, i, verts[i].x, verts[i].v);
1058                 copy_v3_v3(verts[i].txold, verts[i].x);
1059         }
1060
1061         return 1;
1062 }