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

16
#ifdef HAVE_SAAMG_SOLVER
kraus's avatar
kraus committed
17 18
#include "Solvers/ArbitraryDomain.h"

19 20 21 22
#include <map>
#include <cmath>
#include <iostream>
#include <assert.h>
gsell's avatar
gsell committed
23

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

28
ArbitraryDomain::ArbitraryDomain(
kraus's avatar
kraus committed
29 30 31
	BoundaryGeometry * bgeom,
	Vector_t nr,
	Vector_t hr,
32 33 34
	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
    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;
kraus's avatar
kraus committed
71

72 73
    hasGeometryChanged_m = true;

74 75 76 77 78 79 80 81
    IntersectLoX.clear();
    IntersectHiX.clear();
    IntersectLoY.clear();
    IntersectHiY.clear();
    IntersectLoZ.clear();
    IntersectHiZ.clear();


kraus's avatar
kraus committed
82
    //calculate intersection
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
    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]));
		}
	 }
     }

kraus's avatar
kraus committed
122
    //number of ghost nodes to the right
123 124 125 126 127 128
    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

kraus's avatar
kraus committed
133
    //number of ghost nodes to the left
134 135 136 137 138 139
    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

kraus's avatar
kraus committed
144
    //number of ghost nodes to the right
145 146 147 148 149 150
    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
            }
        }
    }

kraus's avatar
kraus committed
155
    //number of ghost nodes to the left
156 157 158 159 160 161 162 163 164
    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


kraus's avatar
kraus committed
167
    //number of ghost nodes to the right
168 169 170 171 172 173
    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
            }
        }
    }

kraus's avatar
kraus committed
178
    //number of ghost nodes to the left
179 180 181 182 183 184 185 186 187
    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
kraus's avatar
kraus committed
189
    int numxy;
190
    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();
kraus's avatar
kraus committed
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
void ArbitraryDomain::Compute(Vector_t hr, NDIndex<3> localId, Vector_t globalMeanR, Vektor<double, 4> globalToLocalQuaternion){

    setHr(hr);
    globalMeanR_m = globalMeanR;
kraus's avatar
kraus committed
232
    globalToLocalQuaternion_m = globalToLocalQuaternion;
233
    localToGlobalQuaternion_m[0] = globalToLocalQuaternion[0];
234
    for (int i=1; i<4; i++)
kraus's avatar
kraus committed
235
		localToGlobalQuaternion_m[i] = -globalToLocalQuaternion[i];
236 237 238 239 240 241 242

    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;
kraus's avatar
kraus committed
243

244 245 246 247 248 249 250 251 252
    hasGeometryChanged_m = true;

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

kraus's avatar
kraus committed
253
    //calculate intersection
254
    Vector_t P, saveP, dir, I;
kraus's avatar
kraus committed
255
    // TODO: Find and set the reference point for any time of geometry
256
    //Reference Point inside the boundary for IsoDar Geo
kraus's avatar
kraus committed
257 258
    Vector_t P0 = Vector_t(0,0,bgeom_m->getmincoords()[2]+hr[2]);
    //Reference Point where the boundary geometry is cylinder
259
    //P0 = Vector_t(0,0,0);  // Uncomment for cylinder Benchmarking -DW
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
		  P = saveP;
kraus's avatar
kraus committed
267
		  rotateWithQuaternion(P, localToGlobalQuaternion_m);
268 269 270
		  P += globalMeanR_m;

		  if (bgeom_m->fastIsInside(P0, P) % 2 == 0) {
271
		     P0 = P;
kraus's avatar
kraus committed
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
			I -= globalMeanR_m;
kraus's avatar
kraus committed
279
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
280 281
       	      		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
		        I -= globalMeanR_m;
kraus's avatar
kraus committed
290
			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
			   *gmsg << "zdir=-1 " << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
	  	     }
kraus's avatar
kraus committed
297

298 299
	             rotateYAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
300
//			 setZRangeMax(MIN2(intersectMaxCoords_m(1),I[1]));
301
			 I -= globalMeanR_m;
kraus's avatar
kraus committed
302
			 rotateWithQuaternion(I, globalToLocalQuaternion_m);
303 304
       	      		 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
		   	I -= globalMeanR_m;
kraus's avatar
kraus committed
313
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
314 315
       	      		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
			I -= globalMeanR_m;
kraus's avatar
kraus committed
325
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
326 327
       	      		IntersectHiX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		     } else {
328
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
329
			   *gmsg << "xdir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
kraus's avatar
kraus committed
330
#endif
331 332 333
		     }

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)){
334
//			setXRangeMin(MAX2(intersectMinCoords_m(0),I[0]));
335
			I -= globalMeanR_m;
kraus's avatar
kraus committed
336
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
337 338
       	      		IntersectLoX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		     } else {
339
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
340
			   *gmsg << "xdir=-1 " << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
kraus's avatar
kraus committed
341
#endif
342
		     }
343 344 345
		  } else {
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
			   *gmsg << "OUTSIDE" << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
kraus's avatar
kraus committed
346
#endif
347
		  }
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)];
kraus's avatar
kraus committed
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
    std::tuple<int, int, int> coordxyz(idx, idy, idz);
kraus's avatar
kraus committed
398

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
        ret = ret && (P[0] <= itrH->second) && (P[0] >= itrL->second);
kraus's avatar
kraus committed
425 426

    return ret;
gsell's avatar
gsell committed
427 428 429
}

int ArbitraryDomain::getNumXY(int z) {
kraus's avatar
kraus committed
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;
kraus's avatar
kraus committed
474
    if(!isInside(idx+1,idy,idz))
475
        E = 0.0;
476

477
    if(!isInside(idx,idy+1,idz))
478
        N = 0.0;
kraus's avatar
kraus committed
479
    if(!isInside(idx,idy-1,idz))
480
        S = 0.0;
kraus's avatar
kraus committed
481 482 483 484 485

    if(!isInside(idx,idy,idz-1))
	F = 0.0;
    if(!isInside(idx,idy,idz+1))
	B = 0.0;
486
}
487

488
/*
kraus's avatar
kraus committed
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

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

    std::tuple<int, int, int> coordxyz(idx, idy, idz);
kraus's avatar
kraus committed
504

505 506 507 508 509 510 511 512 513 514
    //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
    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)
kraus's avatar
kraus committed
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
        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);
    }

kraus's avatar
kraus committed
585
    F = -1/(hr[2]*hr[2]);
586 587 588 589 590 591 592 593 594 595 596
    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
kraus's avatar
kraus committed
597
        // where we distinguish two cases
598 599
        if(z == 0)
            F = 0.0;
kraus's avatar
kraus committed
600
        else
601 602
            B = 0.0;

kraus's avatar
kraus committed
603
        //for test no neumann
604
        //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

kraus's avatar
kraus committed
619
    } else
620 621
        C += 2*1/hr[2]*1/hr[2];
}
kraus's avatar
kraus committed
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

kraus's avatar
kraus committed
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

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

kraus's avatar
kraus committed
653 654 655 656 657
    if(!isInside(idx,idy,idz-1))
	F = -1;

    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
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);
kraus's avatar
kraus committed
674 675 676 677

    v = 2.0 * dot(quaternionVectorComponent, v) * quaternionVectorComponent
        + (quaternionScalarComponent * quaternionScalarComponent
        -  dot(quaternionVectorComponent, quaternionVectorComponent)) * v
678 679 680 681 682
        + 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.
kraus's avatar
kraus committed
683 684 685 686

    v(0) = quaternion(0) * quaternion(0)
         + quaternion(1) * quaternion(1)
         - quaternion(2) * quaternion(2)
687
         - quaternion(3) * quaternion(3);
kraus's avatar
kraus committed
688

689 690 691 692 693 694
    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.
kraus's avatar
kraus committed
695

696
    v(0) = 2.0 * (quaternion(1) * quaternion(2) - quaternion(0) * quaternion(3));
kraus's avatar
kraus committed
697 698

    v(1) = quaternion(0) * quaternion(0)
699 700 701 702 703 704 705 706 707 708
         - 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));
kraus's avatar
kraus committed
709
    v(1) = 2.0 * (quaternion(2) * quaternion(3) - quaternion(0) * quaternion(1));
710

kraus's avatar
kraus committed
711
    v(2) = quaternion(0) * quaternion(0)
712 713 714 715 716
         - quaternion(1) * quaternion(1)
         - quaternion(2) * quaternion(2)
         + quaternion(3) * quaternion(3);

}
717
#endif //#ifdef HAVE_SAAMG_SOLVER