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