ArbitraryDomain.cpp 22.9 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();

212
    // Calculate intersection
213
    Vector_t P, saveP, dir, I;
214 215 216 217 218 219 220

    // Get minimum coordinates of boundary geometry
    Vector_t geomMinCoords = bgeom_m->getmincoords();

    // Reference Point that is inside (allowed particle region)
    // This is dangerous, who knows if this is really an "inside" point? -DW
    // TODO: Change to more generalized approach
221 222
    Vector_t P0 = geomCentroid_m;

223
    for (int idz = localId[2].first()-zGhostOffsetLeft; idz <= localId[2].last()+zGhostOffsetRight; idz++) {
224 225 226 227 228
	 //saveP[2] = (idz - (nr[2]-1)/2.0)*hr[2];
	 saveP[2] = geomMinCoords[2] + (idz + 0.5) * 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];
	     saveP[1] = geomMinCoords[1] + (idy + 0.5) * hr[1];
229
    	     for (int idx = localId[0].first()-xGhostOffsetLeft; idx <= localId[0].last()+xGhostOffsetRight; idx++) {
230 231 232 233 234
		  // We cannot assume that the geometry is symmetric about the xy, xz, and yz planes!
                  // In my spiral inflector simulation, this is not the case for z direction for 
                  // example. -DW   
	       	  // saveP[0] = (idx - (nr[0]-1)/2.0)*hr[0];
		  saveP[0] = geomMinCoords[0] + (idx + 0.5) * hr[0];
235

236
		  P = saveP;
237

kraus's avatar
kraus committed
238
		  rotateWithQuaternion(P, localToGlobalQuaternion_m);
239 240
		  //P += geomCentroid_m; //sorry, this doesn't make sense. -DW
                  P += globalMeanR_m;
241 242

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

244 245 246 247 248 249 250
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
		     *gmsg << "INSIDE" << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
                     // Commenting this out seems to reduce the number of INSIDE points that fail
		     // the intersection test. It was meant to make the algorithm faster, but
		     // is not strictly necessary... -DW
		     //P0 = P;
251

252 253
                     std::tuple<int, int, int> pos(idx, idy, idz);
		     
254
		     rotateZAxisWithQuaternion(dir, localToGlobalQuaternion_m);
255

256
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
257 258 259
			//I -= geomCentroid_m;
			I -= globalMeanR_m;
                        rotateWithQuaternion(I, globalToLocalQuaternion_m);
260 261
       	      		IntersectHiZ.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[2]));
		     } else {
262
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
263 264 265 266 267
			   *gmsg << "zdir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)) {
268 269
			//I -= geomCentroid_m;
			I -= globalMeanR_m;
kraus's avatar
kraus committed
270
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
271
       	      		IntersectLoZ.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[2]));
272
		     } else {
273
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
274 275 276
			   *gmsg << "zdir=-1 " << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
	  	     }
kraus's avatar
kraus committed
277

278
	             rotateYAxisWithQuaternion(dir, localToGlobalQuaternion_m);
279

280
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
281 282
			 //I -= geomCentroid_m;
			 I -= globalMeanR_m;
kraus's avatar
kraus committed
283
			 rotateWithQuaternion(I, globalToLocalQuaternion_m);
284 285
       	      		 IntersectHiY.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[1]));
	   	     } else {
286
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
287 288 289 290 291
			   *gmsg << "ydir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }

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

304
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
305 306
			//I -= geomCentroid_m;
			I -= globalMeanR_m;
kraus's avatar
kraus committed
307
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
308 309
       	      		IntersectHiX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		     } else {
310
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
311
			   *gmsg << "xdir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
kraus's avatar
kraus committed
312
#endif
313 314 315
		     }

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)){
316 317
		        //I -= geomCentroid_m;
			I -= globalMeanR_m;
kraus's avatar
kraus committed
318
			rotateWithQuaternion(I, globalToLocalQuaternion_m);
319 320
       	      		IntersectLoX.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 327
		  } else {
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
			   *gmsg << "OUTSIDE" << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
kraus's avatar
kraus committed
328
#endif
329
		  }
330
	     }
331 332
	 }
     }
333

334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363
    //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;
364

365
    //build up index and coord map
366 367
    IdxMap.clear();
    CoordMap.clear();
368 369 370 371 372 373 374 375 376 377
    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++;
                }
378 379

            }
380 381
        }
    }
382
}
383

384 385
// Conversion from (x,y,z) to index in xyz plane
inline int ArbitraryDomain::toCoordIdx(int idx, int idy, int idz) {
386
    return (idz * nr[1] + idy) * nr[0]  + idx;
gsell's avatar
gsell committed
387 388
}

389 390
// Conversion from (x,y,z) to index on the 3D grid
int ArbitraryDomain::getIdx(int idx, int idy, int idz) {
391 392 393 394
     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
395
}
gsell's avatar
gsell committed
396

397 398 399 400 401 402 403 404
// 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
405 406
}

407
inline bool ArbitraryDomain::isInside(int idx, int idy, int idz) {
408
    Vector_t P;
409

410 411 412
    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];
413

gsell's avatar
gsell committed
414
    bool ret = false;
415
    int  countH, countL;
416
    std::multimap < std::tuple<int, int, int>, double >::iterator itrH, itrL;
417
    std::tuple<int, int, int> coordxyz(idx, idy, idz);
kraus's avatar
kraus committed
418

gsell's avatar
gsell committed
419
    //check if z is inside with x,y coords
420 421
    itrH = IntersectHiZ.find(coordxyz);
    itrL = IntersectLoZ.find(coordxyz);
422

423 424 425
    countH = IntersectHiZ.count(coordxyz);
    countL = IntersectLoZ.count(coordxyz);
    if(countH == 1 && countL == 1)
426
        ret = (P[2] <= itrH->second) && (P[2] >= itrL->second);
427 428

     //check if y is inside with x,z coords
429 430 431 432 433 434
    itrH = IntersectHiY.find(coordxyz);
    itrL = IntersectLoY.find(coordxyz);

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

437
    //check if x is inside with y,z coords
438 439 440 441 442 443
    itrH = IntersectHiX.find(coordxyz);
    itrL = IntersectLoX.find(coordxyz);

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

    return ret;
gsell's avatar
gsell committed
447 448 449
}

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

451 452
	return numXY[z];
}
gsell's avatar
gsell committed
453 454


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

458 459
    getCoord(idxyz, idx, idy, idz);
    getBoundaryStencil(idx, idy, idz, W, E, S, N, F, B, C, scaleFactor);
gsell's avatar
gsell committed
460 461
}

462
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
463

464
    scaleFactor = 1.0;
465
   // determine which interpolation method we use for points near the boundary
466 467
    switch(interpolationMethod){
    	case CONSTANT:
468
        	ConstantInterpolation(idx,idy,idz,W,E,S,N,F,B,C,scaleFactor);
469 470
        	break;
    	case LINEAR:
471
                LinearInterpolation(idx,idy,idz,W,E,S,N,F,B,C,scaleFactor);
472 473
        	break;
    	case QUADRATIC:
474
            //  QuadraticInterpolation(idx,idy,idz,W,E,S,N,F,B,C,scaleFactor);
475
        	break;
476 477 478 479 480 481
    }

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

482
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) {
483 484 485 486 487 488 489 490 491

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

492
    if(!isInside(idx-1,idy,idz))
493
        W = 0.0;
kraus's avatar
kraus committed
494
    if(!isInside(idx+1,idy,idz))
495
        E = 0.0;
496

497
    if(!isInside(idx,idy+1,idz))
498
        N = 0.0;
kraus's avatar
kraus committed
499
    if(!isInside(idx,idy-1,idz))
500
        S = 0.0;
kraus's avatar
kraus committed
501 502 503 504 505

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

kraus's avatar
kraus committed
508
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)
509
{
510
    scaleFactor = 1;
511

512 513 514
    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];
515

516 517 518 519 520 521
    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];
522 523
    C = 0.0;

524 525
    std::tuple<int, int, int> coordxyz(idx, idy, idz);
    std::multimap < std::tuple<int, int, int>, double >::iterator itrH, itrL;
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 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
    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;
597
}
gsell's avatar
gsell committed
598

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

601
    int idx = 0, idy = 0, idz = 0;
gsell's avatar
gsell committed
602

603 604
    getCoord(id, idx, idy, idz);
    getNeighbours(idx, idy, idz, W, E, S, N, F, B);
gsell's avatar
gsell committed
605 606
}

607
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
608

609 610 611 612 613 614
    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);
615

kraus's avatar
kraus committed
616
    if(!isInside(idx+1,idy,idz))
gsell's avatar
gsell committed
617 618
        E = -1;

619 620 621 622
    if(!isInside(idx-1,idy,idz))
        W = -1;

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

kraus's avatar
kraus committed
625
    if(!isInside(idx,idy-1,idz))
gsell's avatar
gsell committed
626 627
        S = -1;

kraus's avatar
kraus committed
628 629 630 631 632
    if(!isInside(idx,idy,idz-1))
	F = -1;

    if(!isInside(idx,idy,idz+1))
	B = -1;
gsell's avatar
gsell committed
633 634 635 636 637 638 639 640 641 642

}


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

643
inline void ArbitraryDomain::rotateWithQuaternion(Vector_t & v, Quaternion_t const quaternion) {
644 645 646 647 648
    // 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
649 650 651 652

    v = 2.0 * dot(quaternionVectorComponent, v) * quaternionVectorComponent
        + (quaternionScalarComponent * quaternionScalarComponent
        -  dot(quaternionVectorComponent, quaternionVectorComponent)) * v
653 654 655
        + 2.0 * quaternionScalarComponent * cross(quaternionVectorComponent, v);
}

656
inline void ArbitraryDomain::rotateXAxisWithQuaternion(Vector_t & v, Quaternion_t const quaternion) {
657
    // rotates the positive xaxis using a quaternion.
kraus's avatar
kraus committed
658 659 660 661

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

664 665 666 667
    v(1) = 2.0 * (quaternion(1) * quaternion(2) + quaternion(0) * quaternion(3));
    v(2) = 2.0 * (quaternion(1) * quaternion(3) - quaternion(0) * quaternion(2));
}

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

671
    v(0) = 2.0 * (quaternion(1) * quaternion(2) - quaternion(0) * quaternion(3));
kraus's avatar
kraus committed
672 673

    v(1) = quaternion(0) * quaternion(0)
674 675 676 677 678 679 680
         - quaternion(1) * quaternion(1)
         + quaternion(2) * quaternion(2)
         - quaternion(3) * quaternion(3);

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

681
inline void ArbitraryDomain::rotateZAxisWithQuaternion(Vector_t & v, Quaternion_t const quaternion) {
682 683
    // rotates the positive zaxis using a quaternion.
    v(0) = 2.0 * (quaternion(1) * quaternion(3) + quaternion(0) * quaternion(2));
kraus's avatar
kraus committed
684
    v(1) = 2.0 * (quaternion(2) * quaternion(3) - quaternion(0) * quaternion(1));
685

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

}
692
#endif //#ifdef HAVE_SAAMG_SOLVER