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