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