ArbitraryDomain.cpp 23.1 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13
// ------------------------------------------------------------------------
// $Version: 1.2.1 $
// ------------------------------------------------------------------------
// Copyright & License: See Copyright.readme in src directory
// ------------------------------------------------------------------------
// Class ArbitraryDomain
//   Interface to iterative solver and boundary geometry 
//   for space charge calculation
//
// ------------------------------------------------------------------------
// $Author: kaman $
// $Date: 2014 $
// ------------------------------------------------------------------------
14
//#define DEBUG_INTERSECT_RAY_BOUNDARY
15

16
#ifdef HAVE_SAAMG_SOLVER
17 18 19 20
#include <map>
#include <cmath>
#include <iostream>
#include <assert.h>
gsell's avatar
gsell committed
21

22 23 24 25
#include <math.h>
#define MIN2(a,b) (((a) < (b)) ? (a) : (b))
#define MAX2(a,b) (((a) > (b)) ? (a) : (b))

gsell's avatar
gsell committed
26
#include "ArbitraryDomain.h"
27

28 29 30 31 32 33 34
ArbitraryDomain::ArbitraryDomain(
	BoundaryGeometry * bgeom, 
	Vector_t nr, 
	Vector_t hr, 
	std::string interpl) {

    	bgeom_m  = bgeom;
35 36
    	intersectMinCoords_m = bgeom->getmincoords();
    	intersectMaxCoords_m = bgeom->getmaxcoords();
37

38 39
    	setNr(nr);
    	setHr(hr);
40

41 42 43 44 45 46 47 48
   	startId = 0;

	if(interpl == "CONSTANT")
        	interpolationMethod = CONSTANT;
	else if(interpl == "LINEAR")
        	interpolationMethod = LINEAR;
	else if(interpl == "QUADRATIC")
        	interpolationMethod = QUADRATIC;
gsell's avatar
gsell committed
49 50
}

51 52
ArbitraryDomain::~ArbitraryDomain() {
    //nothing so far
gsell's avatar
gsell committed
53 54
}

55 56 57 58 59 60
void ArbitraryDomain::Compute(Vector_t hr) {

    setHr(hr);
}


61
void ArbitraryDomain::Compute(Vector_t hr, NDIndex<3> localId) {
gsell's avatar
gsell committed
62 63

    setHr(hr);
64
    setNr(nr);
65 66 67 68 69 70 71
    int zGhostOffsetLeft  = (localId[2].first()== 0) ? 0 : 1;
    int zGhostOffsetRight = (localId[2].last() == nr[2] - 1) ? 0 : 1;
    int yGhostOffsetLeft  = (localId[1].first()== 0) ? 0 : 1;
    int yGhostOffsetRight = (localId[1].last() == nr[1] - 1) ? 0 : 1;
    int xGhostOffsetLeft  = (localId[0].first()== 0) ? 0 : 1;
    int xGhostOffsetRight = (localId[0].last() == nr[0] - 1) ? 0 : 1;
    
72 73
    hasGeometryChanged_m = true;

74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128
    IntersectLoX.clear();
    IntersectHiX.clear();
    IntersectLoY.clear();
    IntersectHiY.clear();
    IntersectLoZ.clear();
    IntersectHiZ.clear();


    //calculate intersection 
    Vector_t P, dir, I;
    for (int idz = localId[2].first()-zGhostOffsetLeft; idz <= localId[2].last()+zGhostOffsetRight; idz++) {
	 P[2] = (idz - (nr[2]-1)/2.0)*hr[2];

	 for (int idy = localId[1].first()-yGhostOffsetLeft; idy <= localId[1].last()+yGhostOffsetRight; idy++) {
	     P[1] = (idy - (nr[1]-1)/2.0)*hr[1];

    	     for (int idx = localId[0].first()-xGhostOffsetLeft; idx <= localId[0].last()+xGhostOffsetRight; idx++) {
	       	  P[0] = (idx - (nr[0]-1)/2.0)*hr[0];

       			std::tuple<int, int, int> pos(idx, idy, idz);

	        	dir = Vector_t(0,0,1);
		        if (bgeom_m->intersectRayBoundary(P, dir, I))
       	      		 IntersectHiZ.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[2]));

	        	dir = Vector_t(0,0,-1);
		        if (bgeom_m->intersectRayBoundary(P, dir, I))
       	      		 IntersectLoZ.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[2]));

	        	dir = Vector_t(0,1,0);
		        if (bgeom_m->intersectRayBoundary(P, dir, I))
       	      		 IntersectHiY.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[1]));

	        	dir = Vector_t(0,-1,0);
		        if (bgeom_m->intersectRayBoundary(P, dir, I))
       	      		 IntersectLoY.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[1]));

	        	dir = Vector_t(1,0,0);
		        if (bgeom_m->intersectRayBoundary(P, dir, I))
       	      		 IntersectHiX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));

	        	dir = Vector_t(-1,0,0);
		        if (bgeom_m->intersectRayBoundary(P, dir, I))
       	      		 IntersectLoX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		}
	 }
     }

    //number of ghost nodes to the right 
    int znumGhostNodesRight = 0;
    if(zGhostOffsetRight != 0) {
        for(int idx = localId[0].first(); idx <= localId[0].last(); idx++) {
            for(int idy = localId[1].first(); idy <= localId[1].last(); idy++) {
                if(isInside(idx, idy, localId[2].last() + zGhostOffsetRight))
                    znumGhostNodesRight++;
gsell's avatar
gsell committed
129 130
            }
        }
131
    }
gsell's avatar
gsell committed
132

133 134 135 136 137 138 139
    //number of ghost nodes to the left 
    int znumGhostNodesLeft = 0;
    if(zGhostOffsetLeft != 0) {
        for(int idx = localId[0].first(); idx <= localId[0].last(); idx++) {
            for(int idy = localId[1].first(); idy <= localId[1].last(); idy++) {
                if(isInside(idx, idy, localId[2].first() - zGhostOffsetLeft))
                    znumGhostNodesLeft++;
gsell's avatar
gsell committed
140 141
            }
        }
142
    }
gsell's avatar
gsell committed
143

144 145 146 147 148 149 150
    //number of ghost nodes to the right 
    int ynumGhostNodesRight = 0;
    if(yGhostOffsetRight != 0) {
        for(int idx = localId[0].first(); idx <= localId[0].last(); idx++) {
            for(int idz = localId[2].first(); idz <= localId[2].last(); idz++) {
                if(isInside(idx, localId[1].last() + yGhostOffsetRight, idz))
                    ynumGhostNodesRight++;
gsell's avatar
gsell committed
151 152 153 154
            }
        }
    }

155 156 157 158 159 160 161 162 163 164
    //number of ghost nodes to the left 
    int ynumGhostNodesLeft = 0;
    if(yGhostOffsetLeft != 0) {
        for(int idx = localId[0].first(); idx <= localId[0].last(); idx++) {
            for(int idz = localId[2].first(); idz <= localId[2].last(); idz++) {
                if(isInside(idx, localId[1].first() - yGhostOffsetLeft, idz))
                    ynumGhostNodesLeft++;
            }
        }
    }
gsell's avatar
gsell committed
165 166


167 168 169 170 171 172 173
    //number of ghost nodes to the right 
    int xnumGhostNodesRight = 0;
    if(xGhostOffsetRight != 0) {
	for (int idy = localId[1].first(); idy <= localId[1].last(); idy++) {
            for (int idz = localId[2].first(); idz <= localId[2].last(); idz++) {
                if(isInside(localId[0].last() + xGhostOffsetRight, idy, idz))
                    xnumGhostNodesRight++;
gsell's avatar
gsell committed
174 175 176 177
            }
        }
    }

178 179 180 181 182 183 184 185 186 187
    //number of ghost nodes to the left 
    int xnumGhostNodesLeft = 0;
    if(xGhostOffsetLeft != 0) {
       	for (int idy = localId[1].first(); idy <= localId[1].last(); idy++) {
            for(int idz = localId[2].first(); idz <= localId[2].last(); idz++) {
                if(isInside(localId[0].first() - xGhostOffsetLeft, idy, idz))
                    xnumGhostNodesLeft++;
            }
        }
    }
gsell's avatar
gsell committed
188
    //xy points in z plane
189 190
    int numxy; 
    int numtotalxy = 0;
gsell's avatar
gsell committed
191

192
    numXY.clear();
gsell's avatar
gsell committed
193

194 195 196 197 198 199
    for (int idz = localId[2].first(); idz <= localId[2].last(); idz++) {
	numxy =0;
        for (int idy = localId[1].first(); idy <= localId[1].last(); idy++) {
            for (int idx =localId[0].first(); idx <= localId[0].last(); idx++) {
                if (isInside(idx, idy, idz))
                   numxy++;
gsell's avatar
gsell committed
200 201
            }
        }
202
        numtotalxy += numxy;
gsell's avatar
gsell committed
203 204
    }

205 206
    startId = 0;
    MPI_Scan(&numtotalxy, &startId, 1, MPI_INTEGER, MPI_SUM, Ippl::getComm());
gsell's avatar
gsell committed
207

208
    startId -= numtotalxy;
gsell's avatar
gsell committed
209 210 211 212

    //build up index and coord map
    IdxMap.clear();
    CoordMap.clear();
213 214 215 216 217 218 219 220 221 222 223 224 225
    
    register int id = startId - xnumGhostNodesLeft - ynumGhostNodesLeft - znumGhostNodesLeft;
     for (int idz = localId[2].first()-zGhostOffsetLeft; idz <= localId[2].last()+zGhostOffsetRight; idz++) {
    	 for (int idy = localId[1].first()-yGhostOffsetLeft; idy <= localId[1].last()+yGhostOffsetRight; idy++) {
    	     for (int idx = localId[0].first()-xGhostOffsetLeft; idx <= localId[0].last()+xGhostOffsetRight; idx++) {
	            if (isInside(idx, idy, idz)) {
                    IdxMap[toCoordIdx(idx, idy, idz)] = id;
                    CoordMap[id] = toCoordIdx(idx, idy, idz);
                    id++;
                 }
             }
         }
     }
gsell's avatar
gsell committed
226 227
}

228 229 230 231 232
void ArbitraryDomain::Compute(Vector_t hr, NDIndex<3> localId, Vector_t globalMeanR, Vektor<double, 4> globalToLocalQuaternion){

    setHr(hr);
    globalMeanR_m = globalMeanR;
    globalToLocalQuaternion_m = globalToLocalQuaternion;	
233
    localToGlobalQuaternion_m[0] = globalToLocalQuaternion[0];
234
    for (int i=1; i<4; i++)
235
		localToGlobalQuaternion_m[i] = -globalToLocalQuaternion[i];	
236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254

    int zGhostOffsetLeft  = (localId[2].first()== 0) ? 0 : 1;
    int zGhostOffsetRight = (localId[2].last() == nr[2] - 1) ? 0 : 1;
    int yGhostOffsetLeft  = (localId[1].first()== 0) ? 0 : 1;
    int yGhostOffsetRight = (localId[1].last() == nr[1] - 1) ? 0 : 1;
    int xGhostOffsetLeft  = (localId[0].first()== 0) ? 0 : 1;
    int xGhostOffsetRight = (localId[0].last() == nr[0] - 1) ? 0 : 1;
    
    hasGeometryChanged_m = true;

    IntersectLoX.clear();
    IntersectHiX.clear();
    IntersectLoY.clear();
    IntersectHiY.clear();
    IntersectLoZ.clear();
    IntersectHiZ.clear();

    //calculate intersection 
    Vector_t P, saveP, dir, I;
255 256 257 258 259
    // TODO: Find and set the reference point for any time of geometry 
    //Reference Point inside the boundary for IsoDar Geo
    Vector_t P0 = Vector_t(0,0,bgeom_m->getmincoords()[2]+hr[2]);     
    //Reference Point where the boundary geometry is cylinder 
    P0 = Vector_t(0,0,0);
260 261 262 263 264 265
    for (int idz = localId[2].first()-zGhostOffsetLeft; idz <= localId[2].last()+zGhostOffsetRight; idz++) {
	 saveP[2] = (idz - (nr[2]-1)/2.0)*hr[2];
	 for (int idy = localId[1].first()-yGhostOffsetLeft; idy <= localId[1].last()+yGhostOffsetRight; idy++) {
	     saveP[1] = (idy - (nr[1]-1)/2.0)*hr[1];
    	     for (int idx = localId[0].first()-xGhostOffsetLeft; idx <= localId[0].last()+xGhostOffsetRight; idx++) {
	       	  saveP[0] = (idx - (nr[0]-1)/2.0)*hr[0];
266 267 268 269 270
		  P = saveP;
		  rotateWithQuaternion(P, localToGlobalQuaternion_m); 
		  P += globalMeanR_m;

		  if (bgeom_m->fastIsInside(P0, P) % 2 == 0) {
271
		     P0 = P;
272 273
       		     
                     std::tuple<int, int, int> pos(idx, idy, idz);
274 275 276

		     rotateZAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
277
//			setYRangeMax(MIN2(intersectMaxCoords_m(2),I[2]));
278 279 280 281
			I -= globalMeanR_m;
			rotateWithQuaternion(I, globalToLocalQuaternion_m); 
       	      		IntersectHiZ.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[2]));
		     } else {
282
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
283 284 285 286 287
			   *gmsg << "zdir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)) {
288
//			setYRangeMin(MAX2(intersectMinCoords_m(2),I[2]));
289 290
		        I -= globalMeanR_m;
			rotateWithQuaternion(I, globalToLocalQuaternion_m); 
291
       	      		IntersectLoZ.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[2]));
292
		     } else {
293
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
294 295 296 297 298 299
			   *gmsg << "zdir=-1 " << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
	  	     }
	
	             rotateYAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
300
//			 setZRangeMax(MIN2(intersectMaxCoords_m(1),I[1]));
301 302 303 304
			 I -= globalMeanR_m;
			 rotateWithQuaternion(I, globalToLocalQuaternion_m); 
       	      		 IntersectHiY.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[1]));
	   	     } else {
305
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
306 307 308 309 310
			   *gmsg << "ydir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)) {
311
//	    	        setZRangeMin(MAX2(intersectMinCoords_m(1),I[1]));
312 313 314 315
		   	I -= globalMeanR_m;
			rotateWithQuaternion(I, globalToLocalQuaternion_m); 
       	      		IntersectLoY.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[1]));
		     } else {
316
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
317 318 319 320 321 322
			   *gmsg << "ydir=-1" << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }

	             rotateXAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
323
//			setXRangeMax(MIN2(intersectMaxCoords_m(0),I[0]));
324 325 326 327
			I -= globalMeanR_m;
			rotateWithQuaternion(I, globalToLocalQuaternion_m); 
       	      		IntersectHiX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		     } else {
328
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
329 330 331 332 333
			   *gmsg << "xdir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif	
		     }

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)){
334
//			setXRangeMin(MAX2(intersectMinCoords_m(0),I[0]));
335 336 337 338
			I -= globalMeanR_m;
			rotateWithQuaternion(I, globalToLocalQuaternion_m); 
       	      		IntersectLoX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		     } else {
339
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
340 341 342
			   *gmsg << "xdir=-1 " << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif		
		     }
343 344 345 346 347
		  } else {
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
			   *gmsg << "OUTSIDE" << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif		
		  }
348
	     }
349 350 351 352 353
	 }
     }

    IdxMap.clear();
    CoordMap.clear();
354 355 356 357 358 359 360 361

    register int id=0;
    for (int idz = 0; idz < nr[2]; idz++) {
        for (int idy = 0; idy < nr[1]; idy++) {
            for (int idx = 0; idx < nr[0]; idx++) {
                IdxMap[toCoordIdx(idx, idy, idz)] = id;
                CoordMap[id++] = toCoordIdx(idx, idy, idz);
            }
362 363 364
         }
     }
}
365

366 367
// Conversion from (x,y,z) to index in xyz plane
inline int ArbitraryDomain::toCoordIdx(int idx, int idy, int idz) {
368
    return (idz * nr[1] + idy) * nr[0]  + idx;
gsell's avatar
gsell committed
369 370
}

371 372
// Conversion from (x,y,z) to index on the 3D grid
int ArbitraryDomain::getIdx(int idx, int idy, int idz) {
373
    return IdxMap[toCoordIdx(idx, idy, idz)];
374
} 
gsell's avatar
gsell committed
375

376 377
// Conversion from a 3D index to (x,y,z)
inline void ArbitraryDomain::getCoord(int idxyz, int &idx, int &idy, int &idz) {
gsell's avatar
gsell committed
378

379 380 381 382 383 384
    int id = CoordMap[idxyz];
    idx = id % (int)nr[0];
    id /= nr[0];
    idy = id % (int)nr[1];
    id /= nr[1];
    idz = id;
gsell's avatar
gsell committed
385 386
}

387
inline bool ArbitraryDomain::isInside(int idx, int idy, int idz) {
388
    Vector_t P;
389

390 391 392
    P[0] = (idx - (nr[0]-1)/2.0)*hr[0];
    P[1] = (idy - (nr[1]-1)/2.0)*hr[1];
    P[2] = (idz - (nr[2]-1)/2.0)*hr[2];
393

gsell's avatar
gsell committed
394
    bool ret = false;
395
    int  countH, countL;
396
    std::multimap < std::tuple<int, int, int>, double >::iterator itrH, itrL;
397 398
    std::tuple<int, int, int> coordxyz(idx, idy, idz);
             
gsell's avatar
gsell committed
399
    //check if z is inside with x,y coords
400 401
    itrH = IntersectHiZ.find(coordxyz);
    itrL = IntersectLoZ.find(coordxyz);
402

403 404 405
    countH = IntersectHiZ.count(coordxyz);
    countL = IntersectLoZ.count(coordxyz);
    if(countH == 1 && countL == 1)
406
        ret = (P[2] <= itrH->second) && (P[2] >= itrL->second);
407 408

     //check if y is inside with x,z coords
409 410 411 412 413 414
    itrH = IntersectHiY.find(coordxyz);
    itrL = IntersectLoY.find(coordxyz);

    countH = IntersectHiY.count(coordxyz);
    countL = IntersectLoY.count(coordxyz);
    if(countH == 1 && countL == 1)
415
        ret = ret && (P[1] <= itrH->second) && (P[1] >= itrL->second);
gsell's avatar
gsell committed
416

417
    //check if x is inside with y,z coords
418 419 420 421 422 423
    itrH = IntersectHiX.find(coordxyz);
    itrL = IntersectLoX.find(coordxyz);

    countH = IntersectHiX.count(coordxyz);
    countL = IntersectLoX.count(coordxyz);
    if(countH == 1 && countL == 1)
424 425
        ret = ret && (P[0] <= itrH->second) && (P[0] >= itrL->second);
   
426
    return ret; 
gsell's avatar
gsell committed
427 428 429
}

int ArbitraryDomain::getNumXY(int z) {
430 431 432
    
	return numXY[z];
}
gsell's avatar
gsell committed
433 434


435
void ArbitraryDomain::getBoundaryStencil(int idxyz, double &W, double &E, double &S, double &N, double &F, double &B, double &C, double &scaleFactor) {
436
    int idx = 0, idy = 0, idz = 0;
437

438 439
    getCoord(idxyz, idx, idy, idz);
    getBoundaryStencil(idx, idy, idz, W, E, S, N, F, B, C, scaleFactor);
gsell's avatar
gsell committed
440 441
}

442
void ArbitraryDomain::getBoundaryStencil(int idx, int idy, int idz, double &W, double &E, double &S, double &N, double &F, double &B, double &C, double &scaleFactor) {
gsell's avatar
gsell committed
443

444
    scaleFactor = 1.0;
445
   // determine which interpolation method we use for points near the boundary
446 447
    switch(interpolationMethod){
    	case CONSTANT:
448
        	ConstantInterpolation(idx,idy,idz,W,E,S,N,F,B,C,scaleFactor);
449 450
        	break;
    	case LINEAR:
451
            //  LinearInterpolation(idx,idy,idz,W,E,S,N,F,B,C,scaleFactor);
452 453
        	break;
    	case QUADRATIC:
454
            //  QuadraticInterpolation(idx,idy,idz,W,E,S,N,F,B,C,scaleFactor);
455
        	break;
456 457 458 459 460 461
    }

    // stencil center value has to be positive!
    assert(C > 0);
}

462
void ArbitraryDomain::ConstantInterpolation(int idx, int idy, int idz, double& W, double& E, double& S, double& N, double& F, double& B, double& C, double &scaleFactor) {
463 464 465 466 467 468 469 470 471

    W = -1/(hr[0]*hr[0]);
    E = -1/(hr[0]*hr[0]);
    N = -1/(hr[1]*hr[1]);
    S = -1/(hr[1]*hr[1]);
    F = -1/(hr[2]*hr[2]);
    B = -1/(hr[2]*hr[2]);
    C = 2/(hr[0]*hr[0]) + 2/(hr[1]*hr[1]) + 2/(hr[2]*hr[2]);

472
    if(!isInside(idx-1,idy,idz))
473
        W = 0.0;
474 475
    if(!isInside(idx+1,idy,idz)) 
        E = 0.0;
476

477
    if(!isInside(idx,idy+1,idz))
478
        N = 0.0;
479
    if(!isInside(idx,idy-1,idz)) 
480
        S = 0.0;
481
    
482
    if(!isInside(idx,idy,idz-1)) 
483
	F = 0.0;	
484
    if(!isInside(idx,idy,idz+1)) 
485
	B = 0.0;	
486
}
487

488
/*
489
void ArbitraryDomain::LinearInterpolation(int idx, int idy, int idz, double& W, double& E, double& S, double& N, double& F, double& B, double& C, double &scaleFactor) 
490 491 492 493
{

    scaleFactor = 1.0;

494 495
    double dx=-1, dy=-1, dz=-1;

496 497 498
    double cx = (idx - (nr[0]-1)/2.0)*hr[0];
    double cy = (idy - (nr[1]-1)/2.0)*hr[1];
    double cz = (idz - (nr[2]-1)/2.0)*hr[2];
499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514

    int    countH, countL;
    std::multimap < std::tuple<int, int, int>, double >::iterator itrH, itrL;

    std::tuple<int, int, int> coordxyz(idx, idy, idz);
             
    //check if z is inside with x,y coords
    itrH = IntersectHiZ.find(coordxyz);
    itrL = IntersectLoZ.find(coordxyz);

    countH = IntersectHiZ.count(coordxyz);
    countL = IntersectLoZ.count(coordxyz);

    if(countH == 1 && countL == 1)
        ret = (cz <= itrH->second) && (cz >= itrL->second);

515 516 517 518 519


    std::multimap< std::pair<int, int>, double >::iterator it;
    std::pair< std::multimap< std::pair<int, int>, double>::iterator, std::multimap< std::pair<int, int>, double>::iterator > ret;

520
    std::pair<int, int> coordyz(idy, idz);
521 522 523 524 525 526 527 528
    ret = IntersectXDir.equal_range(coordyz);
    for(it = ret.first; it != ret.second; ++it) {
        if(fabs(it->second - cx) < hr[0]) {
            dx = it->second;
            break;
        }
    }

529
    std::pair<int, int> coordxz(idx, idz);
530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604
    ret = IntersectYDir.equal_range(coordxz);
    for(it = ret.first; it != ret.second; ++it) {
        if(fabs(it->second - cy) < hr[1]) {
            dy = it->second;
            break;
        }
    }


    double dw=hr[0];
    double de=hr[0];
    double dn=hr[1];
    double ds=hr[1];
    C = 0.0;

    // we are a right boundary point
    if(dx >= 0 && dx > cx)
    {	
        C += 1/((dx-cx)*de);
        E = 0.0;
    } else {
        C += 1/(de*de);
        E = -1/(de*de);
    }

    // we are a left boundary point
    if(dx <= 0 && dx < cx)
    {
        C += 1/((cx-dx)*dw);
        W = 0.0;
    } else {
        C += 1/(dw*dw);
        W = -1/(dw*dw);
    }

    // we are a upper boundary point
    if(dy >= 0 && dy > cy)
    {
        C += 1/((dy-cy)*dn);
        N = 0.0;
    } else {
        C += 1/(dn*dn);
        N = -1/(dn*dn);
    }

    // we are a lower boundary point
    if(dy <= 0 && dy < cy)
    {
        C += 1/((cy-dy)*ds);
        S = 0.0;
    } else {
        C += 1/(ds*ds);
        S = -1/(ds*ds);
    }

    F = -1/(hr[2]*hr[2]); 
    B = -1/(hr[2]*hr[2]);

    //XXX: In stand-alone only Dirichlet for validation purposes
    if(z == 0 || z == nr[2]-1) {

        // Dirichlet
        C += 2/hr[2]*1/hr[2];

        //C += 1/hr[2]*1/hr[2];

        // case where we are on the Neumann BC in Z-direction
        // where we distinguish two cases  
        if(z == 0)
            F = 0.0;
        else 
            B = 0.0;

        //for test no neumann 
        //C += 2/((hr[2]*nr[2]/2.0) * hr[2]);
605 606
        //
        //   double d = hr[2]*(nr[2])/2;
607 608 609
        //   C += 2/(d * hr[2]);


610 611 612 613 614
        ////neumann stuff
        //W /= 2.0;
        //E /= 2.0;
        //N /= 2.0;
        //S /= 2.0;
615 616
        //C /= 2.0;

617
        scaleFactor *= 0.5;
618 619 620 621

    } else 
        C += 2*1/hr[2]*1/hr[2];
}
622
*/        
gsell's avatar
gsell committed
623

624
void ArbitraryDomain::getNeighbours(int id, int &W, int &E, int &S, int &N, int &F, int &B) {
gsell's avatar
gsell committed
625

626
    int idx = 0, idy = 0, idz = 0;
gsell's avatar
gsell committed
627

628 629
    getCoord(id, idx, idy, idz);
    getNeighbours(idx, idy, idz, W, E, S, N, F, B);
gsell's avatar
gsell committed
630 631
}

632
void ArbitraryDomain::getNeighbours(int idx, int idy, int idz, int &W, int &E, int &S, int &N, int &F, int &B) {
gsell's avatar
gsell committed
633

634 635 636 637 638 639
    W = getIdx(idx - 1, idy, idz);
    E = getIdx(idx + 1, idy, idz);
    N = getIdx(idx, idy + 1, idz);
    S = getIdx(idx, idy - 1, idz);
    F = getIdx(idx, idy, idz - 1);
    B = getIdx(idx, idy, idz + 1);
640

641
    if(!isInside(idx+1,idy,idz)) 
gsell's avatar
gsell committed
642 643
        E = -1;

644 645 646 647
    if(!isInside(idx-1,idy,idz))
        W = -1;

    if(!isInside(idx,idy+1,idz))
gsell's avatar
gsell committed
648
        N = -1;
649

650
    if(!isInside(idx,idy-1,idz)) 
gsell's avatar
gsell committed
651
        S = -1;
652 653 654
    
    if(!isInside(idx,idy,idz-1)) 
	F = -1;	
gsell's avatar
gsell committed
655

656 657
    if(!isInside(idx,idy,idz+1)) 
	B = -1;	
gsell's avatar
gsell committed
658 659 660 661 662 663 664 665 666 667

}


inline void ArbitraryDomain::crossProduct(double A[], double B[], double C[]) {
    C[0] = A[1] * B[2] - A[2] * B[1];
    C[1] = A[2] * B[0] - A[0] * B[2];
    C[2] = A[0] * B[1] - A[1] * B[0];
}

668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716
inline void ArbitraryDomain::rotateWithQuaternion(Vector_t & v, Vektor<double, 4> const quaternion) {
    // rotates a Vector_t (3 elements) using a quaternion.
    // Flip direction of rotation by quaternionVectorcomponent *= -1

    Vector_t const quaternionVectorComponent = Vector_t(quaternion(1), quaternion(2), quaternion(3));
    double const quaternionScalarComponent = quaternion(0);
        
    v = 2.0 * dot(quaternionVectorComponent, v) * quaternionVectorComponent 
        + (quaternionScalarComponent * quaternionScalarComponent  
        -  dot(quaternionVectorComponent, quaternionVectorComponent)) * v 
        + 2.0 * quaternionScalarComponent * cross(quaternionVectorComponent, v);
}

inline void ArbitraryDomain::rotateXAxisWithQuaternion(Vector_t & v, Vektor<double, 4> const quaternion) {
    // rotates the positive xaxis using a quaternion.
   
    v(0) = quaternion(0) * quaternion(0) 
         + quaternion(1) * quaternion(1) 
         - quaternion(2) * quaternion(2) 
         - quaternion(3) * quaternion(3);
 
    v(1) = 2.0 * (quaternion(1) * quaternion(2) + quaternion(0) * quaternion(3));
    v(2) = 2.0 * (quaternion(1) * quaternion(3) - quaternion(0) * quaternion(2));
}

inline void ArbitraryDomain::rotateYAxisWithQuaternion(Vector_t & v, Vektor<double, 4> const quaternion) {
    // rotates the positive yaxis using a quaternion.
    
    v(0) = 2.0 * (quaternion(1) * quaternion(2) - quaternion(0) * quaternion(3));
      
    v(1) = quaternion(0) * quaternion(0) 
         - quaternion(1) * quaternion(1)
         + quaternion(2) * quaternion(2)
         - quaternion(3) * quaternion(3);

    v(2) = 2.0 * (quaternion(2) * quaternion(3) + quaternion(0) * quaternion(1));
}

inline void ArbitraryDomain::rotateZAxisWithQuaternion(Vector_t & v, Vektor<double, 4> const quaternion) {
    // rotates the positive zaxis using a quaternion.
    v(0) = 2.0 * (quaternion(1) * quaternion(3) + quaternion(0) * quaternion(2));
    v(1) = 2.0 * (quaternion(2) * quaternion(3) - quaternion(0) * quaternion(1));    

    v(2) = quaternion(0) * quaternion(0) 
         - quaternion(1) * quaternion(1)
         - quaternion(2) * quaternion(2)
         + quaternion(3) * quaternion(3);

}
717
#endif //#ifdef HAVE_SAAMG_SOLVER