ArbitraryDomain.cpp 22 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 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46
ArbitraryDomain::ArbitraryDomain( BoundaryGeometry * bgeom,
   	                          Vector_t nr,
	                          Vector_t hr,
	                          std::string interpl){ 
    bgeom_m  = bgeom;
    minCoords_m = bgeom->getmincoords();
    maxCoords_m = bgeom->getmaxcoords();

    setNr(nr);
    setHr(hr);

    startId = 0;

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

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

53 54 55 56 57
void ArbitraryDomain::Compute(Vector_t hr) {

    setHr(hr);
}

58
/*
59
void ArbitraryDomain::Compute(Vector_t hr, NDIndex<3> localId) {
gsell's avatar
gsell committed
60 61

    setHr(hr);
62
    setNr(nr);
63 64 65 66 67 68
    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
69

70 71
    hasGeometryChanged_m = true;

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


kraus's avatar
kraus committed
80
    //calculate intersection
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
    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
120
    //number of ghost nodes to the right
121 122 123 124 125 126
    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
127 128
            }
        }
129
    }
gsell's avatar
gsell committed
130

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

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

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


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

kraus's avatar
kraus committed
176
    //number of ghost nodes to the left
177 178 179 180 181 182 183 184 185
    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
186
    //xy points in z plane
kraus's avatar
kraus committed
187
    int numxy;
188
    int numtotalxy = 0;
gsell's avatar
gsell committed
189

190
    numXY.clear();
gsell's avatar
gsell committed
191

192 193 194 195 196 197
    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
198 199
            }
        }
200
        numtotalxy += numxy;
gsell's avatar
gsell committed
201 202
    }

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

206
    startId -= numtotalxy;
gsell's avatar
gsell committed
207 208 209 210

    //build up index and coord map
    IdxMap.clear();
    CoordMap.clear();
kraus's avatar
kraus committed
211

212 213 214 215 216 217 218 219 220 221 222 223
    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
224
}
225
*/
gsell's avatar
gsell committed
226

227
void ArbitraryDomain::Compute(Vector_t hr, NDIndex<3> localId){
228 229

    setHr(hr);
230

231 232 233 234
    globalMeanR_m = getGlobalMeanR();

    globalToLocalQuaternion_m = getGlobalToLocalQuaternion();
    localToGlobalQuaternion_m[0] = globalToLocalQuaternion_m[0];
235
    for (int i=1; i<4; i++)
236
		localToGlobalQuaternion_m[i] = -globalToLocalQuaternion_m[i];
237 238 239 240 241 242 243

    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
244

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

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

kraus's avatar
kraus committed
254
    //calculate intersection
255
    Vector_t P, saveP, dir, I;
256 257
    //Reference Point
    Vector_t P0 = globalMeanR_m;
258 259 260 261 262 263
    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];
264
		  P = saveP;
kraus's avatar
kraus committed
265
		  rotateWithQuaternion(P, localToGlobalQuaternion_m);
266 267 268
		  P += globalMeanR_m;

		  if (bgeom_m->fastIsInside(P0, P) % 2 == 0) {
269
		     P0 = P;
kraus's avatar
kraus committed
270

271
                     std::tuple<int, int, int> pos(idx, idy, idz);
272 273 274

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

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)) {
285
                        I -= globalMeanR_m;
kraus's avatar
kraus committed
286
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
287
       	      		IntersectLoZ.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[2]));
288
		     } else {
289
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
290 291 292
			   *gmsg << "zdir=-1 " << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
	  	     }
kraus's avatar
kraus committed
293

294 295
	             rotateYAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
296
                         I -= globalMeanR_m;
kraus's avatar
kraus committed
297
			 rotateWithQuaternion(I, globalToLocalQuaternion_m);
298 299
       	      		 IntersectHiY.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[1]));
	   	     } else {
300
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
301 302 303 304 305
			   *gmsg << "ydir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)) {
306
                        I -= globalMeanR_m;
kraus's avatar
kraus committed
307
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
308 309
       	      		IntersectLoY.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[1]));
		     } else {
310
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
311 312 313 314 315 316
			   *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)) {
317
                        I -= globalMeanR_m;
kraus's avatar
kraus committed
318
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
319 320
       	      		IntersectHiX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		     } else {
321
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
322
			   *gmsg << "xdir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
kraus's avatar
kraus committed
323
#endif
324 325 326
		     }

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)){
327
                        I -= globalMeanR_m;
kraus's avatar
kraus committed
328
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
329 330
       	      		IntersectLoX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		     } else {
331
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
332
			   *gmsg << "xdir=-1 " << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
kraus's avatar
kraus committed
333
#endif
334
		     }
335 336 337
		  } else {
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
			   *gmsg << "OUTSIDE" << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
kraus's avatar
kraus committed
338
#endif
339
		  }
340
	     }
341 342 343 344 345
	 }
     }

    IdxMap.clear();
    CoordMap.clear();
346 347 348 349 350 351 352 353

    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);
            }
354 355 356
         }
     }
}
357

358 359
// Conversion from (x,y,z) to index in xyz plane
inline int ArbitraryDomain::toCoordIdx(int idx, int idy, int idz) {
360
    return (idz * nr[1] + idy) * nr[0]  + idx;
gsell's avatar
gsell committed
361 362
}

363 364
// Conversion from (x,y,z) to index on the 3D grid
int ArbitraryDomain::getIdx(int idx, int idy, int idz) {
365
    return IdxMap[toCoordIdx(idx, idy, idz)];
kraus's avatar
kraus committed
366
}
gsell's avatar
gsell committed
367

368 369
// 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
370

371 372 373 374 375 376
    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
377 378
}

379
inline bool ArbitraryDomain::isInside(int idx, int idy, int idz) {
380
    Vector_t P;
381

382 383 384
    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];
385

gsell's avatar
gsell committed
386
    bool ret = false;
387
    int  countH, countL;
388
    std::multimap < std::tuple<int, int, int>, double >::iterator itrH, itrL;
389
    std::tuple<int, int, int> coordxyz(idx, idy, idz);
kraus's avatar
kraus committed
390

gsell's avatar
gsell committed
391
    //check if z is inside with x,y coords
392 393
    itrH = IntersectHiZ.find(coordxyz);
    itrL = IntersectLoZ.find(coordxyz);
394

395 396 397
    countH = IntersectHiZ.count(coordxyz);
    countL = IntersectLoZ.count(coordxyz);
    if(countH == 1 && countL == 1)
398
        ret = (P[2] <= itrH->second) && (P[2] >= itrL->second);
399 400

     //check if y is inside with x,z coords
401 402 403 404 405 406
    itrH = IntersectHiY.find(coordxyz);
    itrL = IntersectLoY.find(coordxyz);

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

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

    countH = IntersectHiX.count(coordxyz);
    countL = IntersectLoX.count(coordxyz);
    if(countH == 1 && countL == 1)
416
        ret = ret && (P[0] <= itrH->second) && (P[0] >= itrL->second);
kraus's avatar
kraus committed
417 418

    return ret;
gsell's avatar
gsell committed
419 420 421
}

int ArbitraryDomain::getNumXY(int z) {
kraus's avatar
kraus committed
422

423 424
	return numXY[z];
}
gsell's avatar
gsell committed
425 426


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

430 431
    getCoord(idxyz, idx, idy, idz);
    getBoundaryStencil(idx, idy, idz, W, E, S, N, F, B, C, scaleFactor);
gsell's avatar
gsell committed
432 433
}

434
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
435

436
    scaleFactor = 1.0;
437
   // determine which interpolation method we use for points near the boundary
438 439
    switch(interpolationMethod){
    	case CONSTANT:
440
        	ConstantInterpolation(idx,idy,idz,W,E,S,N,F,B,C,scaleFactor);
441 442
        	break;
    	case LINEAR:
443
                LinearInterpolation(idx,idy,idz,W,E,S,N,F,B,C,scaleFactor);
444 445
        	break;
    	case QUADRATIC:
446
            //  QuadraticInterpolation(idx,idy,idz,W,E,S,N,F,B,C,scaleFactor);
447
        	break;
448 449 450 451 452 453
    }

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

454
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) {
455 456 457 458 459 460 461 462 463

    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]);

464
    if(!isInside(idx-1,idy,idz))
465
        W = 0.0;
kraus's avatar
kraus committed
466
    if(!isInside(idx+1,idy,idz))
467
        E = 0.0;
468

469
    if(!isInside(idx,idy+1,idz))
470
        N = 0.0;
kraus's avatar
kraus committed
471
    if(!isInside(idx,idy-1,idz))
472
        S = 0.0;
kraus's avatar
kraus committed
473 474 475 476 477

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

kraus's avatar
kraus committed
480
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)
481
{
482
    scaleFactor = 1;
483

484 485 486
    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];
487

488 489 490 491 492 493
    double dx_w=hr[0];
    double dx_e=hr[0]; 
    double dy_n=hr[1]; 
    double dy_s=hr[1];
    double dz_f=hr[2];
    double dz_b=hr[2];
494 495
    C = 0.0;

496 497
    std::tuple<int, int, int> coordxyz(idx, idy, idz);
    std::multimap < std::tuple<int, int, int>, double >::iterator itrH, itrL;
498

499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 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
    if (idx == nr[0]-1)
       dx_e = fabs(IntersectHiX.find(coordxyz)->second - cx);
    if (idx == 0)
       dx_w = fabs(IntersectLoX.find(coordxyz)->second - cx);
    if (idy == nr[1]-1)
       dy_n = fabs(IntersectHiY.find(coordxyz)->second - cy);
    if (idy == 0)
       dy_s = fabs(IntersectLoY.find(coordxyz)->second - cy);
    if (idz == nr[2]-1)
       dz_b = fabs(IntersectHiZ.find(coordxyz)->second - cz);
    if (idz == 0)
       dz_f = fabs(IntersectLoZ.find(coordxyz)->second - cz);

    if(dx_w != 0)
        W = -(dz_f + dz_b) * (dy_n + dy_s) / dx_w;
    else
        W = 0;
    if(dx_e != 0)
        E = -(dz_f + dz_b) * (dy_n + dy_s) / dx_e;
    else
        E = 0;
    if(dy_n != 0)
        N = -(dz_f + dz_b) * (dx_w + dx_e) / dy_n;
    else
        N = 0;
    if(dy_s != 0)
        S = -(dz_f + dz_b) * (dx_w + dx_e) / dy_s;
    else
        S = 0;
    if(dz_f != 0)
        F = -(dx_w + dx_e) * (dy_n + dy_s) / dz_f;
    else
        F = 0;
    if(dz_b != 0)
        B = -(dx_w + dx_e) * (dy_n + dy_s) / dz_b;
    else
        B = 0;

    //RHS scaleFactor for current 3D index
    //0.5* comes from discretiztaion
    //scaleFactor = 0.5*(dw+de)*(dn+ds)*(df+db);
    scaleFactor = 0.5;
    if(dx_w + dx_e != 0)
        scaleFactor *= (dx_w + dx_e);
    if(dy_n + dy_s != 0)
        scaleFactor *= (dy_n + dy_s);
    if(dz_f + dz_b != 0)
        scaleFactor *= (dz_f + dz_b);

    //catch the case where a point lies on the boundary
    double m1 = dx_w * dx_e;
    double m2 = dy_n * dy_s;
    if(dx_e == 0)
        m1 = dx_w;
    if(dx_w == 0)
        m1 = dx_e;
    if(dy_n == 0)
        m2 = dy_s;
    if(dy_s == 0)
        m2 = dy_n;

    C = 2 / hr[2];
    if(dx_w != 0 || dx_e != 0)
        C *= (dx_w + dx_e);
    if(dy_n != 0 || dy_s != 0)
        C *= (dy_n + dy_s);
    if(dx_w != 0 || dx_e != 0)
        C += (dz_f + dz_b) * (dy_n + dy_s) * (dx_w + dx_e) / m1;
    if(dy_n != 0 || dy_s != 0)
        C += (dz_f + dz_b) * (dx_w + dx_e) * (dy_n + dy_s) / m2;
569
}
gsell's avatar
gsell committed
570

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

573
    int idx = 0, idy = 0, idz = 0;
gsell's avatar
gsell committed
574

575 576
    getCoord(id, idx, idy, idz);
    getNeighbours(idx, idy, idz, W, E, S, N, F, B);
gsell's avatar
gsell committed
577 578
}

579
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
580

581 582 583 584 585 586
    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);
587

kraus's avatar
kraus committed
588
    if(!isInside(idx+1,idy,idz))
gsell's avatar
gsell committed
589 590
        E = -1;

591 592 593 594
    if(!isInside(idx-1,idy,idz))
        W = -1;

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

kraus's avatar
kraus committed
597
    if(!isInside(idx,idy-1,idz))
gsell's avatar
gsell committed
598 599
        S = -1;

kraus's avatar
kraus committed
600 601 602 603 604
    if(!isInside(idx,idy,idz-1))
	F = -1;

    if(!isInside(idx,idy,idz+1))
	B = -1;
gsell's avatar
gsell committed
605 606 607 608 609 610 611 612 613 614

}


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];
}

615
inline void ArbitraryDomain::rotateWithQuaternion(Vector_t & v, Quaternion_t const quaternion) {
616 617 618 619 620
    // 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
621 622 623 624

    v = 2.0 * dot(quaternionVectorComponent, v) * quaternionVectorComponent
        + (quaternionScalarComponent * quaternionScalarComponent
        -  dot(quaternionVectorComponent, quaternionVectorComponent)) * v
625 626 627
        + 2.0 * quaternionScalarComponent * cross(quaternionVectorComponent, v);
}

628
inline void ArbitraryDomain::rotateXAxisWithQuaternion(Vector_t & v, Quaternion_t const quaternion) {
629
    // rotates the positive xaxis using a quaternion.
kraus's avatar
kraus committed
630 631 632 633

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

636 637 638 639
    v(1) = 2.0 * (quaternion(1) * quaternion(2) + quaternion(0) * quaternion(3));
    v(2) = 2.0 * (quaternion(1) * quaternion(3) - quaternion(0) * quaternion(2));
}

640
inline void ArbitraryDomain::rotateYAxisWithQuaternion(Vector_t & v, Quaternion_t const quaternion) {
641
    // rotates the positive yaxis using a quaternion.
kraus's avatar
kraus committed
642

643
    v(0) = 2.0 * (quaternion(1) * quaternion(2) - quaternion(0) * quaternion(3));
kraus's avatar
kraus committed
644 645

    v(1) = quaternion(0) * quaternion(0)
646 647 648 649 650 651 652
         - quaternion(1) * quaternion(1)
         + quaternion(2) * quaternion(2)
         - quaternion(3) * quaternion(3);

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

653
inline void ArbitraryDomain::rotateZAxisWithQuaternion(Vector_t & v, Quaternion_t const quaternion) {
654 655
    // rotates the positive zaxis using a quaternion.
    v(0) = 2.0 * (quaternion(1) * quaternion(3) + quaternion(0) * quaternion(2));
kraus's avatar
kraus committed
656
    v(1) = 2.0 * (quaternion(2) * quaternion(3) - quaternion(0) * quaternion(1));
657

kraus's avatar
kraus committed
658
    v(2) = quaternion(0) * quaternion(0)
659 660 661 662 663
         - quaternion(1) * quaternion(1)
         - quaternion(2) * quaternion(2)
         + quaternion(3) * quaternion(3);

}
664
#endif //#ifdef HAVE_SAAMG_SOLVER