Fix for several memory leaks in fluidsim (found with Valgrind).
[blender.git] / intern / elbeem / intern / controlparticles.cpp
1 // --------------------------------------------------------------------------
2 //
3 // El'Beem - the visual lattice boltzmann freesurface simulator
4 // All code distributed as part of El'Beem is covered by the version 2 of the 
5 // GNU General Public License. See the file COPYING for details.  
6 //
7 // Copyright 2008 Nils Thuerey , Richard Keiser, Mark Pauly, Ulrich Ruede
8 //
9 // implementation of control particle handling
10 //
11 // --------------------------------------------------------------------------
12
13 // indicator for LBM inclusion
14 #include "ntl_geometrymodel.h"
15 #include "ntl_world.h"
16 #include "solver_class.h"
17 #include "controlparticles.h"
18 #include "mvmcoords.h"
19 #include <zlib.h>
20
21 #ifndef sqrtf
22 #define sqrtf sqrt
23 #endif
24
25 // brute force circle test init in initTimeArray
26 // replaced by mDebugInit
27 //#define CP_FORCECIRCLEINIT 0
28
29
30 void ControlParticles::initBlenderTest() {
31         mPartSets.clear();
32
33         ControlParticleSet cps;
34         mPartSets.push_back(cps);
35         int setCnt = mPartSets.size()-1;
36         ControlParticle p; 
37
38         // set for time zero
39         mPartSets[setCnt].time = 0.;
40
41         // add single particle 
42         p.reset();
43         p.pos = LbmVec(0.5, 0.5, -0.5);
44         mPartSets[setCnt].particles.push_back(p);
45
46         // add second set for animation
47         mPartSets.push_back(cps);
48         setCnt = mPartSets.size()-1;
49         mPartSets[setCnt].time = 0.15;
50
51         // insert new position
52         p.reset();
53         p.pos = LbmVec(-0.5, -0.5, 0.5);
54         mPartSets[setCnt].particles.push_back(p);
55
56         // applyTrafos();
57         initTime(0. , 1.);
58 }
59
60 int ControlParticles::initFromObject(ntlGeometryObjModel *model) {
61         vector<ntlTriangle> triangles;
62         vector<ntlVec3Gfx> vertices;
63         vector<ntlVec3Gfx> normals;
64         
65         /*
66         model->loadBobjModel(string(infile));
67         
68         model->setLoaded(true);
69         
70         model->setGeoInitId(gid);
71         
72         
73         printf("a animated? %d\n", model->getIsAnimated());
74         printf("b animated? %d\n", model->getMeshAnimated());
75         */
76         model->setGeoInitType(FGI_FLUID);
77         
78         model->getTriangles(mCPSTimeStart, &triangles, &vertices, &normals, 1 ); 
79         // model->applyTransformation(mCPSTimeStart, &vertices, &normals, 0, vertices.size(), true);
80         
81         // valid mesh?
82         if(triangles.size() <= 0) {
83                 return 0;
84         }
85
86         ntlRenderGlobals *glob = new ntlRenderGlobals;
87         ntlScene *genscene = new ntlScene( glob, false );
88         genscene->addGeoClass(model);
89         genscene->addGeoObject(model);
90         genscene->buildScene(0., false);
91         char treeFlag = (1<<(4+model->getGeoInitId()));
92
93         ntlTree *tree = new ntlTree( 
94         15, 8,  // TREEwarning - fixed values for depth & maxtriangles here...
95         genscene, treeFlag );
96
97         // TODO? use params
98         ntlVec3Gfx start,end;
99         model->getExtends(start,end);
100         /*
101         printf("start - x: %f, y: %f, z: %f\n", start[0], start[1], start[2]);
102         printf("end   - x: %f, y: %f, z: %f\n", end[0], end[1], end[2]);
103         printf("mCPSWidth: %f\n");
104 */
105         LbmFloat width = mCPSWidth;
106         if(width<=LBM_EPSILON) { errMsg("ControlParticles::initFromMVMCMesh","Invalid mCPSWidth! "<<mCPSWidth); width=mCPSWidth=0.1; }
107         ntlVec3Gfx org = start+ntlVec3Gfx(width*0.5);
108         gfxReal distance = -1.;
109         vector<ntlVec3Gfx> inspos;
110         int approxmax = (int)( ((end[0]-start[0])/width)*((end[1]-start[1])/width)*((end[2]-start[2])/width) );
111
112         // printf("distance: %f, width: %f\n", distance, width);
113         
114         while(org[2]<end[2]) {
115                 while(org[1]<end[1]) {
116                         while(org[0]<end[0]) {
117                                 if(checkPointInside(tree, org, distance)) {
118                                         inspos.push_back(org);
119                                 }
120                                 // TODO optimize, use distance
121                                 org[0] += width;
122                         }
123                         org[1] += width;
124                         org[0] = start[0];
125                 }
126                 org[2] += width;
127                 org[1] = start[1];
128         }
129         
130         // printf("inspos.size(): %d\n", inspos.size());
131
132         MeanValueMeshCoords mvm;
133         mvm.calculateMVMCs(vertices,triangles, inspos, mCPSWeightFac);
134         vector<ntlVec3Gfx> ninspos;
135         mvm.transfer(vertices, ninspos);
136
137         // init first set, check dist
138         ControlParticleSet firstcps; //T
139         mPartSets.push_back(firstcps);
140         mPartSets[mPartSets.size()-1].time = (gfxReal)0.;
141         vector<bool> useCP;
142
143         for(int i=0; i<(int)inspos.size(); i++) {
144                 ControlParticle p; p.reset();
145                 p.pos = vec2L(inspos[i]);
146                 
147                 double cpdist = norm(inspos[i]-ninspos[i]);
148                 bool usecpv = true;
149
150                 mPartSets[mPartSets.size()-1].particles.push_back(p);
151                 useCP.push_back(usecpv);
152         }
153
154         // init further sets, temporal mesh sampling
155         double tsampling = mCPSTimestep;
156         // printf("tsampling: %f, ninspos.size(): %d, mCPSTimeEnd: %f\n", tsampling, ninspos.size(), mCPSTimeEnd);
157         
158         int totcnt = (int)( (mCPSTimeEnd-mCPSTimeStart)/tsampling ), tcnt=0;
159         for(double t=mCPSTimeStart+tsampling; ((t<mCPSTimeEnd) && (ninspos.size()>0.)); t+=tsampling) {
160                 ControlParticleSet nextcps; //T
161                 mPartSets.push_back(nextcps);
162                 mPartSets[mPartSets.size()-1].time = (gfxReal)t;
163
164                 vertices.clear(); triangles.clear(); normals.clear();
165                 model->getTriangles(t, &triangles, &vertices, &normals, 1 );
166                 mvm.transfer(vertices, ninspos);
167                 
168                 tcnt++;
169                 for(int i=0; i<(int)ninspos.size(); i++) {
170                         
171                         if(useCP[i]) {
172                                 ControlParticle p; p.reset();
173                                 p.pos = vec2L(ninspos[i]);
174                                 mPartSets[mPartSets.size()-1].particles.push_back(p);
175                         }
176                 }
177         }
178         
179         model->setGeoInitType(FGI_CONTROL);
180         
181         initTime(mCPSTimeStart , mCPSTimeEnd);
182
183         delete tree;
184         delete genscene;
185         delete glob;
186         
187         return 1;
188 }
189
190
191 // init all zero / defaults for a single particle
192 void ControlParticle::reset() {
193         pos = LbmVec(0.,0.,0.);
194         vel = LbmVec(0.,0.,0.);
195         influence = 1.;
196         size = 1.;
197 #ifndef LBMDIM
198 #ifdef MAIN_2D
199         rotaxis = LbmVec(0.,1.,0.); // SPH xz
200 #else // MAIN_2D
201         // 3d - roate in xy plane, vortex
202         rotaxis = LbmVec(0.,0.,1.);
203         // 3d - rotate for wave
204         //rotaxis = LbmVec(0.,1.,0.);
205 #endif // MAIN_2D
206 #else // LBMDIM
207         rotaxis = LbmVec(0.,1.,0.); // LBM xy , is swapped afterwards
208 #endif // LBMDIM
209
210         density = 0.;
211         densityWeight = 0.;
212         avgVelAcc = avgVel = LbmVec(0.);
213         avgVelWeight = 0.;
214 }
215
216
217 // default preset/empty init
218 ControlParticles::ControlParticles() :
219         _influenceTangential(0.f),
220         _influenceAttraction(0.f),
221         _influenceVelocity(0.f),
222         _influenceMaxdist(0.f),
223         _radiusAtt(1.0f),
224         _radiusVel(1.0f),
225         _radiusMinMaxd(2.0f),
226         _radiusMaxd(3.0f),
227         _currTime(-1.0), _currTimestep(1.),
228         _initTimeScale(1.), 
229         _initPartOffset(0.), _initPartScale(1.),
230         _initLastPartOffset(0.), _initLastPartScale(1.),
231         _initMirror(""),
232         _fluidSpacing(1.), _kernelWeight(-1.),
233         _charLength(1.), _charLengthInv(1.),
234         mvCPSStart(-10000.), mvCPSEnd(10000.),
235         mCPSWidth(0.1), mCPSTimestep(0.05),
236         mCPSTimeStart(0.), mCPSTimeEnd(0.5), mCPSWeightFac(1.),
237         mDebugInit(0)
238 {
239         _radiusAtt = 0.15f;
240         _radiusVel = 0.15f;
241         _radiusMinMaxd = 0.16f;
242         _radiusMaxd = 0.3;
243
244         _influenceAttraction = 0.f;
245         _influenceTangential = 0.f;
246         _influenceVelocity = 0.f;
247         // 3d tests */
248 }
249
250
251  
252 ControlParticles::~ControlParticles() {
253         // nothing to do...
254 }
255
256 LbmFloat ControlParticles::getControlTimStart() {
257         if(mPartSets.size()>0) { return mPartSets[0].time; }
258         return -1000.;
259 }
260 LbmFloat ControlParticles::getControlTimEnd() {
261         if(mPartSets.size()>0) { return mPartSets[mPartSets.size()-1].time; }
262         return -1000.;
263 }
264
265 // calculate for delta t
266 void ControlParticles::setInfluenceVelocity(LbmFloat set, LbmFloat dt) {
267         const LbmFloat dtInter = 0.01;
268         LbmFloat facFv = 1.-set; //cparts->getInfluenceVelocity();
269         // mLevel[mMaxRefine].timestep
270         LbmFloat facNv = (LbmFloat)( 1.-pow( (double)facFv, (double)(dt/dtInter)) );
271         //errMsg("vwcalc","ts:"<<dt<< " its:"<<(dt/dtInter) <<" fv"<<facFv<<" nv"<<facNv<<" test:"<< pow( (double)(1.-facNv),(double)(dtInter/dt))      );
272         _influenceVelocity = facNv;
273 }
274
275 int ControlParticles::initExampleSet()
276 {
277         // unused
278 }
279
280 int ControlParticles::getTotalSize()
281 {
282         int s=0;
283         for(int i=0; i<(int)mPartSets.size(); i++) {
284                 s+= mPartSets[i].particles.size();
285         }
286         return s;
287 }
288
289 // --------------------------------------------------------------------------
290 // load positions & timing from text file
291 // WARNING - make sure file has unix format, no win/dos linefeeds...
292 #define LINE_LEN 100
293 int ControlParticles::initFromTextFile(string filename)
294 {
295         const bool debugRead = false;
296         char line[LINE_LEN];
297         line[LINE_LEN-1] = '\0';
298         mPartSets.clear();
299         if(filename.size()<2) return 0;
300
301         // HACK , use "cparts" suffix as old
302         // e.g. "cpart2" as new
303         if(filename[ filename.size()-1 ]=='s') {
304                 return initFromTextFileOld(filename);
305         }
306
307         FILE *infile = fopen(filename.c_str(), "r");
308         if(!infile) {
309                 errMsg("ControlParticles::initFromTextFile","unable to open '"<<filename<<"' " );
310                 // try to open as gz sequence
311                 if(initFromBinaryFile(filename)) { return 1; }
312                 // try mesh MVCM generation
313                 if(initFromMVCMesh(filename)) { return 1; }
314                 // failed...
315                 return 0;
316         }
317
318         int haveNo = false;
319         int haveScale = false;
320         int haveTime = false;
321         int noParts = -1;
322         int partCnt = 0;
323         int setCnt = 0;
324         //ControlParticle p; p.reset();
325         // scale times by constant factor while reading
326         LbmFloat timeScale= 1.0;
327         int lineCnt = 0;
328         bool abortParse = false;
329 #define LASTCP mPartSets[setCnt].particles[ mPartSets[setCnt].particles.size()-1 ]
330
331         while( (!feof(infile)) && (!abortParse)) {
332                 lineCnt++;
333                 fgets(line, LINE_LEN, infile);
334
335                 //if(debugRead) printf("\nDEBUG%d r '%s'\n",lineCnt, line);
336                 if(!line) continue;
337                 int len = (int)strlen(line);
338
339                 // skip empty lines and comments (#,//)
340                 if(len<1) continue;
341                 if( (line[0]=='#') || (line[0]=='\n') ) continue;
342                 if((len>1) && (line[0]=='/' && line[1]=='/')) continue;
343
344                 // debug remove newline
345                 if((len>=1)&&(line[len-1]=='\n')) line[len-1]='\0';
346
347                 switch(line[0]) {
348
349                 case 'N': { // total number of particles, more for debugging...
350                         noParts = atoi(line+2);
351                         if(noParts<=0) {
352                                 errMsg("ControlParticles::initFromTextFile","file '"<<filename<<"' - invalid no of particles "<<noParts);
353                                 mPartSets.clear(); fclose(infile); return 0;
354                         }
355                         if(debugRead) printf("CPDEBUG%d no parts '%d'\n",lineCnt, noParts );
356                         haveNo = true;
357                         } break;
358
359                 case 'T': { // global time scale
360                         timeScale *= (LbmFloat)atof(line+2);
361                         if(debugRead) printf("ControlParticles::initFromTextFile - line %d , set timescale '%f', org %f\n",lineCnt, timeScale , _initTimeScale);
362                         if(timeScale==0.) { fprintf(stdout,"ControlParticles::initFromTextFile - line %d ,error: timescale = 0.! reseting to 1 ...\n",lineCnt); timeScale=1.; }
363                         haveScale = true;
364                         } break;
365
366                 case 'I': { // influence settings, overrides others as of now...
367                         float val = (LbmFloat)atof(line+3);
368                         const char *setvar = "[invalid]";
369                         switch(line[1]) {
370                                 //case 'f': { _influenceFalloff = val; setvar = "falloff"; } break;
371                                 case 't': { _influenceTangential = val; setvar = "tangential"; } break;
372                                 case 'a': { _influenceAttraction = val; setvar = "attraction"; } break;
373                                 case 'v': { _influenceVelocity = val; setvar = "velocity"; } break;
374                                 case 'm': { _influenceMaxdist = val; setvar = "maxdist"; } break;
375                                 default: 
376                                         fprintf(stdout,"ControlParticles::initFromTextFile (%s) - line %d , invalid influence setting %c, %f\n",filename.c_str() ,lineCnt, line[1], val);
377                         }
378                         if(debugRead) printf("CPDEBUG%d set influence '%s'=%f \n",lineCnt, setvar, val);
379                         } break;
380
381                 case 'R': { // radius settings, overrides others as of now...
382                         float val = (LbmFloat)atof(line+3);
383                         const char *setvar = "[invalid]";
384                         switch(line[1]) {
385                                 case 'a': { _radiusAtt = val; setvar = "r_attraction"; } break;
386                                 case 'v': { _radiusVel = val; setvar = "r_velocity"; } break;
387                                 case 'm': { _radiusMaxd = val; setvar = "r_maxdist"; } break;
388                                 default: 
389                                         fprintf(stdout,"ControlParticles::initFromTextFile (%s) - line %d , invalid influence setting %c, %f\n",filename.c_str() ,lineCnt, line[1], val);
390                         }
391                         if(debugRead) printf("CPDEBUG%d set influence '%s'=%f \n",lineCnt, setvar, val);
392                         } break;
393
394                 case 'S': { // new particle set at time T
395                         ControlParticleSet cps;
396                         mPartSets.push_back(cps);
397                         setCnt = (int)mPartSets.size()-1;
398
399                         LbmFloat val = (LbmFloat)atof(line+2);
400                         mPartSets[setCnt].time = val * timeScale;
401                         if(debugRead) printf("CPDEBUG%d new set, time '%f', %d\n",lineCnt, mPartSets[setCnt].time, setCnt );
402                         haveTime = true;
403                         partCnt = -1;
404                         } break;
405
406                 case 'P':   // new particle with pos
407                 case 'n': { // new particle without pos
408                                 if((!haveTime)||(setCnt<0)) { fprintf(stdout,"ControlParticles::initFromTextFile - line %d ,error: set missing!\n",lineCnt); abortParse=true; break; }
409                                 partCnt++;
410                                 if(partCnt>=noParts) {
411                                         if(debugRead) printf("CPDEBUG%d partset done \n",lineCnt);
412                                         haveTime = false;
413                                 } else {
414                                         ControlParticle p; p.reset();
415                                         mPartSets[setCnt].particles.push_back(p);
416                                 }
417                         } 
418                         // only new part, or new with pos?
419                         if(line[0] == 'n') break;
420
421                 // particle properties
422
423                 case 'p': { // new particle set at time T
424                         if((!haveTime)||(setCnt<0)||(mPartSets[setCnt].particles.size()<1)) { fprintf(stdout,"ControlParticles::initFromTextFile - line %d ,error|p: particle missing!\n",lineCnt); abortParse=true; break; }
425                         float px=0.,py=0.,pz=0.;
426                         if( sscanf(line+2,"%f %f %f",&px,&py,&pz) != 3) {
427                                 fprintf(stdout,"CPDEBUG%d, unable to parse position!\n",lineCnt); abortParse=true; break; 
428                         }
429                         if(!(finite(px)&&finite(py)&&finite(pz))) { px=py=pz=0.; }
430                         LASTCP.pos[0] = px;
431                         LASTCP.pos[1] = py;
432                         LASTCP.pos[2] = pz; 
433                         if(debugRead) printf("CPDEBUG%d part%d,%d: position %f,%f,%f \n",lineCnt,setCnt,partCnt, px,py,pz);
434                         } break;
435
436                 case 's': { // particle size
437                         if((!haveTime)||(setCnt<0)||(mPartSets[setCnt].particles.size()<1)) { fprintf(stdout,"ControlParticles::initFromTextFile - line %d ,error|s: particle missing!\n",lineCnt); abortParse=true; break; }
438                         float ps=1.;
439                         if( sscanf(line+2,"%f",&ps) != 1) {
440                                 fprintf(stdout,"CPDEBUG%d, unable to parse size!\n",lineCnt); abortParse=true; break; 
441                         }
442                         if(!(finite(ps))) { ps=0.; }
443                         LASTCP.size = ps;
444                         if(debugRead) printf("CPDEBUG%d part%d,%d: size %f \n",lineCnt,setCnt,partCnt, ps);
445                         } break;
446
447                 case 'i': { // particle influence
448                         if((!haveTime)||(setCnt<0)||(mPartSets[setCnt].particles.size()<1)) { fprintf(stdout,"ControlParticles::initFromTextFile - line %d ,error|i: particle missing!\n",lineCnt); abortParse=true; break; }
449                         float pinf=1.;
450                         if( sscanf(line+2,"%f",&pinf) != 1) {
451                                 fprintf(stdout,"CPDEBUG%d, unable to parse size!\n",lineCnt); abortParse=true; break; 
452                         }
453                         if(!(finite(pinf))) { pinf=0.; }
454                         LASTCP.influence = pinf;
455                         if(debugRead) printf("CPDEBUG%d part%d,%d: influence %f \n",lineCnt,setCnt,partCnt, pinf);
456                         } break;
457
458                 case 'a': { // rotation axis
459                         if((!haveTime)||(setCnt<0)||(mPartSets[setCnt].particles.size()<1)) { fprintf(stdout,"ControlParticles::initFromTextFile - line %d ,error|a: particle missing!\n",lineCnt); abortParse=true; break; }
460                         float px=0.,py=0.,pz=0.;
461                         if( sscanf(line+2,"%f %f %f",&px,&py,&pz) != 3) {
462                                 fprintf(stdout,"CPDEBUG%d, unable to parse rotaxis!\n",lineCnt); abortParse=true; break; 
463                         }
464                         if(!(finite(px)&&finite(py)&&finite(pz))) { px=py=pz=0.; }
465                         LASTCP.rotaxis[0] = px;
466                         LASTCP.rotaxis[1] = py;
467                         LASTCP.rotaxis[2] = pz; 
468                         if(debugRead) printf("CPDEBUG%d part%d,%d: rotaxis %f,%f,%f \n",lineCnt,setCnt,partCnt, px,py,pz);
469                         } break;
470
471
472                 default:
473                         if(debugRead) printf("CPDEBUG%d ignored: '%s'\n",lineCnt, line );
474                         break;
475                 }
476         }
477         if(debugRead && abortParse) printf("CPDEBUG aborted parsing after set... %d\n",(int)mPartSets.size() );
478
479         // sanity check
480         for(int i=0; i<(int)mPartSets.size(); i++) {
481                 if( (int)mPartSets[i].particles.size()!=noParts) {
482                         fprintf(stdout,"ControlParticles::initFromTextFile (%s) - invalid no of particles in set %d, is:%d, shouldbe:%d \n",filename.c_str() ,i,(int)mPartSets[i].particles.size(), noParts);
483                         mPartSets.clear();
484                         fclose(infile);
485                         return 0;
486                 }
487         }
488
489         // print stats
490         printf("ControlParticles::initFromTextFile (%s): Read %d sets, each %d particles\n",filename.c_str() ,
491                         (int)mPartSets.size(), noParts );
492         if(mPartSets.size()>0) {
493                 printf("ControlParticles::initFromTextFile (%s): Time: %f,%f\n",filename.c_str() ,mPartSets[0].time, mPartSets[mPartSets.size()-1].time );
494         }
495         
496         // done...
497         fclose(infile);
498         applyTrafos();
499         return 1;
500 }
501
502
503 int ControlParticles::initFromTextFileOld(string filename)
504 {
505         const bool debugRead = false;
506         char line[LINE_LEN];
507         line[LINE_LEN-1] = '\0';
508         mPartSets.clear();
509         if(filename.size()<1) return 0;
510
511         FILE *infile = fopen(filename.c_str(), "r");
512         if(!infile) {
513                 fprintf(stdout,"ControlParticles::initFromTextFileOld - unable to open '%s'\n",filename.c_str() );
514                 return 0;
515         }
516
517         int haveNo = false;
518         int haveScale = false;
519         int haveTime = false;
520         int noParts = -1;
521         int coordCnt = 0;
522         int partCnt = 0;
523         int setCnt = 0;
524         ControlParticle p; p.reset();
525         // scale times by constant factor while reading
526         LbmFloat timeScale= 1.0;
527         int lineCnt = 0;
528
529         while(!feof(infile)) {
530                 lineCnt++;
531                 fgets(line, LINE_LEN, infile);
532
533                 if(debugRead) printf("\nDEBUG%d r '%s'\n",lineCnt, line);
534
535                 if(!line) continue;
536                 int len = (int)strlen(line);
537
538                 // skip empty lines and comments (#,//)
539                 if(len<1) continue;
540                 if( (line[0]=='#') || (line[0]=='\n') ) continue;
541                 if((len>1) && (line[0]=='/' && line[1]=='/')) continue;
542
543                 // debug remove newline
544                 if((len>=1)&&(line[len-1]=='\n')) line[len-1]='\0';
545
546                 // first read no. of particles
547                 if(!haveNo) {
548                         noParts = atoi(line);
549                         if(noParts<=0) {
550                                 fprintf(stdout,"ControlParticles::initFromTextFileOld - invalid no of particles %d\n",noParts);
551                                 mPartSets.clear();
552                                 fclose(infile);
553                                 return 0;
554                         }
555                         if(debugRead) printf("DEBUG%d noparts '%d'\n",lineCnt, noParts );
556                         haveNo = true;
557                 } 
558
559                 // then read time scale
560                 else if(!haveScale) {
561                         timeScale *= (LbmFloat)atof(line);
562                         if(debugRead) printf("DEBUG%d tsc '%f', org %f\n",lineCnt, timeScale , _initTimeScale);
563                         haveScale = true;
564                 } 
565
566                 // then get set time
567                 else if(!haveTime) {
568                         ControlParticleSet cps;
569                         mPartSets.push_back(cps);
570                         setCnt = (int)mPartSets.size()-1;
571
572                         LbmFloat val = (LbmFloat)atof(line);
573                         mPartSets[setCnt].time = val * timeScale;
574                         if(debugRead) printf("DEBUG%d time '%f', %d\n",lineCnt, mPartSets[setCnt].time, setCnt );
575                         haveTime = true;
576                 }
577
578                 // default read all parts
579                 else {
580                         LbmFloat val = (LbmFloat)atof(line);
581                         if(debugRead) printf("DEBUG: l%d s%d,particle%d '%f' %d,%d/%d\n",lineCnt,(int)mPartSets.size(),(int)mPartSets[setCnt].particles.size(), val ,coordCnt,partCnt,noParts);
582                         p.pos[coordCnt] = val;
583                         coordCnt++;
584                         if(coordCnt>=3) {
585                                 mPartSets[setCnt].particles.push_back(p);
586                                 p.reset();
587                                 coordCnt=0;
588                                 partCnt++;
589                         }
590                         if(partCnt>=noParts) {
591                                 partCnt = 0;
592                                 haveTime = false;
593                         }
594                         //if(debugRead) printf("DEBUG%d par2 %d,%d/%d\n",lineCnt, coordCnt,partCnt,noParts);
595                 }
596                 //read pos, vel ...
597         }
598
599         // sanity check
600         for(int i=0; i<(int)mPartSets.size(); i++) {
601                 if( (int)mPartSets[i].particles.size()!=noParts) {
602                         fprintf(stdout,"ControlParticles::initFromTextFileOld - invalid no of particles in set %d, is:%d, shouldbe:%d \n",i,(int)mPartSets[i].particles.size(), noParts);
603                         mPartSets.clear();
604                         fclose(infile);
605                         return 0;
606                 }
607         }
608         // print stats
609         printf("ControlParticles::initFromTextFileOld: Read %d sets, each %d particles\n",
610                         (int)mPartSets.size(), noParts );
611         if(mPartSets.size()>0) {
612                 printf("ControlParticles::initFromTextFileOld: Time: %f,%f\n",mPartSets[0].time, mPartSets[mPartSets.size()-1].time );
613         }
614         
615         // done...
616         fclose(infile);
617         applyTrafos();
618         return 1;
619 }
620
621 // load positions & timing from gzipped binary file
622 int ControlParticles::initFromBinaryFile(string filename) {
623         mPartSets.clear();
624         if(filename.size()<1) return 0;
625         int fileNotFound=0;
626         int fileFound=0;
627         char ofile[256];
628
629         for(int set=0; ((set<10000)&&(fileNotFound<10)); set++) {
630                 snprintf(ofile,256,"%s%04d.gz",filename.c_str(),set);
631                 //errMsg("ControlParticle::initFromBinaryFile","set"<<set<<" notf"<<fileNotFound<<" ff"<<fileFound);
632
633                 gzFile gzf;
634                 gzf = gzopen(ofile, "rb");
635                 if (!gzf) {
636                         //errMsg("ControlParticles::initFromBinaryFile","Unable to open file for reading '"<<ofile<<"' "); 
637                         fileNotFound++;
638                         continue;
639                 }
640                 fileNotFound=0;
641                 fileFound++;
642
643                 ControlParticleSet cps;
644                 mPartSets.push_back(cps);
645                 int setCnt = (int)mPartSets.size()-1;
646                 //LbmFloat val = (LbmFloat)atof(line+2);
647                 mPartSets[setCnt].time = (gfxReal)set;
648
649                 int totpart = 0;
650                 gzread(gzf, &totpart, sizeof(totpart));
651
652                 for(int a=0; a<totpart; a++) {
653                         int ptype=0;
654                         float psize=0.0;
655                         ntlVec3Gfx ppos,pvel;
656                         gzread(gzf, &ptype, sizeof( ptype )); 
657                         gzread(gzf, &psize, sizeof( float )); 
658
659                         for(int j=0; j<3; j++) { gzread(gzf, &ppos[j], sizeof( float )); }
660                         for(int j=0; j<3; j++) { gzread(gzf, &pvel[j], sizeof( float )); }
661
662                         ControlParticle p; 
663                         p.reset();
664                         p.pos = vec2L(ppos);
665                         mPartSets[setCnt].particles.push_back(p);
666                 } 
667
668                 gzclose(gzf);
669                 //errMsg("ControlParticle::initFromBinaryFile","Read set "<<ofile<<", #"<<mPartSets[setCnt].particles.size() ); // DEBUG
670         } // sets
671
672         if(fileFound==0) return 0;
673         applyTrafos();
674         return 1;
675 }
676
677 int globCPIProblems =0;
678 bool ControlParticles::checkPointInside(ntlTree *tree, ntlVec3Gfx org, gfxReal &distance) {
679         // warning - stripped down version of geoInitCheckPointInside
680         const int globGeoInitDebug = 0;
681         const int  flags = FGI_FLUID;
682         org += ntlVec3Gfx(0.0001);
683         ntlVec3Gfx dir = ntlVec3Gfx(1.0, 0.0, 0.0);
684         int OId = -1;
685         ntlRay ray(org, dir, 0, 1.0, NULL);
686         bool done = false;
687         bool inside = false;
688         int mGiObjInside = 0; 
689         LbmFloat mGiObjDistance = -1.0; 
690         LbmFloat giObjFirstHistSide = 0; 
691         
692         // if not inside, return distance to first hit
693         gfxReal firstHit=-1.0;
694         int     firstOId = -1;
695         if(globGeoInitDebug) errMsg("IIIstart"," isect "<<org);
696
697         while(!done) {
698                 // find first inside intersection
699                 ntlTriangle *triIns = NULL;
700                 distance = -1.0;
701                 ntlVec3Gfx normal(0.0);
702                 tree->intersectX(ray,distance,normal, triIns, flags, true);
703                 if(triIns) {
704                         ntlVec3Gfx norg = ray.getOrigin() + ray.getDirection()*distance;
705                         LbmFloat orientation = dot(normal, dir);
706                         OId = triIns->getObjectId();
707                         if(orientation<=0.0) {
708                                 // outside hit
709                                 normal *= -1.0;
710                                 mGiObjInside++;
711                                 if(giObjFirstHistSide==0) giObjFirstHistSide = 1;
712                                 if(globGeoInitDebug) errMsg("IIO"," oid:"<<OId<<" org"<<org<<" norg"<<norg<<" orient:"<<orientation);
713                         } else {
714                                 // inside hit
715                                 mGiObjInside++;
716                                 if(mGiObjDistance<0.0) mGiObjDistance = distance;
717                                 if(globGeoInitDebug) errMsg("III"," oid:"<<OId<<" org"<<org<<" norg"<<norg<<" orient:"<<orientation);
718                                 if(giObjFirstHistSide==0) giObjFirstHistSide = -1;
719                         }
720                         norg += normal * getVecEpsilon();
721                         ray = ntlRay(norg, dir, 0, 1.0, NULL);
722                         // remember first hit distance, in case we're not 
723                         // inside anything
724                         if(firstHit<0.0) {
725                                 firstHit = distance;
726                                 firstOId = OId;
727                         }
728                 } else {
729                         // no more intersections... return false
730                         done = true;
731                 }
732         }
733
734         distance = -1.0;
735         if(mGiObjInside>0) {
736                 bool mess = false;
737                 if((mGiObjInside%2)==1) {
738                         if(giObjFirstHistSide != -1) mess=true;
739                 } else {
740                         if(giObjFirstHistSide !=  1) mess=true;
741                 }
742                 if(mess) {
743                         // ?
744                         //errMsg("IIIproblem","At "<<org<<" obj  inside:"<<mGiObjInside<<" firstside:"<<giObjFirstHistSide );
745                         globCPIProblems++;
746                         mGiObjInside++; // believe first hit side...
747                 }
748         }
749
750         if(globGeoInitDebug) errMsg("CHIII"," ins="<<mGiObjInside<<" t"<<mGiObjDistance<<" d"<<distance);
751         if(((mGiObjInside%2)==1)&&(mGiObjDistance>0.0)) {
752                 if(  (distance<0.0)                             || // first intersection -> good
753                                 ((distance>0.0)&&(distance>mGiObjDistance)) // more than one intersection -> use closest one
754                         ) {                                             
755                         distance = mGiObjDistance;
756                         OId = 0;
757                         inside = true;
758                 } 
759         }
760
761         if(!inside) {
762                 distance = firstHit;
763                 OId = firstOId;
764         }
765         if(globGeoInitDebug) errMsg("CHIII","ins"<<inside<<"  fh"<<firstHit<<" fo"<<firstOId<<" - h"<<distance<<" o"<<OId);
766
767         return inside;
768 }
769 int ControlParticles::initFromMVCMesh(string filename) {
770         myTime_t mvmstart = getTime(); 
771         ntlGeometryObjModel *model = new ntlGeometryObjModel();
772         int gid=1;
773         char infile[256];
774         vector<ntlTriangle> triangles;
775         vector<ntlVec3Gfx> vertices;
776         vector<ntlVec3Gfx> normals;
777         snprintf(infile,256,"%s.bobj.gz", filename.c_str() );
778         model->loadBobjModel(string(infile));
779         model->setLoaded(true);
780         model->setGeoInitId(gid);
781         model->setGeoInitType(FGI_FLUID);
782         debMsgStd("ControlParticles::initFromMVMCMesh",DM_MSG,"infile:"<<string(infile) ,4);
783
784         //getTriangles(double t,  vector<ntlTriangle> *triangles, vector<ntlVec3Gfx> *vertices, vector<ntlVec3Gfx> *normals, int objectId );
785         model->getTriangles(mCPSTimeStart, &triangles, &vertices, &normals, 1 ); 
786         debMsgStd("ControlParticles::initFromMVMCMesh",DM_MSG," tris:"<<triangles.size()<<" verts:"<<vertices.size()<<" norms:"<<normals.size() , 2);
787         
788         // valid mesh?
789         if(triangles.size() <= 0) {
790                 return 0;
791         }
792
793         ntlRenderGlobals *glob = new ntlRenderGlobals;
794         ntlScene *genscene = new ntlScene( glob, false );
795         genscene->addGeoClass(model);
796         genscene->addGeoObject(model);
797         genscene->buildScene(0., false);
798         char treeFlag = (1<<(4+gid));
799
800         ntlTree *tree = new ntlTree( 
801                         15, 8,  // TREEwarning - fixed values for depth & maxtriangles here...
802                         genscene, treeFlag );
803
804         // TODO? use params
805         ntlVec3Gfx start,end;
806         model->getExtends(start,end);
807
808         LbmFloat width = mCPSWidth;
809         if(width<=LBM_EPSILON) { errMsg("ControlParticles::initFromMVMCMesh","Invalid mCPSWidth! "<<mCPSWidth); width=mCPSWidth=0.1; }
810         ntlVec3Gfx org = start+ntlVec3Gfx(width*0.5);
811         gfxReal distance = -1.;
812         vector<ntlVec3Gfx> inspos;
813         int approxmax = (int)( ((end[0]-start[0])/width)*((end[1]-start[1])/width)*((end[2]-start[2])/width) );
814
815         debMsgStd("ControlParticles::initFromMVMCMesh",DM_MSG,"start"<<start<<" end"<<end<<" w="<<width<<" maxp:"<<approxmax, 5);
816         while(org[2]<end[2]) {
817                 while(org[1]<end[1]) {
818                         while(org[0]<end[0]) {
819                                 if(checkPointInside(tree, org, distance)) {
820                                         inspos.push_back(org);
821                                         //inspos.push_back(org+ntlVec3Gfx(width));
822                                         //inspos.push_back(start+end*0.5);
823                                 }
824                                 // TODO optimize, use distance
825                                 org[0] += width;
826                         }
827                         org[1] += width;
828                         org[0] = start[0];
829                 }
830                 org[2] += width;
831                 org[1] = start[1];
832         }
833         debMsgStd("ControlParticles::initFromMVMCMesh",DM_MSG,"points: "<<inspos.size()<<" initproblems: "<<globCPIProblems,5 );
834
835         MeanValueMeshCoords mvm;
836         mvm.calculateMVMCs(vertices,triangles, inspos, mCPSWeightFac);
837         vector<ntlVec3Gfx> ninspos;
838         mvm.transfer(vertices, ninspos);
839
840         // init first set, check dist
841         ControlParticleSet firstcps; //T
842         mPartSets.push_back(firstcps);
843         mPartSets[mPartSets.size()-1].time = (gfxReal)0.;
844         vector<bool> useCP;
845         bool debugPos=false;
846
847         for(int i=0; i<(int)inspos.size(); i++) {
848                 ControlParticle p; p.reset();
849                 p.pos = vec2L(inspos[i]);
850                 //errMsg("COMP "," "<<inspos[i]<<" vs "<<ninspos[i] );
851                 double cpdist = norm(inspos[i]-ninspos[i]);
852                 bool usecpv = true;
853                 if(debugPos) errMsg("COMP "," "<<cpdist<<usecpv);
854
855                 mPartSets[mPartSets.size()-1].particles.push_back(p);
856                 useCP.push_back(usecpv);
857         }
858
859         // init further sets, temporal mesh sampling
860         double tsampling = mCPSTimestep;
861         int totcnt = (int)( (mCPSTimeEnd-mCPSTimeStart)/tsampling ), tcnt=0;
862         for(double t=mCPSTimeStart+tsampling; ((t<mCPSTimeEnd) && (ninspos.size()>0.)); t+=tsampling) {
863                 ControlParticleSet nextcps; //T
864                 mPartSets.push_back(nextcps);
865                 mPartSets[mPartSets.size()-1].time = (gfxReal)t;
866
867                 vertices.clear(); triangles.clear(); normals.clear();
868                 model->getTriangles(t, &triangles, &vertices, &normals, 1 );
869                 mvm.transfer(vertices, ninspos);
870                 if(tcnt%(totcnt/10)==1) debMsgStd("MeanValueMeshCoords::calculateMVMCs",DM_MSG,"Transferring animation, frame: "<<tcnt<<"/"<<totcnt,5 );
871                 tcnt++;
872                 for(int i=0; i<(int)ninspos.size(); i++) {
873                         if(debugPos) errMsg("COMP "," "<<norm(inspos[i]-ninspos[i]) );
874                         if(useCP[i]) {
875                                 ControlParticle p; p.reset();
876                                 p.pos = vec2L(ninspos[i]);
877                                 mPartSets[mPartSets.size()-1].particles.push_back(p);
878                         }
879                 }
880         }
881
882         applyTrafos();
883
884         myTime_t mvmend = getTime(); 
885         debMsgStd("ControlParticle::initFromMVMCMesh",DM_MSG,"t:"<<getTimeString(mvmend-mvmstart)<<" ",7 );
886         delete tree;
887         delete genscene;
888         delete glob;
889 //exit(1); // DEBUG
890         return 1;
891 }
892
893 #define TRISWAP(v,a,b) { LbmFloat tmp = (v)[b]; (v)[b]=(v)[a]; (v)[a]=tmp; }
894 #define TRISWAPALL(v,a,b) {  \
895                         TRISWAP( (v).pos     ,a,b ); \
896                         TRISWAP( (v).vel     ,a,b ); \
897                         TRISWAP( (v).rotaxis ,a,b ); }
898
899 // helper function for LBM 2D -> swap Y and Z components everywhere
900 void ControlParticles::swapCoords(int a, int b) {
901         //return;
902         for(int i=0; i<(int)mPartSets.size(); i++) {
903                 for(int j=0; j<(int)mPartSets[i].particles.size(); j++) {
904                         TRISWAPALL( mPartSets[i].particles[j],a,b );
905                 }
906         }
907 }
908
909 // helper function for LBM 2D -> mirror time
910 void ControlParticles::mirrorTime() {
911         LbmFloat maxtime = mPartSets[mPartSets.size()-1].time;
912         const bool debugTimeswap = false;
913         
914         for(int i=0; i<(int)mPartSets.size(); i++) {
915                 mPartSets[i].time = maxtime - mPartSets[i].time;
916         }
917
918         for(int i=0; i<(int)mPartSets.size()/2; i++) {
919                 ControlParticleSet cps = mPartSets[i];
920                 if(debugTimeswap) errMsg("TIMESWAP", " s"<<i<<","<<mPartSets[i].time<<"  and s"<<(mPartSets.size()-1-i)<<","<< mPartSets[mPartSets.size()-1-i].time <<"  mt:"<<maxtime );
921                 mPartSets[i] = mPartSets[mPartSets.size()-1-i];
922                 mPartSets[mPartSets.size()-1-i] = cps;
923         }
924
925         for(int i=0; i<(int)mPartSets.size(); i++) {
926                 if(debugTimeswap) errMsg("TIMESWAP", "done: s"<<i<<","<<mPartSets[i].time<<"  "<<mPartSets[i].particles.size() );
927         }
928 }
929
930 // apply init transformations
931 void ControlParticles::applyTrafos() {
932         // apply trafos
933         for(int i=0; i<(int)mPartSets.size(); i++) {
934                 mPartSets[i].time *= _initTimeScale;
935                 /*for(int j=0; j<(int)mPartSets[i].particles.size(); j++) {
936                         for(int k=0; k<3; k++) {
937                                 mPartSets[i].particles[j].pos[k] *= _initPartScale[k];
938                                 mPartSets[i].particles[j].pos[k] += _initPartOffset[k];
939                         }
940                 } now done in initarray */
941         }
942
943         // mirror coords...
944         for(int l=0; l<(int)_initMirror.length(); l++) {
945                 switch(_initMirror[l]) {
946                 case 'X':
947                 case 'x':
948                         //printf("ControlParticles::applyTrafos - mirror x\n");
949                         swapCoords(1,2);
950                         break;
951                 case 'Y':
952                 case 'y':
953                         //printf("ControlParticles::applyTrafos - mirror y\n");
954                         swapCoords(0,2);
955                         break;
956                 case 'Z':
957                 case 'z':
958                         //printf("ControlParticles::applyTrafos - mirror z\n");
959                         swapCoords(0,1);
960                         break;
961                 case 'T':
962                 case 't':
963                         //printf("ControlParticles::applyTrafos - mirror time\n");
964                         mirrorTime();
965                         break;
966                 case ' ':
967                 case '-':
968                 case '\n':
969                         break;
970                 default:
971                         //printf("ControlParticles::applyTrafos - mirror unknown %c !?\n", _initMirror[l] );
972                         break;
973                 }
974         }
975
976         // reset 2d positions
977 #if (CP_PROJECT2D==1) && ( defined(MAIN_2D) || LBMDIM==2 )
978         for(size_t j=0; j<mPartSets.size(); j++) 
979                 for(size_t i=0; i<mPartSets[j].particles.size(); i++) {
980                         // DEBUG 
981                         mPartSets[j].particles[i].pos[1] = 0.f;
982                 }
983 #endif
984
985 #if defined(LBMDIM) 
986         //? if( (getenv("ELBEEM_CPINFILE")) || (getenv("ELBEEM_CPOUTFILE")) ){ 
987                 // gui control test, don swap...
988         //? } else {
989                 //? swapCoords(1,2); // LBM 2D -> swap Y and Z components everywhere
990         //? }
991 #endif
992
993         initTime(0.f, 0.f);
994 }
995
996 #undef TRISWAP
997
998 // --------------------------------------------------------------------------
999 // init for a given time
1000 void ControlParticles::initTime(LbmFloat t, LbmFloat dt) 
1001 {
1002         //fprintf(stdout, "CPINITTIME init %f\n",t);
1003         _currTime = t;
1004         if(mPartSets.size()<1) return;
1005
1006         // init zero velocities
1007         initTimeArray(t, _particles);
1008
1009         // calculate velocities from prev. timestep?
1010         if(dt>0.) {
1011                 _currTimestep = dt;
1012                 std::vector<ControlParticle> prevparts;
1013                 initTimeArray(t-dt, prevparts);
1014                 LbmFloat invdt = 1.0/dt;
1015                 for(size_t j=0; j<_particles.size(); j++) {
1016                         ControlParticle &p = _particles[j];
1017                         ControlParticle &prevp = prevparts[j];
1018                         for(int k=0; k<3; k++) {
1019                                 p.pos[k] *= _initPartScale[k];
1020                                 p.pos[k] += _initPartOffset[k];
1021                                 prevp.pos[k] *= _initLastPartScale[k];
1022                                 prevp.pos[k] += _initLastPartOffset[k];
1023                         }
1024                         p.vel = (p.pos - prevp.pos)*invdt;
1025                 }
1026
1027                 if(0) {
1028                         LbmVec avgvel(0.);
1029                         for(size_t j=0; j<_particles.size(); j++) {
1030                                 avgvel += _particles[j].vel;
1031                         }
1032                         avgvel /= (LbmFloat)_particles.size();
1033                         //fprintf(stdout," AVGVEL %f,%f,%f \n",avgvel[0],avgvel[1],avgvel[2]); // DEBUG
1034                 }
1035         }
1036 }
1037
1038 // helper, init given array
1039 void ControlParticles::initTimeArray(LbmFloat t, std::vector<ControlParticle> &parts) {
1040         if(mPartSets.size()<1) return;
1041
1042         if(parts.size()!=mPartSets[0].particles.size()) {
1043                 //fprintf(stdout,"PRES \n");
1044                 parts.resize(mPartSets[0].particles.size());
1045                 // TODO reset all?
1046                 for(size_t j=0; j<parts.size(); j++) {
1047                         parts[j].reset();
1048                 }
1049         }
1050         if(parts.size()<1) return;
1051
1052         // debug inits
1053         if(mDebugInit==1) {
1054                 // hard coded circle init
1055                 for(size_t j=0; j<mPartSets[0].particles.size(); j++) {
1056                         ControlParticle p = mPartSets[0].particles[j];
1057                         // remember old
1058                         p.density = parts[j].density;
1059                         p.densityWeight = parts[j].densityWeight;
1060                         p.avgVel = parts[j].avgVel;
1061                         p.avgVelAcc = parts[j].avgVelAcc;
1062                         p.avgVelWeight = parts[j].avgVelWeight;
1063                         LbmVec ppos(0.); { // DEBUG
1064                         const float tscale=10.;
1065                         const float tprevo = 0.33;
1066                         const LbmVec toff(50,50,0);
1067                         const LbmVec oscale(30,30,0);
1068                         ppos[0] =  cos(tscale* t - tprevo*(float)j + M_PI -0.1) * oscale[0] + toff[0];
1069                         ppos[1] = -sin(tscale* t - tprevo*(float)j + M_PI -0.1) * oscale[1] + toff[1];
1070                         ppos[2] =                               toff[2]; } // DEBUG
1071                         p.pos = ppos;
1072                         parts[j] = p;
1073                         //errMsg("ControlParticle::initTimeArray","j:"<<j<<" p:"<<parts[j].pos );
1074                 }
1075                 return;
1076         }
1077         else if(mDebugInit==2) {
1078                 // hard coded spiral init
1079                 const float tscale=-10.;
1080                 const float tprevo = 0.33;
1081                 LbmVec   toff(50,0,-50);
1082                 const LbmVec oscale(20,20,0);
1083                 toff[2] += 30. * t +30.;
1084                 for(size_t j=0; j<mPartSets[0].particles.size(); j++) {
1085                         ControlParticle p = mPartSets[0].particles[j];
1086                         // remember old
1087                         p.density = parts[j].density;
1088                         p.densityWeight = parts[j].densityWeight;
1089                         p.avgVel = parts[j].avgVel;
1090                         p.avgVelAcc = parts[j].avgVelAcc;
1091                         p.avgVelWeight = parts[j].avgVelWeight;
1092                         LbmVec ppos(0.); 
1093                         ppos[1] =                               toff[2]; 
1094                         LbmFloat zscal = (ppos[1]+100.)/200.;
1095                         ppos[0] =  cos(tscale* t - tprevo*(float)j + M_PI -0.1) * oscale[0]*zscal + toff[0];
1096                         ppos[2] = -sin(tscale* t - tprevo*(float)j + M_PI -0.1) * oscale[1]*zscal + toff[1];
1097                         p.pos = ppos;
1098                         parts[j] = p;
1099
1100                         toff[2] += 0.25;
1101                 }
1102                 return;
1103         }
1104
1105         // use first set
1106         if((t<=mPartSets[0].time)||(mPartSets.size()==1)) {
1107                 //fprintf(stdout,"PINI %f \n", t);
1108                 //parts = mPartSets[0].particles;
1109                 const int i=0;
1110                 for(size_t j=0; j<mPartSets[i].particles.size(); j++) {
1111                         ControlParticle p = mPartSets[i].particles[j];
1112                         // remember old
1113                         p.density = parts[j].density;
1114                         p.densityWeight = parts[j].densityWeight;
1115                         p.avgVel = parts[j].avgVel;
1116                         p.avgVelAcc = parts[j].avgVelAcc;
1117                         p.avgVelWeight = parts[j].avgVelWeight;
1118                         parts[j] = p;
1119                 }
1120                 return;
1121         }
1122
1123         for(int i=0; i<(int)mPartSets.size()-1; i++) {
1124                 if((mPartSets[i].time<=t) && (mPartSets[i+1].time>t)) {
1125                         LbmFloat d = mPartSets[i+1].time-mPartSets[i].time;
1126                         LbmFloat f = (t-mPartSets[i].time)/d;
1127                         LbmFloat omf = 1.0f - f;
1128         
1129                         for(size_t j=0; j<mPartSets[i].particles.size(); j++) {
1130                                 ControlParticle *src1=&mPartSets[i  ].particles[j];
1131                                 ControlParticle *src2=&mPartSets[i+1].particles[j];
1132                                 ControlParticle &p = parts[j];
1133                                 // do linear interpolation
1134                                 p.pos     = src1->pos * omf     + src2->pos *f;
1135                                 p.vel     = LbmVec(0.); // reset, calculated later on src1->vel * omf     + src2->vel *f;
1136                                 p.rotaxis = src1->rotaxis * omf + src2->rotaxis *f;
1137                                 p.influence = src1->influence * omf + src2->influence *f;
1138                                 p.size    = src1->size * omf    + src2->size *f;
1139                                 // dont modify: density, densityWeight
1140                         }
1141                 }
1142         }
1143
1144         // after last?
1145         if(t>=mPartSets[ mPartSets.size() -1 ].time) {
1146                 //parts = mPartSets[ mPartSets.size() -1 ].particles;
1147                 const int i= (int)mPartSets.size() -1;
1148                 for(size_t j=0; j<mPartSets[i].particles.size(); j++) {
1149                         ControlParticle p = mPartSets[i].particles[j];
1150                         // restore
1151                         p.density = parts[j].density;
1152                         p.densityWeight = parts[j].densityWeight;
1153                         p.avgVel = parts[j].avgVel;
1154                         p.avgVelAcc = parts[j].avgVelAcc;
1155                         p.avgVelWeight = parts[j].avgVelWeight;
1156                         parts[j] = p;
1157                 }
1158         }
1159 }
1160
1161
1162
1163
1164 // --------------------------------------------------------------------------
1165
1166 #define DEBUG_MODVEL 0
1167
1168 // recalculate 
1169 void ControlParticles::calculateKernelWeight() {
1170         const bool debugKernel = true;
1171
1172         // calculate kernel area with respect to particlesize/cellsize
1173         LbmFloat kernelw = -1.;
1174         LbmFloat kernelnorm = -1.;
1175         LbmFloat krad = (_radiusAtt*0.75); // FIXME  use real cone approximation...?
1176         //krad = (_influenceFalloff*1.);
1177 #if (CP_PROJECT2D==1) && (defined(MAIN_2D) || LBMDIM==2)
1178         kernelw = CP_PI*krad*krad;
1179         kernelnorm = 1.0 / (_fluidSpacing * _fluidSpacing);
1180 #else // 2D
1181         kernelw = CP_PI*krad*krad*krad* (4./3.);
1182         kernelnorm = 1.0 / (_fluidSpacing * _fluidSpacing * _fluidSpacing);
1183 #endif // MAIN_2D
1184
1185         if(debugKernel) debMsgStd("ControlParticles::calculateKernelWeight",DM_MSG,"kw"<<kernelw<<", norm"<<
1186                         kernelnorm<<", w*n="<<(kernelw*kernelnorm)<<", rad"<<krad<<", sp"<<_fluidSpacing<<"  ", 7);
1187         LbmFloat kernelws = kernelw*kernelnorm;
1188         _kernelWeight = kernelws;
1189         if(debugKernel) debMsgStd("ControlParticles::calculateKernelWeight",DM_MSG,"influence f="<<_radiusAtt<<" t="<<
1190                         _influenceTangential<<" a="<<_influenceAttraction<<" v="<<_influenceVelocity<<" kweight="<<_kernelWeight, 7);
1191         if(_kernelWeight<=0.) {
1192                 errMsg("ControlParticles::calculateKernelWeight", "invalid kernel! "<<_kernelWeight<<", resetting");
1193                 _kernelWeight = 1.;
1194         }
1195 }
1196
1197 void 
1198 ControlParticles::prepareControl(LbmFloat simtime, LbmFloat dt, ControlParticles *motion) {
1199         debMsgStd("ControlParticle::prepareControl",DM_MSG," simtime="<<simtime<<" dt="<<dt<<" ", 5);
1200
1201         //fprintf(stdout,"PREPARE \n");
1202         LbmFloat avgdw = 0.;
1203         for(size_t i=0; i<_particles.size(); i++) {
1204                 ControlParticle *cp = &_particles[i];
1205
1206                 if(this->getInfluenceAttraction()<0.) {
1207                         cp->density= 
1208                         cp->densityWeight = 1.0;
1209                         continue;
1210                 } 
1211
1212                 // normalize by kernel 
1213                 //cp->densityWeight = (1.0 - (cp->density / _kernelWeight)); // store last
1214 #if (CP_PROJECT2D==1) && (defined(MAIN_2D) || LBMDIM==2)
1215                 cp->densityWeight = (1.0 - (cp->density / (_kernelWeight*cp->size*cp->size) )); // store last
1216 #else // 2D
1217                 cp->densityWeight = (1.0 - (cp->density / (_kernelWeight*cp->size*cp->size*cp->size) )); // store last
1218 #endif // MAIN_2D
1219
1220                 if(i<10) debMsgStd("ControlParticle::prepareControl",DM_MSG,"kernelDebug i="<<i<<" densWei="<<cp->densityWeight<<" 1/kw"<<(1.0/_kernelWeight)<<" cpdensity="<<cp->density, 9 );
1221                 if(cp->densityWeight<0.) cp->densityWeight=0.;
1222                 if(cp->densityWeight>1.) cp->densityWeight=1.;
1223                 
1224                 avgdw += cp->densityWeight;
1225                 // reset for next step
1226                 cp->density = 0.; 
1227
1228                 if(cp->avgVelWeight>0.) {
1229                  cp->avgVel     = cp->avgVelAcc/cp->avgVelWeight; 
1230                  cp->avgVelWeight       = 0.; 
1231                  cp->avgVelAcc  = LbmVec(0.,0.,0.); 
1232                 }
1233         }
1234         //if(debugKernel) for(size_t i=0; i<_particles.size(); i++) { ControlParticle *cp = &_particles[i]; fprintf(stdout,"A %f,%f \n",cp->density,cp->densityWeight); }
1235         avgdw /= (LbmFloat)(_particles.size());
1236         //if(motion) { printf("ControlParticle::kernel: avgdw:%f,  kw%f, sp%f \n", avgdw, _kernelWeight, _fluidSpacing); }
1237         
1238         //if((simtime>=0.) && (simtime != _currTime)) 
1239         initTime(simtime, dt);
1240
1241         if((motion) && (motion->getSize()>0)){
1242                 ControlParticle *motionp = motion->getParticle(0);
1243                 //printf("ControlParticle::prepareControl motion: pos[%f,%f,%f] vel[%f,%f,%f] \n", motionp->pos[0], motionp->pos[1], motionp->pos[2], motionp->vel[0], motionp->vel[1], motionp->vel[2] );
1244                 for(size_t i=0; i<_particles.size(); i++) {
1245                         ControlParticle *cp = &_particles[i];
1246                         cp->pos = cp->pos + motionp->pos;
1247                         cp->vel = cp->vel + motionp->vel;
1248                         cp->size = cp->size * motionp->size;
1249                         cp->influence = cp->size * motionp->influence;
1250                 }
1251         }
1252         
1253         // reset to radiusAtt by default
1254         if(_radiusVel==0.) _radiusVel = _radiusAtt;
1255         if(_radiusMinMaxd==0.) _radiusMinMaxd = _radiusAtt;
1256         if(_radiusMaxd==0.) _radiusMaxd = 2.*_radiusAtt;
1257         // has to be radiusVel<radiusAtt<radiusMinMaxd<radiusMaxd
1258         if(_radiusVel>_radiusAtt) _radiusVel = _radiusAtt;
1259         if(_radiusAtt>_radiusMinMaxd) _radiusAtt = _radiusMinMaxd;
1260         if(_radiusMinMaxd>_radiusMaxd) _radiusMinMaxd = _radiusMaxd;
1261
1262         //printf("ControlParticle::radii vel:%f att:%f min:%f max:%f \n", _radiusVel,_radiusAtt,_radiusMinMaxd,_radiusMaxd);
1263         // prepareControl done
1264 }
1265
1266 void ControlParticles::finishControl(std::vector<ControlForces> &forces, LbmFloat iatt, LbmFloat ivel, LbmFloat imaxd) {
1267
1268         //const LbmFloat iatt  = this->getInfluenceAttraction() * this->getCurrTimestep();
1269         //const LbmFloat ivel  = this->getInfluenceVelocity();
1270         //const LbmFloat imaxd = this->getInfluenceMaxdist() * this->getCurrTimestep();
1271         // prepare for usage
1272         iatt  *= this->getCurrTimestep();
1273         ivel  *= 1.; // not necessary!
1274         imaxd *= this->getCurrTimestep();
1275
1276         // skip when size=0
1277         for(int i=0; i<(int)forces.size(); i++) {
1278                 if(DEBUG_MODVEL) fprintf(stdout, "CPFORGF %d , wf:%f,f:%f,%f,%f  ,  v:%f,%f,%f   \n",i, forces[i].weightAtt, forces[i].forceAtt[0],forces[i].forceAtt[1],forces[i].forceAtt[2],   forces[i].forceVel[0], forces[i].forceVel[1], forces[i].forceVel[2] );
1279                 LbmFloat cfweight = forces[i].weightAtt; // always normalize
1280                 if((cfweight!=0.)&&(iatt!=0.)) {
1281                         // multiple kernels, normalize - note this does not normalize in d>r/2 region
1282                         if(ABS(cfweight)>1.) { cfweight = 1.0/cfweight; }
1283                         // multiply iatt afterwards to allow stronger force
1284                         cfweight *= iatt;
1285                         forces[i].forceAtt *= cfweight;
1286                 } else {
1287                         forces[i].weightAtt =  0.;
1288                         forces[i].forceAtt = LbmVec(0.);
1289                 }
1290
1291                 if( (cfweight==0.) && (imaxd>0.) && (forces[i].maxDistance>0.) ) {
1292                         forces[i].forceMaxd *= imaxd;
1293                 } else {
1294                         forces[i].maxDistance=  0.;
1295                         forces[i].forceMaxd = LbmVec(0.);
1296                 }
1297
1298                 LbmFloat cvweight = forces[i].weightVel; // always normalize
1299                 if(cvweight>0.) {
1300                         forces[i].forceVel /= cvweight;
1301                         forces[i].compAv /= cvweight;
1302                         // now modify cvweight, and write back
1303                         // important, cut at 1 - otherwise strong vel. influences...
1304                         if(cvweight>1.) { cvweight = 1.; }
1305                         // thus cvweight is in the range of 0..influenceVelocity, currently not normalized by numCParts
1306                         cvweight *= ivel;
1307                         if(cvweight<0.) cvweight=0.; if(cvweight>1.) cvweight=1.;
1308                         // LBM, FIXME todo use relaxation factor
1309                         //pvel = (cvel*0.5 * cvweight) + (pvel * (1.0-cvweight)); 
1310                         forces[i].weightVel = cvweight;
1311
1312                         //errMsg("COMPAV","i"<<i<<" compav"<<forces[i].compAv<<" forcevel"<<forces[i].forceVel<<" ");
1313                 } else {
1314                         forces[i].weightVel = 0.;
1315                         if(forces[i].maxDistance==0.) forces[i].forceVel = LbmVec(0.);
1316                         forces[i].compAvWeight = 0.;
1317                         forces[i].compAv = LbmVec(0.);
1318                 }
1319                 if(DEBUG_MODVEL) fprintf(stdout, "CPFINIF %d , wf:%f,f:%f,%f,%f  ,  v:%f,%f,%f   \n",i, forces[i].weightAtt, forces[i].forceAtt[0],forces[i].forceAtt[1],forces[i].forceAtt[2],  forces[i].forceVel[0],forces[i].forceVel[1],forces[i].forceVel[2] );
1320         }
1321
1322         // unused...
1323         if(DEBUG_MODVEL) fprintf(stdout,"MFC iatt:%f,%f ivel:%f,%f ifmd:%f,%f \n", iatt,_radiusAtt, ivel,_radiusVel, imaxd, _radiusMaxd);
1324         //for(size_t i=0; i<_particles.size(); i++) { ControlParticle *cp = &_particles[i]; fprintf(stdout," %f,%f,%f ",cp->density,cp->densityWeight, (1.0 - (12.0*cp->densityWeight))); }
1325         //fprintf(stdout,"\n\nCP DONE \n\n\n");
1326 }
1327
1328
1329 // --------------------------------------------------------------------------
1330 // calculate forces at given position, and modify velocity
1331 // according to timestep
1332 void ControlParticles::calculateCpInfluenceOpt(ControlParticle *cp, LbmVec fluidpos, LbmVec fluidvel, ControlForces *force, LbmFloat fillFactor) {
1333         // dont reset, only add...
1334         // test distance, simple squared distance reject
1335         const LbmFloat cpfo = _radiusAtt*cp->size;
1336
1337         LbmVec posDelta;
1338         if(DEBUG_MODVEL) fprintf(stdout, "CP at %f,%f,%f bef fw:%f, f:%f,%f,%f  , vw:%f, v:%f,%f,%f   \n",fluidpos[0],fluidpos[1],fluidpos[2], force->weightAtt, force->forceAtt[0], force->forceAtt[1], force->forceAtt[2], force->weightVel, force->forceVel[0], force->forceVel[1], force->forceVel[2]);
1339         posDelta        = cp->pos - fluidpos;
1340 #if LBMDIM==2 && (CP_PROJECT2D==1)
1341         posDelta[2] = 0.; // project to xy plane, z-velocity should already be gone...
1342 #endif
1343
1344         const LbmFloat distsqr = posDelta[0]*posDelta[0]+posDelta[1]*posDelta[1]+posDelta[2]*posDelta[2];
1345         if(DEBUG_MODVEL) fprintf(stdout, " Pd at %f,%f,%f d%f   \n",posDelta[0],posDelta[1],posDelta[2], distsqr);
1346         // cut at influence=0.5 , scaling not really makes sense
1347         if(cpfo*cpfo < distsqr) {
1348                 /*if(cp->influence>0.5) {
1349                         if(force->weightAtt == 0.) {
1350                                 if(force->maxDistance*force->maxDistance > distsqr) {
1351                                 const LbmFloat dis = sqrtf((float)distsqr);
1352                                 const LbmFloat sc = dis-cpfo;
1353                                 force->maxDistance = dis;
1354                                 force->forceMaxd = (posDelta)*(sc/dis);
1355                                 }
1356                         } } */
1357                 return;
1358         }
1359         force->weightAtt += 1e-6; // for distance
1360         force->maxDistance = 0.; // necessary for SPH?
1361
1362         const LbmFloat pdistance = MAGNITUDE(posDelta);
1363         LbmFloat pdistinv = 0.;
1364         if(ABS(pdistance)>0.) pdistinv = 1./pdistance;
1365         posDelta *= pdistinv;
1366
1367         LbmFloat falloffAtt = 0.; //CPKernel::kernel(cpfo * 1.0, pdistance);
1368         const LbmFloat qac = pdistance / cpfo ;
1369         if (qac < 1.0){ // return 0.;
1370                 if(qac < 0.5) falloffAtt =  1.0f;
1371                 else         falloffAtt = (1.0f - qac) * 2.0f;
1372         }
1373
1374         // vorticity force:
1375         // - //LbmVec forceVort; 
1376         // - //CROSS(forceVort, posDelta, cp->rotaxis);
1377         // - //NORMALIZE(forceVort);
1378         // - if(falloffAtt>1.0) falloffAtt=1.0;
1379
1380 #if (CP_PROJECT2D==1) && (defined(MAIN_2D) || LBMDIM==2)
1381         // fillFactor *= 2.0 *0.75 * pdistance; // 2d>3d sampling
1382 #endif // (CP_PROJECT2D==1) && (defined(MAIN_2D) || LBMDIM==2)
1383         cp->density += falloffAtt * fillFactor;
1384         force->forceAtt += posDelta *cp->densityWeight *cp->influence; 
1385         force->weightAtt += falloffAtt*cp->densityWeight *cp->influence;
1386         
1387         LbmFloat falloffVel = 0.; //CPKernel::kernel(cpfo * 1.0, pdistance);
1388         const LbmFloat cpfv = _radiusVel*cp->size;
1389         if(cpfv*cpfv < distsqr) { return; }
1390         const LbmFloat qvc = pdistance / cpfo ;
1391         //if (qvc < 1.0){ 
1392                 //if(qvc < 0.5) falloffVel =  1.0f;
1393                 //else         falloffVel = (1.0f - qvc) * 2.0f;
1394         //}
1395         falloffVel = 1.-qvc;
1396
1397         LbmFloat pvWeight; // = (1.0-cp->densityWeight) * _currTimestep * falloffVel;
1398         pvWeight = falloffVel *cp->influence; // std, without density influence
1399         //pvWeight *= (1.0-cp->densityWeight); // use inverse density weight
1400         //pvWeight *=      cp->densityWeight; // test, use density weight
1401         LbmVec modvel(0.);
1402         modvel += cp->vel * pvWeight;
1403         //pvWeight = 1.; modvel = partVel; // DEBUG!?
1404
1405         if(pvWeight>0.) {
1406                 force->forceVel += modvel;
1407                 force->weightVel += pvWeight;
1408
1409                 cp->avgVelWeight += falloffVel;
1410                 cp->avgVel += fluidvel;
1411         } 
1412         if(DEBUG_MODVEL) fprintf(stdout, "CP at %f,%f,%f aft fw:%f, f:%f,%f,%f  , vw:%f, v:%f,%f,%f   \n",fluidpos[0],fluidpos[1],fluidpos[2], force->weightAtt, force->forceAtt[0], force->forceAtt[1], force->forceAtt[2], force->weightVel, force->forceVel[0], force->forceVel[1], force->forceVel[2]);
1413         return;
1414 }
1415
1416 void ControlParticles::calculateMaxdForce(ControlParticle *cp, LbmVec fluidpos, ControlForces *force) {
1417         if(force->weightAtt != 0.) return; // maxd force off
1418         if(cp->influence <= 0.5) return;   // ignore
1419
1420         LbmVec posDelta;
1421         //if(DEBUG_MODVEL) fprintf(stdout, "CP at %f,%f,%f bef fw:%f, f:%f,%f,%f  , vw:%f, v:%f,%f,%f   \n",fluidpos[0],fluidpos[1],fluidpos[2], force->weightAtt, force->forceAtt[0], force->forceAtt[1], force->forceAtt[2], force->weightVel, force->forceVel[0], force->forceVel[1], force->forceVel[2]);
1422         posDelta        = cp->pos - fluidpos;
1423 #if LBMDIM==2 && (CP_PROJECT2D==1)
1424         posDelta[2] = 0.; // project to xy plane, z-velocity should already be gone...
1425 #endif
1426
1427         // dont reset, only add...
1428         // test distance, simple squared distance reject
1429         const LbmFloat distsqr = posDelta[0]*posDelta[0]+posDelta[1]*posDelta[1]+posDelta[2]*posDelta[2];
1430         
1431         // closer cp found
1432         if(force->maxDistance*force->maxDistance < distsqr) return;
1433         
1434         const LbmFloat dmin = _radiusMinMaxd*cp->size;
1435         if(distsqr<dmin*dmin) return; // inside min
1436         const LbmFloat dmax = _radiusMaxd*cp->size;
1437         if(distsqr>dmax*dmax) return; // outside
1438
1439
1440         if(DEBUG_MODVEL) fprintf(stdout, " Pd at %f,%f,%f d%f   \n",posDelta[0],posDelta[1],posDelta[2], distsqr);
1441         // cut at influence=0.5 , scaling not really makes sense
1442         const LbmFloat dis = sqrtf((float)distsqr);
1443         //const LbmFloat sc = dis - dmin;
1444         const LbmFloat sc = (dis-dmin)/(dmax-dmin); // scale from 0-1
1445         force->maxDistance = dis;
1446         force->forceMaxd = (posDelta/dis) * sc;
1447         //debug errMsg("calculateMaxdForce","pos"<<fluidpos<<" dis"<<dis<<" sc"<<sc<<" dmin"<<dmin<<" maxd"<< force->maxDistance <<" fmd"<<force->forceMaxd );
1448         return;
1449 }
1450