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