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