misc macro --> bli math lib functions
[blender.git] / source / blender / blenkernel / intern / cloth.c
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  * Contributor(s): Daniel Genrich
22  *
23  * ***** END GPL LICENSE BLOCK *****
24  */
25
26 /** \file blender/blenkernel/intern/cloth.c
27  *  \ingroup bke
28  */
29
30
31 #include "MEM_guardedalloc.h"
32
33 #include "DNA_cloth_types.h"
34 #include "DNA_scene_types.h"
35 #include "DNA_object_types.h"
36 #include "DNA_meshdata_types.h"
37
38 #include "BLI_math.h"
39 #include "BLI_edgehash.h"
40 #include "BLI_utildefines.h"
41 #include "BLI_linklist.h"
42
43 #include "BKE_cdderivedmesh.h"
44 #include "BKE_cloth.h"
45 #include "BKE_effect.h"
46 #include "BKE_global.h"
47 #include "BKE_modifier.h"
48 #include "BKE_pointcache.h"
49
50
51 #ifdef _WIN32
52 void tstart ( void )
53 {}
54 void tend ( void )
55 {
56 }
57 double tval( void )
58 {
59         return 0;
60 }
61 #else
62 #include <sys/time.h>
63                          static struct timeval _tstart, _tend;
64          static struct timezone tz;
65          void tstart ( void )
66 {
67         gettimeofday ( &_tstart, &tz );
68 }
69 void tend ( void )
70 {
71         gettimeofday ( &_tend,&tz );
72 }
73 double tval(void)
74 {
75         double t1, t2;
76         t1 = ( double ) _tstart.tv_sec + ( double ) _tstart.tv_usec/ ( 1000*1000 );
77         t2 = ( double ) _tend.tv_sec + ( double ) _tend.tv_usec/ ( 1000*1000 );
78         return t2-t1;
79 }
80 #endif
81
82 /* Our available solvers. */
83 // 255 is the magic reserved number, so NEVER try to put 255 solvers in here!
84 // 254 = MAX!
85 static CM_SOLVER_DEF    solvers [] =
86 {
87         { "Implicit", CM_IMPLICIT, implicit_init, implicit_solver, implicit_free },
88                 // { "Implicit C++", CM_IMPLICITCPP, implicitcpp_init, implicitcpp_solver, implicitcpp_free },
89 };
90
91 /* ********** cloth engine ******* */
92 /* Prototypes for internal functions.
93 */
94 static void cloth_to_object (Object *ob,  ClothModifierData *clmd, DerivedMesh *dm);
95 static void cloth_from_mesh ( ClothModifierData *clmd, DerivedMesh *dm );
96 static int cloth_from_object(Object *ob, ClothModifierData *clmd, DerivedMesh *dm, float framenr, int first);
97 static int cloth_build_springs ( ClothModifierData *clmd, DerivedMesh *dm );
98 static void cloth_apply_vgroup ( ClothModifierData *clmd, DerivedMesh *dm );
99
100
101 /******************************************************************************
102 *
103 * External interface called by modifier.c clothModifier functions.
104 *
105 ******************************************************************************/
106 /**
107  * cloth_init -  creates a new cloth simulation.
108  *
109  * 1. create object
110  * 2. fill object with standard values or with the GUI settings if given
111  */
112 void cloth_init ( ClothModifierData *clmd )
113 {       
114         /* Initialize our new data structure to reasonable values. */
115         clmd->sim_parms->gravity [0] = 0.0;
116         clmd->sim_parms->gravity [1] = 0.0;
117         clmd->sim_parms->gravity [2] = -9.81;
118         clmd->sim_parms->structural = 15.0;
119         clmd->sim_parms->shear = 15.0;
120         clmd->sim_parms->bending = 0.5;
121         clmd->sim_parms->Cdis = 5.0; 
122         clmd->sim_parms->Cvi = 1.0;
123         clmd->sim_parms->mass = 0.3f;
124         clmd->sim_parms->stepsPerFrame = 5;
125         clmd->sim_parms->flags = 0;
126         clmd->sim_parms->solver_type = 0;
127         clmd->sim_parms->preroll = 0;
128         clmd->sim_parms->maxspringlen = 10;
129         clmd->sim_parms->vgroup_mass = 0;
130         clmd->sim_parms->avg_spring_len = 0.0;
131         clmd->sim_parms->presets = 2; /* cotton as start setting */
132         clmd->sim_parms->timescale = 1.0f; /* speed factor, describes how fast cloth moves */
133         clmd->sim_parms->reset = 0;
134         
135         clmd->coll_parms->self_friction = 5.0;
136         clmd->coll_parms->friction = 5.0;
137         clmd->coll_parms->loop_count = 2;
138         clmd->coll_parms->epsilon = 0.015f;
139         clmd->coll_parms->flags = CLOTH_COLLSETTINGS_FLAG_ENABLED;
140         clmd->coll_parms->collision_list = NULL;
141         clmd->coll_parms->self_loop_count = 1.0;
142         clmd->coll_parms->selfepsilon = 0.75;
143
144         /* These defaults are copied from softbody.c's
145         * softbody_calc_forces() function.
146         */
147         clmd->sim_parms->eff_force_scale = 1000.0;
148         clmd->sim_parms->eff_wind_scale = 250.0;
149
150         // also from softbodies
151         clmd->sim_parms->maxgoal = 1.0f;
152         clmd->sim_parms->mingoal = 0.0f;
153         clmd->sim_parms->defgoal = 0.0f;
154         clmd->sim_parms->goalspring = 1.0f;
155         clmd->sim_parms->goalfrict = 0.0f;
156         clmd->sim_parms->velocity_smooth = 0.0f;
157
158         if(!clmd->sim_parms->effector_weights)
159                 clmd->sim_parms->effector_weights = BKE_add_effector_weights(NULL);
160
161         if(clmd->point_cache)
162                 clmd->point_cache->step = 1;
163 }
164
165 static BVHTree *bvhselftree_build_from_cloth (ClothModifierData *clmd, float epsilon)
166 {
167         unsigned int i;
168         BVHTree *bvhtree;
169         Cloth *cloth;
170         ClothVertex *verts;
171         float co[12];
172
173         if(!clmd)
174                 return NULL;
175
176         cloth = clmd->clothObject;
177
178         if(!cloth)
179                 return NULL;
180         
181         verts = cloth->verts;
182         
183         // in the moment, return zero if no faces there
184         if(!cloth->numverts)
185                 return NULL;
186         
187         // create quadtree with k=26
188         bvhtree = BLI_bvhtree_new(cloth->numverts, epsilon, 4, 6);
189         
190         // fill tree
191         for(i = 0; i < cloth->numverts; i++, verts++)
192         {
193                 VECCOPY(&co[0*3], verts->xold);
194                 
195                 BLI_bvhtree_insert(bvhtree, i, co, 1);
196         }
197         
198         // balance tree
199         BLI_bvhtree_balance(bvhtree);
200         
201         return bvhtree;
202 }
203
204 static BVHTree *bvhtree_build_from_cloth (ClothModifierData *clmd, float epsilon)
205 {
206         unsigned int i;
207         BVHTree *bvhtree;
208         Cloth *cloth;
209         ClothVertex *verts;
210         MFace *mfaces;
211         float co[12];
212
213         if(!clmd)
214                 return NULL;
215
216         cloth = clmd->clothObject;
217
218         if(!cloth)
219                 return NULL;
220         
221         verts = cloth->verts;
222         mfaces = cloth->mfaces;
223         
224         // in the moment, return zero if no faces there
225         if(!cloth->numfaces)
226                 return NULL;
227         
228         // create quadtree with k=26
229         bvhtree = BLI_bvhtree_new(cloth->numfaces, epsilon, 4, 26);
230         
231         // fill tree
232         for(i = 0; i < cloth->numfaces; i++, mfaces++)
233         {
234                 VECCOPY(&co[0*3], verts[mfaces->v1].xold);
235                 VECCOPY(&co[1*3], verts[mfaces->v2].xold);
236                 VECCOPY(&co[2*3], verts[mfaces->v3].xold);
237                 
238                 if(mfaces->v4)
239                         VECCOPY(&co[3*3], verts[mfaces->v4].xold);
240                 
241                 BLI_bvhtree_insert(bvhtree, i, co, (mfaces->v4 ? 4 : 3));
242         }
243         
244         // balance tree
245         BLI_bvhtree_balance(bvhtree);
246         
247         return bvhtree;
248 }
249
250 void bvhtree_update_from_cloth(ClothModifierData *clmd, int moving)
251 {       
252         unsigned int i = 0;
253         Cloth *cloth = clmd->clothObject;
254         BVHTree *bvhtree = cloth->bvhtree;
255         ClothVertex *verts = cloth->verts;
256         MFace *mfaces;
257         float co[12], co_moving[12];
258         int ret = 0;
259         
260         if(!bvhtree)
261                 return;
262         
263         mfaces = cloth->mfaces;
264         
265         // update vertex position in bvh tree
266         if(verts && mfaces)
267         {
268                 for(i = 0; i < cloth->numfaces; i++, mfaces++)
269                 {
270                         VECCOPY(&co[0*3], verts[mfaces->v1].txold);
271                         VECCOPY(&co[1*3], verts[mfaces->v2].txold);
272                         VECCOPY(&co[2*3], verts[mfaces->v3].txold);
273                         
274                         if(mfaces->v4)
275                                 VECCOPY(&co[3*3], verts[mfaces->v4].txold);
276                 
277                         // copy new locations into array
278                         if(moving)
279                         {
280                                 // update moving positions
281                                 VECCOPY(&co_moving[0*3], verts[mfaces->v1].tx);
282                                 VECCOPY(&co_moving[1*3], verts[mfaces->v2].tx);
283                                 VECCOPY(&co_moving[2*3], verts[mfaces->v3].tx);
284                                 
285                                 if(mfaces->v4)
286                                         VECCOPY(&co_moving[3*3], verts[mfaces->v4].tx);
287                                 
288                                 ret = BLI_bvhtree_update_node(bvhtree, i, co, co_moving, (mfaces->v4 ? 4 : 3));
289                         }
290                         else
291                         {
292                                 ret = BLI_bvhtree_update_node(bvhtree, i, co, NULL, (mfaces->v4 ? 4 : 3));
293                         }
294                         
295                         // check if tree is already full
296                         if(!ret)
297                                 break;
298                 }
299                 
300                 BLI_bvhtree_update_tree(bvhtree);
301         }
302 }
303
304 void bvhselftree_update_from_cloth(ClothModifierData *clmd, int moving)
305 {       
306         unsigned int i = 0;
307         Cloth *cloth = clmd->clothObject;
308         BVHTree *bvhtree = cloth->bvhselftree;
309         ClothVertex *verts = cloth->verts;
310         MFace *mfaces;
311         float co[12], co_moving[12];
312         int ret = 0;
313         
314         if(!bvhtree)
315                 return;
316         
317         mfaces = cloth->mfaces;
318         
319         // update vertex position in bvh tree
320         if(verts && mfaces)
321         {
322                 for(i = 0; i < cloth->numverts; i++, verts++)
323                 {
324                         VECCOPY(&co[0*3], verts->txold);
325                         
326                         // copy new locations into array
327                         if(moving)
328                         {
329                                 // update moving positions
330                                 VECCOPY(&co_moving[0*3], verts->tx);
331                                 
332                                 ret = BLI_bvhtree_update_node(bvhtree, i, co, co_moving, 1);
333                         }
334                         else
335                         {
336                                 ret = BLI_bvhtree_update_node(bvhtree, i, co, NULL, 1);
337                         }
338                         
339                         // check if tree is already full
340                         if(!ret)
341                                 break;
342                 }
343                 
344                 BLI_bvhtree_update_tree(bvhtree);
345         }
346 }
347
348 void cloth_clear_cache(Object *ob, ClothModifierData *clmd, float framenr)
349 {
350         PTCacheID pid;
351         
352         BKE_ptcache_id_from_cloth(&pid, ob, clmd);
353
354         // don't do anything as long as we're in editmode!
355         if(pid.cache->edit && ob->mode & OB_MODE_PARTICLE_EDIT)
356                 return;
357         
358         BKE_ptcache_id_clear(&pid, PTCACHE_CLEAR_AFTER, framenr);
359 }
360
361 static int do_init_cloth(Object *ob, ClothModifierData *clmd, DerivedMesh *result, int framenr)
362 {
363         PointCache *cache;
364
365         cache= clmd->point_cache;
366
367         /* initialize simulation data if it didn't exist already */
368         if(clmd->clothObject == NULL) { 
369                 if(!cloth_from_object(ob, clmd, result, framenr, 1)) {
370                         BKE_ptcache_invalidate(cache);
371                         return 0;
372                 }
373         
374                 if(clmd->clothObject == NULL) {
375                         BKE_ptcache_invalidate(cache);
376                         return 0;
377                 }
378         
379                 implicit_set_positions(clmd);
380         }
381
382         return 1;
383 }
384
385 static int do_step_cloth(Object *ob, ClothModifierData *clmd, DerivedMesh *result, int framenr)
386 {
387         ClothVertex *verts = NULL;
388         Cloth *cloth;
389         ListBase *effectors = NULL;
390         MVert *mvert;
391         unsigned int i = 0;
392         int ret = 0;
393
394         /* simulate 1 frame forward */
395         cloth = clmd->clothObject;
396         verts = cloth->verts;
397         mvert = result->getVertArray(result);
398
399         /* force any pinned verts to their constrained location. */
400         for(i = 0; i < clmd->clothObject->numverts; i++, verts++) {
401                 /* save the previous position. */
402                 VECCOPY(verts->xold, verts->xconst);
403                 VECCOPY(verts->txold, verts->x);
404
405                 /* Get the current position. */
406                 VECCOPY(verts->xconst, mvert[i].co);
407                 mul_m4_v3(ob->obmat, verts->xconst);
408         }
409
410         effectors = pdInitEffectors(clmd->scene, ob, NULL, clmd->sim_parms->effector_weights);
411         
412         tstart();
413
414         /* call the solver. */
415         if(solvers [clmd->sim_parms->solver_type].solver)
416                 ret = solvers[clmd->sim_parms->solver_type].solver(ob, framenr, clmd, effectors);
417
418         tend();
419
420         pdEndEffectors(&effectors);
421
422         // printf ( "%f\n", ( float ) tval() );
423         
424         return ret;
425 }
426
427 /************************************************
428  * clothModifier_do - main simulation function
429 ************************************************/
430 DerivedMesh *clothModifier_do(ClothModifierData *clmd, Scene *scene, Object *ob, DerivedMesh *dm)
431 {
432         DerivedMesh *result;
433         PointCache *cache;
434         PTCacheID pid;
435         float timescale;
436         int framenr, startframe, endframe;
437         int cache_result;
438
439         clmd->scene= scene;     /* nice to pass on later :) */
440         framenr= (int)scene->r.cfra;
441         cache= clmd->point_cache;
442         result = CDDM_copy(dm);
443
444         BKE_ptcache_id_from_cloth(&pid, ob, clmd);
445         BKE_ptcache_id_time(&pid, scene, framenr, &startframe, &endframe, &timescale);
446         clmd->sim_parms->timescale= timescale;
447
448         if(!result) {
449                 BKE_ptcache_invalidate(cache);
450                 return dm;
451         }
452
453         if(clmd->sim_parms->reset
454                 || (framenr == (startframe - clmd->sim_parms->preroll) && clmd->sim_parms->preroll != 0)
455                 || (clmd->clothObject && result->getNumVerts(result) != clmd->clothObject->numverts))
456         {
457                 clmd->sim_parms->reset = 0;
458                 cache->flag |= PTCACHE_OUTDATED;
459                 BKE_ptcache_id_reset(scene, &pid, PTCACHE_RESET_OUTDATED);
460                 BKE_ptcache_validate(cache, 0);
461                 cache->last_exact= 0;
462                 cache->flag &= ~PTCACHE_REDO_NEEDED;
463                 return result;
464         }
465         
466         // unused in the moment, calculated separately in implicit.c
467         clmd->sim_parms->dt = clmd->sim_parms->timescale / clmd->sim_parms->stepsPerFrame;
468
469         /* handle continuous simulation with the play button */
470         if(BKE_ptcache_get_continue_physics() || ((clmd->sim_parms->preroll > 0) && (framenr > startframe - clmd->sim_parms->preroll) && (framenr < startframe))) {
471                 BKE_ptcache_invalidate(cache);
472
473                 /* do simulation */
474                 if(!do_init_cloth(ob, clmd, dm, framenr))
475                         return result;
476
477                 do_step_cloth(ob, clmd, dm, framenr);
478                 cloth_to_object(ob, clmd, result);
479
480                 return result;
481         }
482
483         /* simulation is only active during a specific period */
484         if(framenr < startframe) {
485                 BKE_ptcache_invalidate(cache);
486                 return result;
487         }
488         else if(framenr > endframe) {
489                 framenr= endframe;
490         }
491
492         /* initialize simulation data if it didn't exist already */
493         if(!do_init_cloth(ob, clmd, dm, framenr))
494                 return result;
495
496         if((framenr == startframe) && (clmd->sim_parms->preroll == 0)) {
497                 BKE_ptcache_id_reset(scene, &pid, PTCACHE_RESET_OUTDATED);
498                 do_init_cloth(ob, clmd, dm, framenr);
499                 BKE_ptcache_validate(cache, framenr);
500                 cache->flag &= ~PTCACHE_REDO_NEEDED;
501                 return result;
502         }
503
504         /* try to read from cache */
505         cache_result = BKE_ptcache_read(&pid, (float)framenr+scene->r.subframe);
506
507         if(cache_result == PTCACHE_READ_EXACT || cache_result == PTCACHE_READ_INTERPOLATED) {
508                 implicit_set_positions(clmd);
509                 cloth_to_object (ob, clmd, result);
510
511                 BKE_ptcache_validate(cache, framenr);
512
513                 if(cache_result == PTCACHE_READ_INTERPOLATED && cache->flag & PTCACHE_REDO_NEEDED)
514                         BKE_ptcache_write(&pid, framenr);
515
516                 return result;
517         }
518         else if(cache_result==PTCACHE_READ_OLD) {
519                 implicit_set_positions(clmd);
520         }
521         else if( /*ob->id.lib ||*/ (cache->flag & PTCACHE_BAKED)) { /* 2.4x disabled lib, but this can be used in some cases, testing further - campbell */
522                 /* if baked and nothing in cache, do nothing */
523                 BKE_ptcache_invalidate(cache);
524                 return result;
525         }
526
527         /* if on second frame, write cache for first frame */
528         if(cache->simframe == startframe && (cache->flag & PTCACHE_OUTDATED || cache->last_exact==0))
529                 BKE_ptcache_write(&pid, startframe);
530
531         clmd->sim_parms->timescale *= framenr - cache->simframe;
532
533         /* do simulation */
534         BKE_ptcache_validate(cache, framenr);
535
536         if(!do_step_cloth(ob, clmd, dm, framenr)) {
537                 BKE_ptcache_invalidate(cache);
538         }
539         else
540                 BKE_ptcache_write(&pid, framenr);
541
542         cloth_to_object (ob, clmd, result);
543
544         return result;
545 }
546
547 /* frees all */
548 void cloth_free_modifier(ClothModifierData *clmd )
549 {
550         Cloth   *cloth = NULL;
551         
552         if ( !clmd )
553                 return;
554
555         cloth = clmd->clothObject;
556
557         
558         if ( cloth )
559         {       
560                 // If our solver provides a free function, call it
561                 if ( solvers [clmd->sim_parms->solver_type].free )
562                 {
563                         solvers [clmd->sim_parms->solver_type].free ( clmd );
564                 }
565
566                 // Free the verts.
567                 if ( cloth->verts != NULL )
568                         MEM_freeN ( cloth->verts );
569
570                 cloth->verts = NULL;
571                 cloth->numverts = 0;
572
573                 // Free the springs.
574                 if ( cloth->springs != NULL )
575                 {
576                         LinkNode *search = cloth->springs;
577                         while(search)
578                         {
579                                 ClothSpring *spring = search->link;
580                                                 
581                                 MEM_freeN ( spring );
582                                 search = search->next;
583                         }
584                         BLI_linklist_free(cloth->springs, NULL);
585                 
586                         cloth->springs = NULL;
587                 }
588
589                 cloth->springs = NULL;
590                 cloth->numsprings = 0;
591
592                 // free BVH collision tree
593                 if ( cloth->bvhtree )
594                         BLI_bvhtree_free ( cloth->bvhtree );
595                 
596                 if ( cloth->bvhselftree )
597                         BLI_bvhtree_free ( cloth->bvhselftree );
598
599                 // we save our faces for collision objects
600                 if ( cloth->mfaces )
601                         MEM_freeN ( cloth->mfaces );
602                 
603                 if(cloth->edgehash)
604                         BLI_edgehash_free ( cloth->edgehash, NULL );
605                 
606                 
607                 /*
608                 if(clmd->clothObject->facemarks)
609                 MEM_freeN(clmd->clothObject->facemarks);
610                 */
611                 MEM_freeN ( cloth );
612                 clmd->clothObject = NULL;
613         }
614 }
615
616 /* frees all */
617 void cloth_free_modifier_extern ( ClothModifierData *clmd )
618 {
619         Cloth   *cloth = NULL;
620         if(G.rt > 0)
621                 printf("cloth_free_modifier_extern\n");
622         
623         if ( !clmd )
624                 return;
625
626         cloth = clmd->clothObject;
627         
628         if ( cloth )
629         {       
630                 if(G.rt > 0)
631                         printf("cloth_free_modifier_extern in\n");
632                 
633                 // If our solver provides a free function, call it
634                 if ( solvers [clmd->sim_parms->solver_type].free )
635                 {
636                         solvers [clmd->sim_parms->solver_type].free ( clmd );
637                 }
638
639                 // Free the verts.
640                 if ( cloth->verts != NULL )
641                         MEM_freeN ( cloth->verts );
642
643                 cloth->verts = NULL;
644                 cloth->numverts = 0;
645
646                 // Free the springs.
647                 if ( cloth->springs != NULL )
648                 {
649                         LinkNode *search = cloth->springs;
650                         while(search)
651                         {
652                                 ClothSpring *spring = search->link;
653                                                 
654                                 MEM_freeN ( spring );
655                                 search = search->next;
656                         }
657                         BLI_linklist_free(cloth->springs, NULL);
658                 
659                         cloth->springs = NULL;
660                 }
661
662                 cloth->springs = NULL;
663                 cloth->numsprings = 0;
664
665                 // free BVH collision tree
666                 if ( cloth->bvhtree )
667                         BLI_bvhtree_free ( cloth->bvhtree );
668                 
669                 if ( cloth->bvhselftree )
670                         BLI_bvhtree_free ( cloth->bvhselftree );
671
672                 // we save our faces for collision objects
673                 if ( cloth->mfaces )
674                         MEM_freeN ( cloth->mfaces );
675                 
676                 if(cloth->edgehash)
677                         BLI_edgehash_free ( cloth->edgehash, NULL );
678                 
679                 
680                 /*
681                 if(clmd->clothObject->facemarks)
682                 MEM_freeN(clmd->clothObject->facemarks);
683                 */
684                 MEM_freeN ( cloth );
685                 clmd->clothObject = NULL;
686         }
687 }
688
689 /******************************************************************************
690 *
691 * Internal functions.
692 *
693 ******************************************************************************/
694
695 /**
696  * cloth_to_object - copies the deformed vertices to the object.
697  *
698  **/
699 static void cloth_to_object (Object *ob,  ClothModifierData *clmd, DerivedMesh *dm)
700 {
701         unsigned int    i = 0;
702         MVert *mvert = NULL;
703         unsigned int numverts;
704         Cloth *cloth = clmd->clothObject;
705
706         if (clmd->clothObject) {
707                 /* inverse matrix is not uptodate... */
708                 invert_m4_m4(ob->imat, ob->obmat);
709
710                 mvert = CDDM_get_verts(dm);
711                 numverts = dm->getNumVerts(dm);
712
713                 for (i = 0; i < numverts; i++)
714                 {
715                         VECCOPY (mvert[i].co, cloth->verts[i].x);
716                         mul_m4_v3(ob->imat, mvert[i].co);       /* cloth is in global coords */
717                 }
718         }
719 }
720
721
722 int cloth_uses_vgroup(ClothModifierData *clmd)
723 {
724         return (((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_SCALING ) || 
725                 (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )) && 
726                 ((clmd->sim_parms->vgroup_mass>0) || 
727                 (clmd->sim_parms->vgroup_struct>0)||
728                 (clmd->sim_parms->vgroup_bend>0)));
729 }
730
731 /**
732  * cloth_apply_vgroup - applies a vertex group as specified by type
733  *
734  **/
735 /* can be optimized to do all groups in one loop */
736 static void cloth_apply_vgroup ( ClothModifierData *clmd, DerivedMesh *dm )
737 {
738         int i = 0;
739         int j = 0;
740         MDeformVert *dvert = NULL;
741         Cloth *clothObj = NULL;
742         int numverts;
743         /* float goalfac = 0; */ /* UNUSED */
744         ClothVertex *verts = NULL;
745
746         if (!clmd || !dm) return;
747
748         clothObj = clmd->clothObject;
749
750         numverts = dm->getNumVerts ( dm );
751
752         verts = clothObj->verts;
753         
754         if (cloth_uses_vgroup(clmd))
755         {
756                 for ( i = 0; i < numverts; i++, verts++ )
757                 {       
758                         dvert = dm->getVertData ( dm, i, CD_MDEFORMVERT );
759                         if ( dvert )
760                         {
761                                 for ( j = 0; j < dvert->totweight; j++ )
762                                 {
763                                         if (( dvert->dw[j].def_nr == (clmd->sim_parms->vgroup_mass-1)) && (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ))
764                                         {
765                                                 verts->goal = dvert->dw [j].weight;
766                                                 /* goalfac= 1.0f; */ /* UNUSED */
767                                                 
768                                                 /*
769                                                 // Kicking goal factor to simplify things...who uses that anyway?
770                                                 // ABS ( clmd->sim_parms->maxgoal - clmd->sim_parms->mingoal );
771                                                 */
772                                                 
773                                                 verts->goal  = ( float ) pow ( verts->goal , 4.0f );
774                                                 if ( verts->goal >=SOFTGOALSNAP )
775                                                 {
776                                                          verts->flags |= CLOTH_VERT_FLAG_PINNED;
777                                                 }
778                                         }
779                                         
780                                         if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_SCALING )
781                                         {
782                                                 if( dvert->dw[j].def_nr == (clmd->sim_parms->vgroup_struct-1))
783                                                 {
784                                                         verts->struct_stiff = dvert->dw [j].weight;
785                                                         verts->shear_stiff = dvert->dw [j].weight;
786                                                 }
787                                                 
788                                                 if( dvert->dw[j].def_nr == (clmd->sim_parms->vgroup_bend-1))
789                                                 {
790                                                         verts->bend_stiff = dvert->dw [j].weight;
791                                                 }
792                                         }
793                                         /*
794                                         // for later
795                                         if( dvert->dw[j].def_nr == (clmd->sim_parms->vgroup_weight-1))
796                                         {
797                                                 verts->mass = dvert->dw [j].weight;
798                                         }
799                                         */
800                                 }
801                         }
802                 }
803         }
804 }
805
806 static int cloth_from_object(Object *ob, ClothModifierData *clmd, DerivedMesh *dm, float UNUSED(framenr), int first)
807 {
808         int i = 0;
809         MVert *mvert = NULL;
810         ClothVertex *verts = NULL;
811         float (*shapekey_rest)[3]= NULL;
812         float tnull[3] = {0,0,0};
813         Cloth *cloth = NULL;
814         float maxdist = 0;
815
816         // If we have a clothObject, free it. 
817         if ( clmd->clothObject != NULL )
818         {
819                 cloth_free_modifier ( clmd );
820                 if(G.rt > 0)
821                         printf("cloth_free_modifier cloth_from_object\n");
822         }
823
824         // Allocate a new cloth object.
825         clmd->clothObject = MEM_callocN ( sizeof ( Cloth ), "cloth" );
826         if ( clmd->clothObject )
827         {
828                 clmd->clothObject->old_solver_type = 255;
829                 // clmd->clothObject->old_collision_type = 255;
830                 cloth = clmd->clothObject;
831                 clmd->clothObject->edgehash = NULL;
832         }
833         else if ( !clmd->clothObject )
834         {
835                 modifier_setError ( & ( clmd->modifier ), "Out of memory on allocating clmd->clothObject." );
836                 return 0;
837         }
838
839         // mesh input objects need DerivedMesh
840         if ( !dm )
841                 return 0;
842
843         cloth_from_mesh ( clmd, dm );
844
845         // create springs 
846         clmd->clothObject->springs = NULL;
847         clmd->clothObject->numsprings = -1;
848         
849         if( clmd->sim_parms->shapekey_rest )
850                 shapekey_rest = dm->getVertDataArray ( dm, CD_CLOTH_ORCO );
851
852         mvert = dm->getVertArray ( dm );
853
854         verts = clmd->clothObject->verts;
855
856         // set initial values
857         for ( i = 0; i < dm->getNumVerts(dm); i++, verts++ )
858         {
859                 if(first)
860                 {
861                         copy_v3_v3( verts->x, mvert[i].co );
862
863                         mul_m4_v3( ob->obmat, verts->x );
864
865                         if( shapekey_rest ) {
866                                 verts->xrest= shapekey_rest[i];
867                                 mul_m4_v3( ob->obmat, verts->xrest );
868                         }
869                         else
870                                 verts->xrest = verts->x;
871                 }
872                 
873                 /* no GUI interface yet */
874                 verts->mass = clmd->sim_parms->mass; 
875                 verts->impulse_count = 0;
876
877                 if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
878                         verts->goal= clmd->sim_parms->defgoal;
879                 else
880                         verts->goal= 0.0f;
881
882                 verts->flags = 0;
883                 VECCOPY ( verts->xold, verts->x );
884                 VECCOPY ( verts->xconst, verts->x );
885                 VECCOPY ( verts->txold, verts->x );
886                 VECCOPY ( verts->tx, verts->x );
887                 mul_v3_fl( verts->v, 0.0f );
888
889                 verts->impulse_count = 0;
890                 VECCOPY ( verts->impulse, tnull );
891         }
892         
893         // apply / set vertex groups
894         // has to be happen before springs are build!
895         cloth_apply_vgroup (clmd, dm);
896
897         if ( !cloth_build_springs ( clmd, dm ) )
898         {
899                 cloth_free_modifier ( clmd );
900                 modifier_setError ( & ( clmd->modifier ), "Can't build springs." );
901                 printf("cloth_free_modifier cloth_build_springs\n");
902                 return 0;
903         }
904         
905         for ( i = 0; i < dm->getNumVerts(dm); i++)
906         {
907                 if((!(cloth->verts[i].flags & CLOTH_VERT_FLAG_PINNED)) && (cloth->verts[i].goal > ALMOST_ZERO))
908                 {
909                         cloth_add_spring (clmd, i, i, 0.0, CLOTH_SPRING_TYPE_GOAL);
910                 }
911         }
912         
913         // init our solver
914         if ( solvers [clmd->sim_parms->solver_type].init ) {
915                 solvers [clmd->sim_parms->solver_type].init ( ob, clmd );
916         }
917         
918         if(!first)
919                 implicit_set_positions(clmd);
920
921         clmd->clothObject->bvhtree = bvhtree_build_from_cloth ( clmd, MAX2(clmd->coll_parms->epsilon, clmd->coll_parms->distance_repel) );
922         
923         for(i = 0; i < dm->getNumVerts(dm); i++)
924         {
925                 maxdist = MAX2(maxdist, clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len*2.0f));
926         }
927         
928         clmd->clothObject->bvhselftree = bvhselftree_build_from_cloth ( clmd, maxdist );
929
930         return 1;
931 }
932
933 static void cloth_from_mesh ( ClothModifierData *clmd, DerivedMesh *dm )
934 {
935         unsigned int numverts = dm->getNumVerts ( dm );
936         unsigned int numfaces = dm->getNumFaces ( dm );
937         MFace *mface = dm->getFaceArray( dm );
938         unsigned int i = 0;
939
940         /* Allocate our vertices. */
941         clmd->clothObject->numverts = numverts;
942         clmd->clothObject->verts = MEM_callocN ( sizeof ( ClothVertex ) * clmd->clothObject->numverts, "clothVertex" );
943         if ( clmd->clothObject->verts == NULL )
944         {
945                 cloth_free_modifier ( clmd );
946                 modifier_setError ( & ( clmd->modifier ), "Out of memory on allocating clmd->clothObject->verts." );
947                 printf("cloth_free_modifier clmd->clothObject->verts\n");
948                 return;
949         }
950
951         // save face information
952         clmd->clothObject->numfaces = numfaces;
953         clmd->clothObject->mfaces = MEM_callocN ( sizeof ( MFace ) * clmd->clothObject->numfaces, "clothMFaces" );
954         if ( clmd->clothObject->mfaces == NULL )
955         {
956                 cloth_free_modifier ( clmd );
957                 modifier_setError ( & ( clmd->modifier ), "Out of memory on allocating clmd->clothObject->mfaces." );
958                 printf("cloth_free_modifier clmd->clothObject->mfaces\n");
959                 return;
960         }
961         for ( i = 0; i < numfaces; i++ )
962                 memcpy ( &clmd->clothObject->mfaces[i], &mface[i], sizeof ( MFace ) );
963
964         /* Free the springs since they can't be correct if the vertices
965         * changed.
966         */
967         if ( clmd->clothObject->springs != NULL )
968                 MEM_freeN ( clmd->clothObject->springs );
969
970 }
971
972 /***************************************************************************************
973 * SPRING NETWORK BUILDING IMPLEMENTATION BEGIN
974 ***************************************************************************************/
975
976 // be careful: implicit solver has to be resettet when using this one!
977 // --> only for implicit handling of this spring!
978 int cloth_add_spring ( ClothModifierData *clmd, unsigned int indexA, unsigned int indexB, float restlength, int spring_type)
979 {
980         Cloth *cloth = clmd->clothObject;
981         ClothSpring *spring = NULL;
982         
983         if(cloth)
984         {
985                 // TODO: look if this spring is already there
986                 
987                 spring = ( ClothSpring * ) MEM_callocN ( sizeof ( ClothSpring ), "cloth spring" );
988                 
989                 if(!spring)
990                         return 0;
991                 
992                 spring->ij = indexA;
993                 spring->kl = indexB;
994                 spring->restlen =  restlength;
995                 spring->type = spring_type;
996                 spring->flags = 0;
997                 spring->stiffness = 0;
998                 
999                 cloth->numsprings++;
1000         
1001                 BLI_linklist_prepend ( &cloth->springs, spring );
1002                 
1003                 return 1;
1004         }
1005         return 0;
1006 }
1007
1008 static void cloth_free_errorsprings(Cloth *cloth, EdgeHash *UNUSED(edgehash), LinkNode **edgelist)
1009 {
1010         unsigned int i = 0;
1011         
1012         if ( cloth->springs != NULL )
1013         {
1014                 LinkNode *search = cloth->springs;
1015                 while(search)
1016                 {
1017                         ClothSpring *spring = search->link;
1018                                                 
1019                         MEM_freeN ( spring );
1020                         search = search->next;
1021                 }
1022                 BLI_linklist_free(cloth->springs, NULL);
1023                 
1024                 cloth->springs = NULL;
1025         }
1026         
1027         if(edgelist)
1028         {
1029                 for ( i = 0; i < cloth->numverts; i++ )
1030                 {
1031                         BLI_linklist_free ( edgelist[i],NULL );
1032                 }
1033
1034                 MEM_freeN ( edgelist );
1035         }
1036         
1037         if(cloth->edgehash)
1038                 BLI_edgehash_free ( cloth->edgehash, NULL );
1039 }
1040
1041 static int cloth_build_springs ( ClothModifierData *clmd, DerivedMesh *dm )
1042 {
1043         Cloth *cloth = clmd->clothObject;
1044         ClothSpring *spring = NULL, *tspring = NULL, *tspring2 = NULL;
1045         unsigned int struct_springs = 0, shear_springs=0, bend_springs = 0;
1046         unsigned int i = 0;
1047         unsigned int numverts = (unsigned int)dm->getNumVerts ( dm );
1048         unsigned int numedges = (unsigned int)dm->getNumEdges ( dm );
1049         unsigned int numfaces = (unsigned int)dm->getNumFaces ( dm );
1050         MEdge *medge = dm->getEdgeArray ( dm );
1051         MFace *mface = dm->getFaceArray ( dm );
1052         int index2 = 0; // our second vertex index
1053         LinkNode **edgelist = NULL;
1054         EdgeHash *edgehash = NULL;
1055         LinkNode *search = NULL, *search2 = NULL;
1056         float temp[3];
1057         
1058         // error handling
1059         if ( numedges==0 )
1060                 return 0;
1061
1062         cloth->springs = NULL;
1063
1064         edgelist = MEM_callocN ( sizeof ( LinkNode * ) * numverts, "cloth_edgelist_alloc" );
1065         
1066         if(!edgelist)
1067                 return 0;
1068         
1069         for ( i = 0; i < numverts; i++ )
1070         {
1071                 edgelist[i] = NULL;
1072         }
1073
1074         if ( cloth->springs )
1075                 MEM_freeN ( cloth->springs );
1076
1077         // create spring network hash
1078         edgehash = BLI_edgehash_new();
1079
1080         // structural springs
1081         for ( i = 0; i < numedges; i++ )
1082         {
1083                 spring = ( ClothSpring * ) MEM_callocN ( sizeof ( ClothSpring ), "cloth spring" );
1084
1085                 if ( spring )
1086                 {
1087                         spring->ij = MIN2(medge[i].v1, medge[i].v2);
1088                         spring->kl = MAX2(medge[i].v2, medge[i].v1);
1089                         VECSUB ( temp, cloth->verts[spring->kl].xrest, cloth->verts[spring->ij].xrest );
1090                         spring->restlen =  sqrt ( INPR ( temp, temp ) );
1091                         clmd->sim_parms->avg_spring_len += spring->restlen;
1092                         cloth->verts[spring->ij].avg_spring_len += spring->restlen;
1093                         cloth->verts[spring->kl].avg_spring_len += spring->restlen;
1094                         cloth->verts[spring->ij].spring_count++;
1095                         cloth->verts[spring->kl].spring_count++;
1096                         spring->type = CLOTH_SPRING_TYPE_STRUCTURAL;
1097                         spring->flags = 0;
1098                         spring->stiffness = (cloth->verts[spring->kl].struct_stiff + cloth->verts[spring->ij].struct_stiff) / 2.0;
1099                         struct_springs++;
1100                         
1101                         BLI_linklist_prepend ( &cloth->springs, spring );
1102                 }
1103                 else
1104                 {
1105                         cloth_free_errorsprings(cloth, edgehash, edgelist);
1106                         return 0;
1107                 }
1108         }
1109         
1110         if(struct_springs > 0)
1111                 clmd->sim_parms->avg_spring_len /= struct_springs;
1112         
1113         for(i = 0; i < numverts; i++)
1114         {
1115                 cloth->verts[i].avg_spring_len = cloth->verts[i].avg_spring_len * 0.49 / ((float)cloth->verts[i].spring_count);
1116         }
1117         
1118         // shear springs
1119         for ( i = 0; i < numfaces; i++ )
1120         {
1121                 // triangle faces already have shear springs due to structural geometry
1122                 if ( !mface[i].v4 )
1123                         continue; 
1124                 
1125                 spring = ( ClothSpring *) MEM_callocN ( sizeof ( ClothSpring ), "cloth spring" );
1126                 
1127                 if(!spring)
1128                 {
1129                         cloth_free_errorsprings(cloth, edgehash, edgelist);
1130                         return 0;
1131                 }
1132
1133                 spring->ij = MIN2(mface[i].v1, mface[i].v3);
1134                 spring->kl = MAX2(mface[i].v3, mface[i].v1);
1135                 VECSUB ( temp, cloth->verts[spring->kl].xrest, cloth->verts[spring->ij].xrest );
1136                 spring->restlen =  sqrt ( INPR ( temp, temp ) );
1137                 spring->type = CLOTH_SPRING_TYPE_SHEAR;
1138                 spring->stiffness = (cloth->verts[spring->kl].shear_stiff + cloth->verts[spring->ij].shear_stiff) / 2.0;
1139
1140                 BLI_linklist_append ( &edgelist[spring->ij], spring );
1141                 BLI_linklist_append ( &edgelist[spring->kl], spring );
1142                 shear_springs++;
1143
1144                 BLI_linklist_prepend ( &cloth->springs, spring );
1145
1146                 
1147                 // if ( mface[i].v4 ) --> Quad face
1148                 spring = ( ClothSpring * ) MEM_callocN ( sizeof ( ClothSpring ), "cloth spring" );
1149                 
1150                 if(!spring)
1151                 {
1152                         cloth_free_errorsprings(cloth, edgehash, edgelist);
1153                         return 0;
1154                 }
1155
1156                 spring->ij = MIN2(mface[i].v2, mface[i].v4);
1157                 spring->kl = MAX2(mface[i].v4, mface[i].v2);
1158                 VECSUB ( temp, cloth->verts[spring->kl].xrest, cloth->verts[spring->ij].xrest );
1159                 spring->restlen =  sqrt ( INPR ( temp, temp ) );
1160                 spring->type = CLOTH_SPRING_TYPE_SHEAR;
1161                 spring->stiffness = (cloth->verts[spring->kl].shear_stiff + cloth->verts[spring->ij].shear_stiff) / 2.0;
1162
1163                 BLI_linklist_append ( &edgelist[spring->ij], spring );
1164                 BLI_linklist_append ( &edgelist[spring->kl], spring );
1165                 shear_springs++;
1166
1167                 BLI_linklist_prepend ( &cloth->springs, spring );
1168         }
1169         
1170         if(numfaces) {
1171                 // bending springs
1172                 search2 = cloth->springs;
1173                 for ( i = struct_springs; i < struct_springs+shear_springs; i++ )
1174                 {
1175                         if ( !search2 )
1176                                 break;
1177
1178                         tspring2 = search2->link;
1179                         search = edgelist[tspring2->kl];
1180                         while ( search )
1181                         {
1182                                 tspring = search->link;
1183                                 index2 = ( ( tspring->ij==tspring2->kl ) ? ( tspring->kl ) : ( tspring->ij ) );
1184                                 
1185                                 // check for existing spring
1186                                 // check also if startpoint is equal to endpoint
1187                                 if ( !BLI_edgehash_haskey ( edgehash, MIN2(tspring2->ij, index2), MAX2(tspring2->ij, index2) )
1188                                 && ( index2!=tspring2->ij ) )
1189                                 {
1190                                         spring = ( ClothSpring * ) MEM_callocN ( sizeof ( ClothSpring ), "cloth spring" );
1191                                         
1192                                         if(!spring)
1193                                         {
1194                                                 cloth_free_errorsprings(cloth, edgehash, edgelist);
1195                                                 return 0;
1196                                         }
1197
1198                                         spring->ij = MIN2(tspring2->ij, index2);
1199                                         spring->kl = MAX2(tspring2->ij, index2);
1200                                         VECSUB ( temp, cloth->verts[spring->kl].xrest, cloth->verts[spring->ij].xrest );
1201                                         spring->restlen =  sqrt ( INPR ( temp, temp ) );
1202                                         spring->type = CLOTH_SPRING_TYPE_BENDING;
1203                                         spring->stiffness = (cloth->verts[spring->kl].bend_stiff + cloth->verts[spring->ij].bend_stiff) / 2.0;
1204                                         BLI_edgehash_insert ( edgehash, spring->ij, spring->kl, NULL );
1205                                         bend_springs++;
1206
1207                                         BLI_linklist_prepend ( &cloth->springs, spring );
1208                                 }
1209                                 search = search->next;
1210                         }
1211                         search2 = search2->next;
1212                 }
1213         }
1214         else if(struct_springs > 2) {
1215                 /* bending springs for hair strands */
1216                 /* The current algorightm only goes through the edges in order of the mesh edges list   */
1217                 /* and makes springs between the outer vert of edges sharing a vertice. This works just */
1218                 /* fine for hair, but not for user generated string meshes. This could/should be later  */
1219                 /* extended to work with non-ordered edges so that it can be used for general "rope             */
1220                 /* dynamics" without the need for the vertices or edges to be ordered through the length*/
1221                 /* of the strands. -jahka */
1222                 search = cloth->springs;
1223                 search2 = search->next;
1224                 while(search && search2)
1225                 {
1226                         tspring = search->link;
1227                         tspring2 = search2->link;
1228
1229                         if(tspring->ij == tspring2->kl) {
1230                                 spring = ( ClothSpring * ) MEM_callocN ( sizeof ( ClothSpring ), "cloth spring" );
1231                                 
1232                                 if(!spring)
1233                                 {
1234                                         cloth_free_errorsprings(cloth, edgehash, edgelist);
1235                                         return 0;
1236                                 }
1237
1238                                 spring->ij = tspring2->ij;
1239                                 spring->kl = tspring->kl;
1240                                 VECSUB ( temp, cloth->verts[spring->kl].xrest, cloth->verts[spring->ij].xrest );
1241                                 spring->restlen =  sqrt ( INPR ( temp, temp ) );
1242                                 spring->type = CLOTH_SPRING_TYPE_BENDING;
1243                                 spring->stiffness = (cloth->verts[spring->kl].bend_stiff + cloth->verts[spring->ij].bend_stiff) / 2.0;
1244                                 bend_springs++;
1245
1246                                 BLI_linklist_prepend ( &cloth->springs, spring );
1247                         }
1248                         
1249                         search = search->next;
1250                         search2 = search2->next;
1251                 }
1252         }
1253         
1254         /* insert other near springs in edgehash AFTER bending springs are calculated (for selfcolls) */
1255         for ( i = 0; i < numedges; i++ ) // struct springs
1256                 BLI_edgehash_insert ( edgehash, MIN2(medge[i].v1, medge[i].v2), MAX2(medge[i].v2, medge[i].v1), NULL );
1257         
1258         for ( i = 0; i < numfaces; i++ ) // edge springs
1259         {
1260                 if(mface[i].v4)
1261                 {
1262                         BLI_edgehash_insert ( edgehash, MIN2(mface[i].v1, mface[i].v3), MAX2(mface[i].v3, mface[i].v1), NULL );
1263                         
1264                         BLI_edgehash_insert ( edgehash, MIN2(mface[i].v2, mface[i].v4), MAX2(mface[i].v2, mface[i].v4), NULL );
1265                 }
1266         }
1267         
1268         
1269         cloth->numsprings = struct_springs + shear_springs + bend_springs;
1270         
1271         if ( edgelist )
1272         {
1273                 for ( i = 0; i < numverts; i++ )
1274                 {
1275                         BLI_linklist_free ( edgelist[i],NULL );
1276                 }
1277         
1278                 MEM_freeN ( edgelist );
1279         }
1280         
1281         cloth->edgehash = edgehash;
1282         
1283         if(G.rt>0)
1284                 printf("avg_len: %f\n",clmd->sim_parms->avg_spring_len);
1285
1286         return 1;
1287
1288 } /* cloth_build_springs */
1289 /***************************************************************************************
1290 * SPRING NETWORK BUILDING IMPLEMENTATION END
1291 ***************************************************************************************/
1292