ArbitraryDomain.cpp 21.7 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
ArbitraryDomain::ArbitraryDomain( BoundaryGeometry * bgeom,
   	                          Vector_t nr,
	                          Vector_t hr,
	                          std::string interpl){ 
    bgeom_m  = bgeom;
    minCoords_m = bgeom->getmincoords();
    maxCoords_m = bgeom->getmaxcoords();
35
    geomCentroid_m = (minCoords_m + maxCoords_m)/2.0;
36 37

    setNr(nr);
38 39 40
    for(int i=0; i<3; i++)
    Geo_hr_m[i] = (maxCoords_m[i] - minCoords_m[i])/nr[i];
    setHr(Geo_hr_m);
41 42 43 44 45 46 47 48 49

    startId = 0;

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

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

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

    setHr(hr);

60
    globalMeanR_m = getGlobalMeanR();
gsell's avatar
gsell committed
61

62 63 64 65
    globalToLocalQuaternion_m = getGlobalToLocalQuaternion();
    localToGlobalQuaternion_m[0] = globalToLocalQuaternion_m[0];
    for (int i=1; i<4; i++)
		localToGlobalQuaternion_m[i] = -globalToLocalQuaternion_m[i];
kraus's avatar
kraus committed
66

67 68
    hasGeometryChanged_m = true;

69 70 71 72 73 74 75
    IntersectLoX.clear();
    IntersectHiX.clear();
    IntersectLoY.clear();
    IntersectHiY.clear();
    IntersectLoZ.clear();
    IntersectHiZ.clear();

kraus's avatar
kraus committed
76
    //calculate intersection
77 78 79
    Vector_t P, saveP, dir, I;
    //Reference Point
    Vector_t P0 = geomCentroid_m;
80

81 82 83 84 85 86 87
    for (int idz = 0; idz < nr[2] ; idz++) {
        saveP[2] = (idz - (nr[2]-1)/2.0)*hr[2];
	 for (int idy = 0; idy < nr[1] ; idy++) {
	     saveP[1] = (idy - (nr[1]-1)/2.0)*hr[1];
    	     for (int idx = 0; idx <nr[0]; idx++) {
	       	  saveP[0] = (idx - (nr[0]-1)/2.0)*hr[0];
		  P = saveP;
88

89 90
		  rotateWithQuaternion(P, localToGlobalQuaternion_m);
		  P += geomCentroid_m;
91

92 93
		  if (bgeom_m->fastIsInside(P0, P) % 2 == 0) {
		     P0 = P;
94

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

97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116
		     rotateZAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
                        I -= geomCentroid_m;
  		        rotateWithQuaternion(I, globalToLocalQuaternion_m);
       	      		IntersectHiZ.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[2]));
		     } else {
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
			   *gmsg << "zdir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)) {
                        I -= geomCentroid_m;
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
       	      		IntersectLoZ.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[2]));
		     } else {
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
			   *gmsg << "zdir=-1 " << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
	  	     }
117

118 119 120 121
	             rotateYAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
                         I -= geomCentroid_m;
			 rotateWithQuaternion(I, globalToLocalQuaternion_m);
122
       	      		 IntersectHiY.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[1]));
123 124 125 126 127
	   	     } else {
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
			   *gmsg << "ydir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }
128

129 130 131 132 133 134 135 136 137
		     if (bgeom_m->intersectRayBoundary(P, -dir, I)) {
                        I -= geomCentroid_m;
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
       	      		IntersectLoY.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[1]));
		     } else {
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
			   *gmsg << "ydir=-1" << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }
138

139 140 141 142 143 144 145 146 147 148
	             rotateXAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
                        I -= geomCentroid_m;
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
       	      		IntersectHiX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		     } else {
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
			   *gmsg << "xdir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }
149

150 151 152 153 154 155 156 157 158 159 160 161 162 163 164
		     if (bgeom_m->intersectRayBoundary(P, -dir, I)){
                        I -= geomCentroid_m;
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
       	      		IntersectLoX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		     } else {
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
			   *gmsg << "xdir=-1 " << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }
		  } else {
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
			   *gmsg << "OUTSIDE" << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		  }
	     }
165 166
	 }
     }
gsell's avatar
gsell committed
167 168
    IdxMap.clear();
    CoordMap.clear();
kraus's avatar
kraus committed
169

170 171 172 173 174 175 176 177 178 179 180
    register int id=0;
    register int idx, idy, idz;
    for (idz = 0; idz < nr[2]; idz++) {
        for (idy = 0; idy < nr[1]; idy++) {
            for (idx = 0; idx < nr[0]; idx++) {
		if (isInside(idx, idy, idz)) {
                IdxMap[toCoordIdx(idx, idy, idz)] = id;
                CoordMap[id] = toCoordIdx(idx, idy, idz);
	        id++;
		}
            }
181 182
         }
     }
gsell's avatar
gsell committed
183 184
}

185
void ArbitraryDomain::Compute(Vector_t hr, NDIndex<3> localId){
186 187

    setHr(hr);
188

189 190 191 192
    globalMeanR_m = getGlobalMeanR();

    globalToLocalQuaternion_m = getGlobalToLocalQuaternion();
    localToGlobalQuaternion_m[0] = globalToLocalQuaternion_m[0];
193
    for (int i=1; i<4; i++)
194
		localToGlobalQuaternion_m[i] = -globalToLocalQuaternion_m[i];
195 196 197 198 199 200 201

    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
202

203 204 205 206 207 208 209 210 211
    hasGeometryChanged_m = true;

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

kraus's avatar
kraus committed
212
    //calculate intersection
213
    Vector_t P, saveP, dir, I;
214
    //Reference Point
215 216
    Vector_t P0 = geomCentroid_m;

217 218 219 220 221 222
    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];
223

224
		  P = saveP;
kraus's avatar
kraus committed
225
		  rotateWithQuaternion(P, localToGlobalQuaternion_m);
226
		  P += geomCentroid_m;
227 228

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

231
                     std::tuple<int, int, int> pos(idx, idy, idz);
232 233 234

		     rotateZAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
235 236
                        I -= geomCentroid_m;
  		        rotateWithQuaternion(I, globalToLocalQuaternion_m);
237 238
       	      		IntersectHiZ.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[2]));
		     } else {
239
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
240 241 242 243 244
			   *gmsg << "zdir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)) {
245
                        I -= geomCentroid_m;
kraus's avatar
kraus committed
246
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
247
       	      		IntersectLoZ.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[2]));
248
		     } else {
249
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
250 251 252
			   *gmsg << "zdir=-1 " << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
	  	     }
kraus's avatar
kraus committed
253

254 255
	             rotateYAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
256
                         I -= geomCentroid_m;
kraus's avatar
kraus committed
257
			 rotateWithQuaternion(I, globalToLocalQuaternion_m);
258 259
       	      		 IntersectHiY.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[1]));
	   	     } else {
260
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
261 262 263 264 265
			   *gmsg << "ydir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)) {
266
                        I -= geomCentroid_m;
kraus's avatar
kraus committed
267
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
268 269
       	      		IntersectLoY.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[1]));
		     } else {
270
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
271 272 273 274 275 276
			   *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)) {
277
                        I -= geomCentroid_m;
kraus's avatar
kraus committed
278
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
279 280
       	      		IntersectHiX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		     } else {
281
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
282
			   *gmsg << "xdir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
kraus's avatar
kraus committed
283
#endif
284 285 286
		     }

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)){
287
                        I -= geomCentroid_m;
kraus's avatar
kraus committed
288
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
289 290
       	      		IntersectLoX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		     } else {
291
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
292
			   *gmsg << "xdir=-1 " << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
kraus's avatar
kraus committed
293
#endif
294
		     }
295 296 297
		  } else {
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
			   *gmsg << "OUTSIDE" << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
kraus's avatar
kraus committed
298
#endif
299
		  }
300
	     }
301 302
	 }
     }
303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332
    //number of ghost nodes to the left
    int numGhostNodesLeft = 0;
    if(localId[2].first() != 0) {
        for(int idx = 0; idx < nr[0]; idx++) {
            for(int idy = 0; idy < nr[1]; idy++) {
                if(isInside(idx, idy, localId[2].first() - zGhostOffsetLeft))
                    numGhostNodesLeft++;
            }
        }
    }

    //xy points in z plane
    int numxy = 0;
    int numtotal = 0;
    numXY.clear();
    for(int idz = localId[2].first(); idz <= localId[2].last(); idz++) {
        numxy = 0;
        for(int idx = 0; idx < nr[0]; idx++) {
            for(int idy = 0; idy < nr[1]; idy++) {
                if(isInside(idx, idy, idz))
                    numxy++;
            }
        }
        numXY[idz-localId[2].first()] = numxy;
        numtotal += numxy;
    }

    int startIdx = 0;
    MPI_Scan(&numtotal, &startIdx, 1, MPI_INTEGER, MPI_SUM, MPI_COMM_WORLD);
    startIdx -= numtotal;
333

334
    //build up index and coord map
335 336
    IdxMap.clear();
    CoordMap.clear();
337 338 339 340 341 342 343 344 345 346
    register int index = startIdx - numGhostNodesLeft;

    for(int idz = localId[2].first() - zGhostOffsetLeft; idz <= localId[2].last() + zGhostOffsetRight; idz++) {
        for(int idy = 0; idy < nr[1]; idy++) {
            for(int idx = 0; idx < nr[0]; idx++) {
                if(isInside(idx, idy, idz)) {
                    IdxMap[toCoordIdx(idx, idy, idz)] = index;
                    CoordMap[index] = toCoordIdx(idx, idy, idz);
                    index++;
                }
347 348

            }
349 350
        }
    }
351
}
352

353 354
// Conversion from (x,y,z) to index in xyz plane
inline int ArbitraryDomain::toCoordIdx(int idx, int idy, int idz) {
355
    return (idz * nr[1] + idy) * nr[0]  + idx;
gsell's avatar
gsell committed
356 357
}

358 359
// Conversion from (x,y,z) to index on the 3D grid
int ArbitraryDomain::getIdx(int idx, int idy, int idz) {
360 361 362 363
     if(isInside(idx, idy, idz) && idx >= 0 && idy >= 0 && idz >= 0)
         return IdxMap[toCoordIdx(idx, idy, idz)];
     else
         return -1;
kraus's avatar
kraus committed
364
}
gsell's avatar
gsell committed
365

366 367 368 369 370 371 372 373
// Conversion from a 3D index to (x,y,z)
inline void ArbitraryDomain::getCoord(int idxyz, int &idx, int &idy, int &idz) {
    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
374 375
}

376
inline bool ArbitraryDomain::isInside(int idx, int idy, int idz) {
377
    Vector_t P;
378

379 380 381
    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];
382

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

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

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

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

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

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

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

    return ret;
gsell's avatar
gsell committed
416 417 418
}

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

420 421
	return numXY[z];
}
gsell's avatar
gsell committed
422 423


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

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

431
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
432

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

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

451
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) {
452 453 454 455 456 457 458 459 460

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

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

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

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

kraus's avatar
kraus committed
477
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)
478
{
479
    scaleFactor = 1;
480

481 482 483
    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];
484

485 486 487 488 489 490
    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];
491 492
    C = 0.0;

493 494
    std::tuple<int, int, int> coordxyz(idx, idy, idz);
    std::multimap < std::tuple<int, int, int>, double >::iterator itrH, itrL;
495

496 497 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
    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;
566
}
gsell's avatar
gsell committed
567

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

570
    int idx = 0, idy = 0, idz = 0;
gsell's avatar
gsell committed
571

572 573
    getCoord(id, idx, idy, idz);
    getNeighbours(idx, idy, idz, W, E, S, N, F, B);
gsell's avatar
gsell committed
574 575
}

576
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
577

578 579 580 581 582 583
    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);
584

kraus's avatar
kraus committed
585
    if(!isInside(idx+1,idy,idz))
gsell's avatar
gsell committed
586 587
        E = -1;

588 589 590 591
    if(!isInside(idx-1,idy,idz))
        W = -1;

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

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

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

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

}


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

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

    v = 2.0 * dot(quaternionVectorComponent, v) * quaternionVectorComponent
        + (quaternionScalarComponent * quaternionScalarComponent
        -  dot(quaternionVectorComponent, quaternionVectorComponent)) * v
622 623 624
        + 2.0 * quaternionScalarComponent * cross(quaternionVectorComponent, v);
}

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

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

633 634 635 636
    v(1) = 2.0 * (quaternion(1) * quaternion(2) + quaternion(0) * quaternion(3));
    v(2) = 2.0 * (quaternion(1) * quaternion(3) - quaternion(0) * quaternion(2));
}

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

640
    v(0) = 2.0 * (quaternion(1) * quaternion(2) - quaternion(0) * quaternion(3));
kraus's avatar
kraus committed
641 642

    v(1) = quaternion(0) * quaternion(0)
643 644 645 646 647 648 649
         - quaternion(1) * quaternion(1)
         + quaternion(2) * quaternion(2)
         - quaternion(3) * quaternion(3);

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

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

kraus's avatar
kraus committed
655
    v(2) = quaternion(0) * quaternion(0)
656 657 658 659 660
         - quaternion(1) * quaternion(1)
         - quaternion(2) * quaternion(2)
         + quaternion(3) * quaternion(3);

}
661
#endif //#ifdef HAVE_SAAMG_SOLVER