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