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