Fix for warnings/errors
[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/blenkernel/intern/BPH_mass_spring.c
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.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 /* Number of off-diagonal non-zero matrix blocks.
55  * Basically there is one of these for each vertex-vertex interaction.
56  */
57 static int cloth_count_nondiag_blocks(Cloth *cloth)
58 {
59         LinkNode *link;
60         int nondiag = 0;
61         
62         for (link = cloth->springs; link; link = link->next) {
63                 ClothSpring *spring = (ClothSpring *)link->link;
64                 switch (spring->type) {
65                         case CLOTH_SPRING_TYPE_BENDING_ANG:
66                                 /* angular bending combines 3 vertices */
67                                 nondiag += 3;
68                                 break;
69                                 
70                         default:
71                                 /* all other springs depend on 2 vertices only */
72                                 nondiag += 1;
73                                 break;
74                 }
75         }
76         
77         return nondiag;
78 }
79
80 int BPH_cloth_solver_init(Object *UNUSED(ob), ClothModifierData *clmd)
81 {
82         Cloth *cloth = clmd->clothObject;
83         ClothVertex *verts = cloth->verts;
84         const float ZERO[3] = {0.0f, 0.0f, 0.0f};
85         Implicit_Data *id;
86         unsigned int i, nondiag;
87         
88         nondiag = cloth_count_nondiag_blocks(cloth);
89         cloth->implicit = id = BPH_mass_spring_solver_create(cloth->numverts, nondiag);
90         
91         for (i = 0; i < cloth->numverts; i++) {
92                 BPH_mass_spring_set_vertex_mass(id, i, verts[i].mass);
93         }
94         
95         for (i = 0; i < cloth->numverts; i++) {
96                 BPH_mass_spring_set_motion_state(id, i, verts[i].x, ZERO);
97         }
98         
99         return 1;
100 }
101
102 void BPH_cloth_solver_free(ClothModifierData *clmd)
103 {
104         Cloth *cloth = clmd->clothObject;
105         
106         if (cloth->implicit) {
107                 BPH_mass_spring_solver_free(cloth->implicit);
108                 cloth->implicit = NULL;
109         }
110 }
111
112 void BKE_cloth_solver_set_positions(ClothModifierData *clmd)
113 {
114         Cloth *cloth = clmd->clothObject;
115         ClothVertex *verts = cloth->verts;
116         unsigned int numverts = cloth->numverts, i;
117         ClothHairRoot *cloth_roots = clmd->roots;
118         Implicit_Data *id = cloth->implicit;
119         const float ZERO[3] = {0.0f, 0.0f, 0.0f};
120         
121         for (i = 0; i < numverts; i++) {
122                 ClothHairRoot *root = &cloth_roots[i];
123                 
124                 BPH_mass_spring_set_rest_transform(id, i, root->rot);
125                 BPH_mass_spring_set_motion_state(id, i, verts[i].x, verts[i].v);
126         }
127 }
128
129 static bool collision_response(ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, float dt, float restitution, float r_impulse[3])
130 {
131         Cloth *cloth = clmd->clothObject;
132         int index = collpair->ap1;
133         bool result = false;
134         
135         float v1[3], v2_old[3], v2_new[3], v_rel_old[3], v_rel_new[3];
136         float epsilon2 = BLI_bvhtree_getepsilon(collmd->bvhtree);
137
138         float margin_distance = collpair->distance - epsilon2;
139         float mag_v_rel;
140         
141         zero_v3(r_impulse);
142         
143         if (margin_distance > 0.0f)
144                 return false; /* XXX tested before already? */
145         
146         /* only handle static collisions here */
147         if ( collpair->flag & COLLISION_IN_FUTURE )
148                 return false;
149         
150         /* velocity */
151         copy_v3_v3(v1, cloth->verts[index].v);
152         collision_get_collider_velocity(v2_old, v2_new, collmd, collpair);
153         /* relative velocity = velocity of the cloth point relative to the collider */
154         sub_v3_v3v3(v_rel_old, v1, v2_old);
155         sub_v3_v3v3(v_rel_new, v1, v2_new);
156         /* normal component of the relative velocity */
157         mag_v_rel = dot_v3v3(v_rel_old, collpair->normal);
158         
159         /* only valid when moving toward the collider */
160         if (mag_v_rel < -ALMOST_ZERO) {
161                 float v_nor_old, v_nor_new;
162                 float v_tan_old[3], v_tan_new[3];
163                 float bounce, repulse;
164                 
165                 /* Collision response based on
166                  * "Simulating Complex Hair with Robust Collision Handling" (Choe, Choi, Ko, ACM SIGGRAPH 2005)
167                  * http://graphics.snu.ac.kr/publications/2005-choe-HairSim/Choe_2005_SCA.pdf
168                  */
169                 
170                 v_nor_old = mag_v_rel;
171                 v_nor_new = dot_v3v3(v_rel_new, collpair->normal);
172                 
173                 madd_v3_v3v3fl(v_tan_old, v_rel_old, collpair->normal, -v_nor_old);
174                 madd_v3_v3v3fl(v_tan_new, v_rel_new, collpair->normal, -v_nor_new);
175                 
176                 bounce = -v_nor_old * restitution;
177                 
178                 repulse = -margin_distance / dt; /* base repulsion velocity in normal direction */
179                 /* XXX this clamping factor is quite arbitrary ...
180                  * not sure if there is a more scientific approach, but seems to give good results
181                  */
182                 CLAMP(repulse, 0.0f, 4.0f * bounce);
183                 
184                 if (margin_distance < -epsilon2) {
185                         mul_v3_v3fl(r_impulse, collpair->normal, max_ff(repulse, bounce) - v_nor_new);
186                 }
187                 else {
188                         bounce = 0.0f;
189                         mul_v3_v3fl(r_impulse, collpair->normal, repulse - v_nor_new);
190                 }
191                 
192                 result = true;
193         }
194         
195         return result;
196 }
197
198 /* Init constraint matrix
199  * This is part of the modified CG method suggested by Baraff/Witkin in
200  * "Large Steps in Cloth Simulation" (Siggraph 1998)
201  */
202 static void cloth_setup_constraints(ClothModifierData *clmd, ColliderContacts *contacts, int totcolliders, float dt)
203 {
204         Cloth *cloth = clmd->clothObject;
205         Implicit_Data *data = cloth->implicit;
206         ClothVertex *verts = cloth->verts;
207         int numverts = cloth->numverts;
208         int i, j, v;
209         
210         const float ZERO[3] = {0.0f, 0.0f, 0.0f};
211         
212         for (v = 0; v < numverts; v++) {
213                 if (verts[v].flags & CLOTH_VERT_FLAG_PINNED) {
214                         /* pinned vertex constraints */
215                         BPH_mass_spring_add_constraint_ndof0(data, v, ZERO); /* velocity is defined externally */
216                 }
217                 
218                 verts[v].impulse_count = 0;
219         }
220
221         for (i = 0; i < totcolliders; ++i) {
222                 ColliderContacts *ct = &contacts[i];
223                 for (j = 0; j < ct->totcollisions; ++j) {
224                         CollPair *collpair = &ct->collisions[j];
225 //                      float restitution = (1.0f - clmd->coll_parms->damping) * (1.0f - ct->ob->pd->pdef_sbdamp);
226                         float restitution = 0.0f;
227                         int v = collpair->face1;
228                         float impulse[3];
229                         
230                         /* pinned verts handled separately */
231                         if (verts[v].flags & CLOTH_VERT_FLAG_PINNED)
232                                 continue;
233                         
234                         /* XXX cheap way of avoiding instability from multiple collisions in the same step
235                          * this should eventually be supported ...
236                          */
237                         if (verts[v].impulse_count > 0)
238                                 continue;
239                         
240                         /* calculate collision response */
241                         if (!collision_response(clmd, ct->collmd, collpair, dt, restitution, impulse))
242                                 continue;
243                         
244                         BPH_mass_spring_add_constraint_ndof2(data, v, collpair->normal, impulse);
245                         ++verts[v].impulse_count;
246                         
247                         BKE_sim_debug_data_add_dot(clmd->debug_data, collpair->pa, 0, 1, 0, "collision", hash_collpair(936, collpair));
248 //                      BKE_sim_debug_data_add_dot(clmd->debug_data, collpair->pb, 1, 0, 0, "collision", hash_collpair(937, collpair));
249 //                      BKE_sim_debug_data_add_line(clmd->debug_data, collpair->pa, collpair->pb, 0.7, 0.7, 0.7, "collision", hash_collpair(938, collpair));
250                         
251                         { /* DEBUG */
252                                 float nor[3];
253                                 mul_v3_v3fl(nor, collpair->normal, -collpair->distance);
254                                 BKE_sim_debug_data_add_vector(clmd->debug_data, collpair->pa, nor, 1, 1, 0, "collision", hash_collpair(939, collpair));
255 //                              BKE_sim_debug_data_add_vector(clmd->debug_data, collpair->pb, impulse, 1, 1, 0, "collision", hash_collpair(940, collpair));
256 //                              BKE_sim_debug_data_add_vector(clmd->debug_data, collpair->pb, collpair->normal, 1, 1, 0, "collision", hash_collpair(941, collpair));
257                         }
258                 }
259         }
260 }
261
262 /* computes where the cloth would be if it were subject to perfectly stiff edges
263  * (edge distance constraints) in a lagrangian solver.  then add forces to help
264  * guide the implicit solver to that state.  this function is called after
265  * collisions*/
266 static int UNUSED_FUNCTION(cloth_calc_helper_forces)(Object *UNUSED(ob), ClothModifierData *clmd, float (*initial_cos)[3], float UNUSED(step), float dt)
267 {
268         Cloth *cloth= clmd->clothObject;
269         float (*cos)[3] = (float (*)[3])MEM_callocN(sizeof(float)*3*cloth->numverts, "cos cloth_calc_helper_forces");
270         float *masses = (float *)MEM_callocN(sizeof(float)*cloth->numverts, "cos cloth_calc_helper_forces");
271         LinkNode *node;
272         ClothSpring *spring;
273         ClothVertex *cv;
274         int i, steps;
275         
276         cv = cloth->verts;
277         for (i=0; i<cloth->numverts; i++, cv++) {
278                 copy_v3_v3(cos[i], cv->tx);
279                 
280                 if (cv->goal == 1.0f || len_squared_v3v3(initial_cos[i], cv->tx) != 0.0f) {
281                         masses[i] = 1e+10;
282                 }
283                 else {
284                         masses[i] = cv->mass;
285                 }
286         }
287         
288         steps = 55;
289         for (i=0; i<steps; i++) {
290                 for (node=cloth->springs; node; node=node->next) {
291                         /* ClothVertex *cv1, *cv2; */ /* UNUSED */
292                         int v1, v2;
293                         float len, c, l, vec[3];
294                         
295                         spring = (ClothSpring *)node->link;
296                         if (spring->type != CLOTH_SPRING_TYPE_STRUCTURAL && spring->type != CLOTH_SPRING_TYPE_SHEAR) 
297                                 continue;
298                         
299                         v1 = spring->ij; v2 = spring->kl;
300                         /* cv1 = cloth->verts + v1; */ /* UNUSED */
301                         /* cv2 = cloth->verts + v2; */ /* UNUSED */
302                         len = len_v3v3(cos[v1], cos[v2]);
303                         
304                         sub_v3_v3v3(vec, cos[v1], cos[v2]);
305                         normalize_v3(vec);
306                         
307                         c = (len - spring->restlen);
308                         if (c == 0.0f)
309                                 continue;
310                         
311                         l = c / ((1.0f / masses[v1]) + (1.0f / masses[v2]));
312                         
313                         mul_v3_fl(vec, -(1.0f / masses[v1]) * l);
314                         add_v3_v3(cos[v1], vec);
315         
316                         sub_v3_v3v3(vec, cos[v2], cos[v1]);
317                         normalize_v3(vec);
318                         
319                         mul_v3_fl(vec, -(1.0f / masses[v2]) * l);
320                         add_v3_v3(cos[v2], vec);
321                 }
322         }
323         
324         cv = cloth->verts;
325         for (i=0; i<cloth->numverts; i++, cv++) {
326                 float vec[3];
327                 
328                 /*compute forces*/
329                 sub_v3_v3v3(vec, cos[i], cv->tx);
330                 mul_v3_fl(vec, cv->mass*dt*20.0f);
331                 add_v3_v3(cv->tv, vec);
332                 //copy_v3_v3(cv->tx, cos[i]);
333         }
334         
335         MEM_freeN(cos);
336         MEM_freeN(masses);
337         
338         return 1;
339 }
340
341 BLI_INLINE void cloth_calc_spring_force(ClothModifierData *clmd, ClothSpring *s, float time)
342 {
343         Cloth *cloth = clmd->clothObject;
344         ClothSimSettings *parms = clmd->sim_parms;
345         Implicit_Data *data = cloth->implicit;
346         ClothVertex *verts = cloth->verts;
347         
348         bool no_compress = parms->flags & CLOTH_SIMSETTINGS_FLAG_NO_SPRING_COMPRESS;
349         
350         zero_v3(s->f);
351         zero_m3(s->dfdx);
352         zero_m3(s->dfdv);
353         
354         s->flags &= ~CLOTH_SPRING_FLAG_NEEDED;
355         
356         // calculate force of structural + shear springs
357         if ((s->type & CLOTH_SPRING_TYPE_STRUCTURAL) || (s->type & CLOTH_SPRING_TYPE_SHEAR) || (s->type & CLOTH_SPRING_TYPE_SEWING) ) {
358 #ifdef CLOTH_FORCE_SPRING_STRUCTURAL
359                 float k, scaling;
360                 
361                 s->flags |= CLOTH_SPRING_FLAG_NEEDED;
362                 
363                 scaling = parms->structural + s->stiffness * fabsf(parms->max_struct - parms->structural);
364                 k = scaling / (parms->avg_spring_len + FLT_EPSILON);
365                 
366                 if (s->type & CLOTH_SPRING_TYPE_SEWING) {
367                         // TODO: verify, half verified (couldn't see error)
368                         // sewing springs usually have a large distance at first so clamp the force so we don't get tunnelling through colission objects
369                         BPH_mass_spring_force_spring_linear(data, s->ij, s->kl, s->restlen, k, parms->Cdis, no_compress, parms->max_sewing, s->f, s->dfdx, s->dfdv);
370                 }
371                 else {
372                         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);
373                 }
374 #endif
375         }
376         else if (s->type & CLOTH_SPRING_TYPE_GOAL) {
377 #ifdef CLOTH_FORCE_SPRING_GOAL
378                 float goal_x[3], goal_v[3];
379                 float k, scaling;
380                 
381                 s->flags |= CLOTH_SPRING_FLAG_NEEDED;
382                 
383                 // current_position = xold + t * (newposition - xold)
384                 interp_v3_v3v3(goal_x, verts[s->ij].xold, verts[s->ij].xconst, time);
385                 sub_v3_v3v3(goal_v, verts[s->ij].xconst, verts[s->ij].xold); // distance covered over dt==1
386                 
387                 scaling = parms->goalspring + s->stiffness * fabsf(parms->max_struct - parms->goalspring);
388                 k = verts[s->ij].goal * scaling / (parms->avg_spring_len + FLT_EPSILON);
389                 
390                 BPH_mass_spring_force_spring_goal(data, s->ij, goal_x, goal_v, k, parms->goalfrict * 0.01f, s->f, s->dfdx, s->dfdv);
391 #endif
392         }
393         else if (s->type & CLOTH_SPRING_TYPE_BENDING) {  /* calculate force of bending springs */
394 #ifdef CLOTH_FORCE_SPRING_BEND
395                 float kb, cb, scaling;
396                 
397                 s->flags |= CLOTH_SPRING_FLAG_NEEDED;
398                 
399                 scaling = parms->bending + s->stiffness * fabsf(parms->max_bend - parms->bending);
400                 kb = scaling / (20.0f * (parms->avg_spring_len + FLT_EPSILON));
401                 
402                 scaling = parms->bending_damping;
403                 cb = scaling / (20.0f * (parms->avg_spring_len + FLT_EPSILON));
404                 
405                 BPH_mass_spring_force_spring_bending(data, s->ij, s->kl, s->restlen, kb, cb, s->f, s->dfdx, s->dfdv);
406 #endif
407         }
408         else if (s->type & CLOTH_SPRING_TYPE_BENDING_ANG) {
409 #ifdef CLOTH_FORCE_SPRING_BEND
410                 float kb, cb, scaling;
411                 
412                 s->flags |= CLOTH_SPRING_FLAG_NEEDED;
413                 
414                 scaling = parms->bending + s->stiffness * fabsf(parms->max_bend - parms->bending);
415                 kb = scaling / (20.0f * (parms->avg_spring_len + FLT_EPSILON));
416                 
417                 scaling = parms->bending_damping;
418                 cb = scaling / (20.0f * (parms->avg_spring_len + FLT_EPSILON));
419                 
420                 /* XXX assuming same restlen for ij and jk segments here, this can be done correctly for hair later */
421                 BPH_mass_spring_force_spring_bending_angular(data, s->ij, s->kl, s->mn, s->target, kb, cb);
422                 
423                 {
424                         float x_kl[3], x_mn[3], v[3], d[3];
425                         
426                         BPH_mass_spring_get_motion_state(data, s->kl, x_kl, v);
427                         BPH_mass_spring_get_motion_state(data, s->mn, x_mn, v);
428                         
429                         BKE_sim_debug_data_add_dot(clmd->debug_data, x_kl, 0.9, 0.9, 0.9, "target", hash_vertex(7980, s->kl));
430                         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));
431                         
432                         copy_v3_v3(d, s->target);
433                         BKE_sim_debug_data_add_vector(clmd->debug_data, x_kl, d, 0.8, 0.8, 0.2, "target", hash_vertex(7982, s->kl));
434                         
435 //                      copy_v3_v3(d, s->target_ij);
436 //                      BKE_sim_debug_data_add_vector(clmd->debug_data, x, d, 1, 0.4, 0.4, "target", hash_vertex(7983, s->kl));
437                 }
438 #endif
439         }
440 }
441
442 static void hair_get_boundbox(ClothModifierData *clmd, float gmin[3], float gmax[3])
443 {
444         Cloth *cloth = clmd->clothObject;
445         Implicit_Data *data = cloth->implicit;
446         unsigned int numverts = cloth->numverts;
447         int i;
448         
449         INIT_MINMAX(gmin, gmax);
450         for (i = 0; i < numverts; i++) {
451                 float x[3];
452                 BPH_mass_spring_get_motion_state(data, i, x, NULL);
453                 DO_MINMAX(x, gmin, gmax);
454         }
455 }
456
457 static void cloth_calc_volume_force(ClothModifierData *clmd)
458 {
459         ClothSimSettings *parms = clmd->sim_parms;
460         Cloth *cloth = clmd->clothObject;
461         Implicit_Data *data = cloth->implicit;
462         int numverts = cloth->numverts;
463         
464         /* 2.0f is an experimental value that seems to give good results */
465         float smoothfac = 2.0f * parms->velocity_smooth;
466         // float collfac = 2.0f * parms->collider_friction;
467         float pressfac = parms->pressure;
468         float minpress = parms->pressure_threshold;
469         float gmin[3], gmax[3];
470         int i;
471         
472         hair_get_boundbox(clmd, gmin, gmax);
473         
474         /* gather velocities & density */
475         if (smoothfac > 0.0f || pressfac > 0.0f) {
476                 HairVertexGrid *vertex_grid = BPH_hair_volume_create_vertex_grid(clmd->sim_parms->voxel_res, gmin, gmax);
477                 
478                 for (i = 0; i < numverts; i++) {
479                         float x[3], v[3];
480                         
481                         BPH_mass_spring_get_motion_state(data, i, x, v);
482                         BPH_hair_volume_add_vertex(vertex_grid, x, v);
483                 }
484                 BPH_hair_volume_normalize_vertex_grid(vertex_grid);
485                 
486 #if 0
487                 /* apply velocity filter */
488                 BPH_hair_volume_vertex_grid_filter_box(vertex_grid, clmd->sim_parms->voxel_filter_size);
489 #endif
490                 
491                 for (i = 0; i < numverts; i++) {
492                         float x[3], v[3], f[3], dfdx[3][3], dfdv[3][3];
493                         
494                         /* calculate volumetric forces */
495                         BPH_mass_spring_get_motion_state(data, i, x, v);
496                         BPH_hair_volume_vertex_grid_forces(vertex_grid, x, v, smoothfac, pressfac, minpress, f, dfdx, dfdv);
497                         /* apply on hair data */
498                         BPH_mass_spring_force_extern(data, i, f, dfdx, dfdv);
499                 }
500                 
501                 BPH_hair_volume_free_vertex_grid(vertex_grid);
502         }
503 }
504
505 static void cloth_calc_force(ClothModifierData *clmd, float UNUSED(frame), ListBase *effectors, float time)
506 {
507         /* Collect forces and derivatives:  F, dFdX, dFdV */
508         Cloth *cloth = clmd->clothObject;
509         Implicit_Data *data = cloth->implicit;
510         unsigned int i  = 0;
511         float           drag    = clmd->sim_parms->Cvi * 0.01f; /* viscosity of air scaled in percent */
512         float           gravity[3] = {0.0f, 0.0f, 0.0f};
513         MFace           *mfaces         = cloth->mfaces;
514         unsigned int numverts = cloth->numverts;
515         
516 #ifdef CLOTH_FORCE_GRAVITY
517         /* global acceleration (gravitation) */
518         if (clmd->scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) {
519                 /* scale gravity force */
520                 mul_v3_v3fl(gravity, clmd->scene->physics_settings.gravity, 0.001f * clmd->sim_parms->effector_weights->global_gravity);
521         }
522         vert = cloth->verts;
523         for (i = 0; i < cloth->numverts; i++, vert++) {
524                 BPH_mass_spring_force_gravity(data, i, vert->mass, gravity);
525         }
526 #endif
527
528         cloth_calc_volume_force(clmd);
529
530 #ifdef CLOTH_FORCE_DRAG
531         BPH_mass_spring_force_drag(data, drag);
532 #endif
533         
534         /* handle external forces like wind */
535         if (effectors) {
536                 /* cache per-vertex forces to avoid redundant calculation */
537                 float (*winvec)[3] = (float (*)[3])MEM_callocN(sizeof(float) * 3 * numverts, "effector forces");
538                 for (i = 0; i < cloth->numverts; i++) {
539                         float x[3], v[3];
540                         EffectedPoint epoint;
541                         
542                         BPH_mass_spring_get_motion_state(data, i, x, v);
543                         pd_point_from_loc(clmd->scene, x, v, i, &epoint);
544                         pdDoEffectors(effectors, NULL, clmd->sim_parms->effector_weights, &epoint, winvec[i], NULL);
545                 }
546                 
547                 for (i = 0; i < cloth->numfaces; i++) {
548                         MFace *mf = &mfaces[i];
549                         BPH_mass_spring_force_face_wind(data, mf->v1, mf->v2, mf->v3, mf->v4, winvec);
550                 }
551
552                 /* Hair has only edges */
553                 if (cloth->numfaces == 0) {
554                         for (LinkNode *link = cloth->springs; link; link = link->next) {
555                                 ClothSpring *spring = (ClothSpring *)link->link;
556                                 if (spring->type == CLOTH_SPRING_TYPE_STRUCTURAL)
557                                         BPH_mass_spring_force_edge_wind(data, spring->ij, spring->kl, winvec);
558                         }
559                 }
560
561                 MEM_freeN(winvec);
562         }
563         
564         // calculate spring forces
565         for (LinkNode *link = cloth->springs; link; link = link->next) {
566                 ClothSpring *spring = (ClothSpring *)link->link;
567                 // only handle active springs
568                 if (!(spring->flags & CLOTH_SPRING_FLAG_DEACTIVATE))
569                         cloth_calc_spring_force(clmd, spring, time);
570         }
571 }
572
573 static void cloth_clear_result(ClothModifierData *clmd)
574 {
575         ClothSolverResult *sres = clmd->solver_result;
576         
577         sres->status = 0;
578         sres->max_error = sres->min_error = sres->avg_error = 0.0f;
579         sres->max_iterations = sres->min_iterations = 0;
580         sres->avg_iterations = 0.0f;
581 }
582
583 static void cloth_record_result(ClothModifierData *clmd, ImplicitSolverResult *result, int steps)
584 {
585         ClothSolverResult *sres = clmd->solver_result;
586         
587         if (sres->status) { /* already initialized ? */
588                 /* error only makes sense for successful iterations */
589                 if (result->status == BPH_SOLVER_SUCCESS) {
590                         sres->min_error = min_ff(sres->min_error, result->error);
591                         sres->max_error = max_ff(sres->max_error, result->error);
592                         sres->avg_error += result->error / (float)steps;
593                 }
594                 
595                 sres->min_iterations = min_ii(sres->min_iterations, result->iterations);
596                 sres->max_iterations = max_ii(sres->max_iterations, result->iterations);
597                 sres->avg_iterations += (float)result->iterations / (float)steps;
598         }
599         else {
600                 /* error only makes sense for successful iterations */
601                 if (result->status == BPH_SOLVER_SUCCESS) {
602                         sres->min_error = sres->max_error = result->error;
603                         sres->avg_error += result->error / (float)steps;
604                 }
605                 
606                 sres->min_iterations = sres->max_iterations  = result->iterations;
607                 sres->avg_iterations += (float)result->iterations / (float)steps;
608         }
609         
610         sres->status |= result->status;
611 }
612
613 int BPH_cloth_solve(Object *ob, float frame, ClothModifierData *clmd, ListBase *effectors)
614 {
615         unsigned int i=0;
616         float step=0.0f, tf=clmd->sim_parms->timescale;
617         Cloth *cloth = clmd->clothObject;
618         ClothVertex *verts = cloth->verts/*, *cv*/;
619         unsigned int numverts = cloth->numverts;
620         float dt = clmd->sim_parms->timescale / clmd->sim_parms->stepsPerFrame;
621         Implicit_Data *id = cloth->implicit;
622         ColliderContacts *contacts = NULL;
623         int totcolliders = 0;
624         
625         BPH_mass_spring_solver_debug_data(id, clmd->debug_data);
626         
627         BKE_sim_debug_data_clear_category(clmd->debug_data, "collision");
628         
629         if (!clmd->solver_result)
630                 clmd->solver_result = (ClothSolverResult *)MEM_callocN(sizeof(ClothSolverResult), "cloth solver result");
631         cloth_clear_result(clmd);
632         
633         if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) { /* do goal stuff */
634                 for (i = 0; i < numverts; i++) {
635                         // update velocities with constrained velocities from pinned verts
636                         if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) {
637                                 float v[3];
638                                 sub_v3_v3v3(v, verts[i].xconst, verts[i].xold);
639                                 // mul_v3_fl(v, clmd->sim_parms->stepsPerFrame);
640                                 BPH_mass_spring_set_velocity(id, i, v);
641                         }
642                 }
643         }
644         
645         if (clmd->debug_data) {
646                 for (i = 0; i < numverts; i++) {
647 //                      BKE_sim_debug_data_add_dot(clmd->debug_data, verts[i].x, 1.0f, 0.1f, 1.0f, "points", hash_vertex(583, i));
648                 }
649         }
650         
651         while (step < tf) {
652                 ImplicitSolverResult result;
653                 
654                 /* initialize forces to zero */
655                 BPH_mass_spring_clear_forces(id);
656                 BPH_mass_spring_clear_constraints(id);
657                 
658                 /* copy velocities for collision */
659                 for (i = 0; i < numverts; i++) {
660                         BPH_mass_spring_get_motion_state(id, i, NULL, verts[i].tv);
661                         copy_v3_v3(verts[i].v, verts[i].tv);
662                 }
663                 
664                 /* determine contact points */
665                 if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_ENABLED) {
666                         if (clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_POINTS) {
667                                 cloth_find_point_contacts(ob, clmd, 0.0f, tf, &contacts, &totcolliders);
668                         }
669                 }
670                 
671                 /* setup vertex constraints for pinned vertices and contacts */
672                 cloth_setup_constraints(clmd, contacts, totcolliders, dt);
673                 
674                 // damping velocity for artistic reasons
675                 // this is a bad way to do it, should be removed imo - lukas_t
676                 if (clmd->sim_parms->vel_damping != 1.0f) {
677                         for (i = 0; i < numverts; i++) {
678                                 float v[3];
679                                 BPH_mass_spring_get_motion_state(id, i, NULL, v);
680                                 mul_v3_fl(v, clmd->sim_parms->vel_damping);
681                                 BPH_mass_spring_set_velocity(id, i, v);
682                         }
683                 }
684                 
685                 // calculate forces
686                 cloth_calc_force(clmd, frame, effectors, step);
687                 
688                 // calculate new velocity and position
689                 BPH_mass_spring_solve(id, dt, &result);
690                 cloth_record_result(clmd, &result, clmd->sim_parms->stepsPerFrame);
691                 
692                 BPH_mass_spring_apply_result(id);
693                 
694                 /* move pinned verts to correct position */
695                 for (i = 0; i < numverts; i++) {
696                         if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) {
697                                 if (verts[i].flags & CLOTH_VERT_FLAG_PINNED) {
698                                         float x[3];
699                                         interp_v3_v3v3(x, verts[i].xold, verts[i].xconst, step + dt);
700                                         BPH_mass_spring_set_position(id, i, x);
701                                 }
702                         }
703                         
704                         BPH_mass_spring_get_motion_state(id, i, verts[i].txold, NULL);
705                         
706 //                      if (!(verts[i].flags & CLOTH_VERT_FLAG_PINNED) && i > 0) {
707 //                              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));
708 //                              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));
709 //                      }
710 //                      BKE_sim_debug_data_add_vector(clmd->debug_data, id->X[i], id->V[i], 0, 0, 1, "velocity", hash_vertex(3158, i));
711                 }
712                 
713                 /* free contact points */
714                 if (contacts) {
715                         cloth_free_contacts(contacts, totcolliders);
716                 }
717                 
718                 step += dt;
719         }
720         
721         /* copy results back to cloth data */
722         for (i = 0; i < numverts; i++) {
723                 BPH_mass_spring_get_motion_state(id, i, verts[i].x, verts[i].v);
724                 copy_v3_v3(verts[i].txold, verts[i].x);
725         }
726         
727         BPH_mass_spring_solver_debug_data(id, NULL);
728         
729         return 1;
730 }