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