ArbitraryDomain.cpp 23.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
#include "Solvers/ArbitraryDomain.h"
snuverink_j's avatar
snuverink_j committed
18
#include "Structure/BoundaryGeometry.h"
kraus's avatar
kraus committed
19

20 21
#include <cmath>
#include <iostream>
snuverink_j's avatar
snuverink_j committed
22
#include <tuple>
23
#include <assert.h>
gsell's avatar
gsell committed
24

25 26
#include <math.h>

27 28 29
ArbitraryDomain::ArbitraryDomain( BoundaryGeometry * bgeom,
   	                          Vector_t nr,
	                          Vector_t hr,
30
	                          std::string interpl){
31 32 33
    bgeom_m  = bgeom;
    minCoords_m = bgeom->getmincoords();
    maxCoords_m = bgeom->getmaxcoords();
34
    geomCentroid_m = (minCoords_m + maxCoords_m)/2.0;
35 36

    // TODO: THis needs to be made into OPTION of the geometry.
37 38
    // A user defined point that is INSIDE with 100% certainty. -DW
    globalInsideP0_m = Vector_t(0.0, 0.0, -0.13);
39 40

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

    startId = 0;

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

55 56
ArbitraryDomain::~ArbitraryDomain() {
    //nothing so far
gsell's avatar
gsell committed
57 58
}

59 60
void ArbitraryDomain::compute(Vector_t hr){

61 62
    setHr(hr);

63
    globalMeanR_m = getGlobalMeanR();
gsell's avatar
gsell committed
64

65 66 67 68
    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
69

70 71
    hasGeometryChanged_m = true;

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

kraus's avatar
kraus committed
79
    //calculate intersection
80 81 82
    Vector_t P, saveP, dir, I;
    //Reference Point
    Vector_t P0 = geomCentroid_m;
83

84 85 86 87 88 89 90
    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;
91

92 93
		  rotateWithQuaternion(P, localToGlobalQuaternion_m);
		  P += geomCentroid_m;
94

95 96
		  if (bgeom_m->fastIsInside(P0, P) % 2 == 0) {
		     P0 = P;
97

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

100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119
		     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
	  	     }
120

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

132 133 134 135 136 137 138 139 140
		     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
		     }
141

142 143 144 145 146 147 148 149 150 151
	             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
		     }
152

153 154 155 156 157 158 159 160 161 162 163 164 165 166 167
		     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
		  }
	     }
168 169
	 }
     }
gsell's avatar
gsell committed
170 171
    IdxMap.clear();
    CoordMap.clear();
kraus's avatar
kraus committed
172

173 174
    int id=0;
    int idx, idy, idz;
175 176 177 178 179 180 181 182 183
    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++;
		}
            }
184 185
         }
     }
gsell's avatar
gsell committed
186 187
}

188
void ArbitraryDomain::compute(Vector_t hr, NDIndex<3> localId){
189

190 191
    INFOMSG(level2 << "* Starting the Boundary Intersection Tests..." << endl);

192
    setHr(hr);
193

194 195 196 197
    globalMeanR_m = getGlobalMeanR();

    globalToLocalQuaternion_m = getGlobalToLocalQuaternion();
    localToGlobalQuaternion_m[0] = globalToLocalQuaternion_m[0];
198
    for (int i=1; i<4; i++)
199
        localToGlobalQuaternion_m[i] = -globalToLocalQuaternion_m[i];
200 201 202 203 204 205 206

    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
207

208 209 210 211 212 213 214 215 216
    hasGeometryChanged_m = true;

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

217
    // Calculate intersection
218 219 220
    Vector_t P, dir, I;
    // Vector_t saveP, saveP_old;
    Vector_t P0 = globalInsideP0_m;
221

222
    // We cannot assume that the geometry is symmetric about the xy, xz, and yz planes!
223 224
    // In my spiral inflector simulation, this is not the case for z direction for
    // example (-0.13 to +0.025). -DW
225
    for (int idz = localId[2].first()-zGhostOffsetLeft; idz <= localId[2].last()+zGhostOffsetRight; idz++) {
226 227 228

	 //saveP_old[2] = (idz - (nr[2]-1)/2.0)*hr[2];
	 P[2] = minCoords_m[2] + (idz + 0.5) * hr[2];
229

230
         for (int idy = localId[1].first()-yGhostOffsetLeft; idy <= localId[1].last()+yGhostOffsetRight; idy++) {
231

232 233
	     //saveP_old[1] = (idy - (nr[1]-1)/2.0)*hr[1];
	     P[1] = minCoords_m[1] + (idy + 0.5) * hr[1];
234

235
   	     for (int idx = localId[0].first()-xGhostOffsetLeft; idx <= localId[0].last()+xGhostOffsetRight; idx++) {
236

237 238 239 240 241 242 243 244
	          //saveP_old[0] = (idx - (nr[0]-1)/2.0)*hr[0];
		  P[0] = minCoords_m[0] + (idx + 0.5) * hr[0];

		  // *gmsg << "Now working on point " << saveP << " (original was " << saveP_old << ")" << endl;

		  //P = saveP;

		  //rotateWithQuaternion(P, localToGlobalQuaternion_m);
245
		  //P += geomCentroid_m; //sorry, this doesn't make sense. -DW
246
                  //P += globalMeanR_m;
247 248

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

250 251 252
		      // Fill the map with true or false values for very fast isInside tests
                      // during the rest of the fieldsolve.
		      IsInsideMap[toCoordIdx(idx, idy, idz)] = true;
253

254 255 256 257 258 259 260 261
                      // Replace the old reference point with the new point (which we know is
		      // inside because we just tested for it. This makes the algorithm faster
		      // because fastIsInside() creates a number of segments that depends on the
		      // distance between P and P0. Using the previous P as the new P0
                      // assures the smallest possible distance in most cases. -DW
		      P0 = P;

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

263 264
		      //rotateZAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		      dir = Vector_t(0, 0, 1);
265

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

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

288 289
	             //rotateYAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		     dir = Vector_t(0, 1, 0);
290

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

		     if (bgeom_m->intersectRayBoundary(P, -dir, I)) {
303
			//I -= geomCentroid_m;
304 305
			//I -= globalMeanR_m;
			//rotateWithQuaternion(I, globalToLocalQuaternion_m);
306 307
       	      		IntersectLoY.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[1]));
		     } else {
308
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
309 310 311
			   *gmsg << "ydir=-1" << -dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
#endif
		     }
312

313 314
	             //rotateXAxisWithQuaternion(dir, localToGlobalQuaternion_m);
		     dir = Vector_t(1, 0, 0);
315

316
		     if (bgeom_m->intersectRayBoundary(P, dir, I)) {
317
			//I -= geomCentroid_m;
318 319
			//I -= globalMeanR_m;
			//rotateWithQuaternion(I, globalToLocalQuaternion_m);
320 321
       	      		IntersectHiX.insert(std::pair< std::tuple<int, int, int>, double >(pos, I[0]));
		     } else {
322
#ifdef DEBUG_INTERSECT_RAY_BOUNDARY
323
			   *gmsg << "xdir=+1 " << dir << " x,y,z= " << idx << "," << idy << "," << idz << " P=" << P <<" I=" << I << endl;
kraus's avatar
kraus committed
324
#endif
325 326 327
		     }

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

347 348
    INFOMSG(level2 << "* Finding number of ghost nodes to the left..." << endl);

349 350 351 352 353
    //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++) {
354
		    if(isInside(idx, idy, localId[2].first() - zGhostOffsetLeft))
355 356 357 358 359
                    numGhostNodesLeft++;
            }
        }
    }

360 361
    INFOMSG(level2 << "* Finding number of xy points in each plane along z..." << endl);

362 363 364 365
    //xy points in z plane
    int numtotal = 0;
    numXY.clear();
    for(int idz = localId[2].first(); idz <= localId[2].last(); idz++) {
snuverink_j's avatar
snuverink_j committed
366
        int numxy = 0;
367 368 369 370 371 372 373 374 375 376 377 378 379
        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;
380

381
    // Build up index and coord map
382 383
    IdxMap.clear();
    CoordMap.clear();
384
    int index = startIdx - numGhostNodesLeft;
385

386 387
    INFOMSG(level2 << "* Building up index and coordinate map..." << endl);

388 389 390
    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++) {
391
		if(isInside(idx, idy, idz)) {
392 393 394 395
                    IdxMap[toCoordIdx(idx, idy, idz)] = index;
                    CoordMap[index] = toCoordIdx(idx, idy, idz);
                    index++;
                }
396
            }
397 398
        }
    }
399 400

    INFOMSG(level2 << "* Done." << endl);
401
}
402

403 404
// Conversion from (x,y,z) to index in xyz plane
inline int ArbitraryDomain::toCoordIdx(int idx, int idy, int idz) {
405
    return (idz * nr[1] + idy) * nr[0]  + idx;
gsell's avatar
gsell committed
406 407
}

408 409
// Conversion from (x,y,z) to index on the 3D grid
int ArbitraryDomain::getIdx(int idx, int idy, int idz) {
410

411 412 413 414
     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
415
}
gsell's avatar
gsell committed
416

417 418 419 420 421 422 423 424
// 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
425 426
}

427
inline bool ArbitraryDomain::isInside(int idx, int idy, int idz) {
428

429 430 431 432 433 434 435 436
    return IsInsideMap[toCoordIdx(idx, idy, idz)];
}

/*
inline bool ArbitraryDomain::isInside(int idx, int idy, int idz) {
    Vector_t P;

    P[0] = minCoords_m[0] + (idx + 0.5) * hr[0];
437
    P[1] = minCoords_m[1] + (idy + 0.5) * hr[1];
438
    P[2] = minCoords_m[2] + (idz + 0.5) * hr[2];
439

440 441 442 443 444
    return (bgeom_m->fastIsInside(globalInsideP0_m, P) % 2 == 0);
}
*/

/*
445
inline bool ArbitraryDomain::isInside(int idx, int idy, int idz) {
446
    Vector_t P;
447

448 449 450
    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];
451

gsell's avatar
gsell committed
452
    bool ret = false;
453
    int  countH, countL;
454
    std::multimap < std::tuple<int, int, int>, double >::iterator itrH, itrL;
455
    std::tuple<int, int, int> coordxyz(idx, idy, idz);
kraus's avatar
kraus committed
456

gsell's avatar
gsell committed
457
    //check if z is inside with x,y coords
458 459
    itrH = IntersectHiZ.find(coordxyz);
    itrL = IntersectLoZ.find(coordxyz);
460

461 462 463
    countH = IntersectHiZ.count(coordxyz);
    countL = IntersectLoZ.count(coordxyz);
    if(countH == 1 && countL == 1)
464
        ret = (P[2] <= itrH->second) && (P[2] >= itrL->second);
465 466

     //check if y is inside with x,z coords
467 468 469 470 471 472
    itrH = IntersectHiY.find(coordxyz);
    itrL = IntersectLoY.find(coordxyz);

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

475
    //check if x is inside with y,z coords
476 477 478 479 480 481
    itrH = IntersectHiX.find(coordxyz);
    itrL = IntersectLoX.find(coordxyz);

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

    return ret;
gsell's avatar
gsell committed
485
}
486
*/
gsell's avatar
gsell committed
487 488

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

490 491
	return numXY[z];
}
gsell's avatar
gsell committed
492

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

496 497
    getCoord(idxyz, idx, idy, idz);
    getBoundaryStencil(idx, idy, idz, W, E, S, N, F, B, C, scaleFactor);
gsell's avatar
gsell committed
498 499
}

500
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
501

502
    scaleFactor = 1.0;
503
   // determine which interpolation method we use for points near the boundary
504 505
    switch(interpolationMethod){
    	case CONSTANT:
506
        	constantInterpolation(idx,idy,idz,W,E,S,N,F,B,C,scaleFactor);
507 508
        	break;
    	case LINEAR:
509
                linearInterpolation(idx,idy,idz,W,E,S,N,F,B,C,scaleFactor);
510 511
        	break;
    	case QUADRATIC:
512
            //  QuadraticInterpolation(idx,idy,idz,W,E,S,N,F,B,C,scaleFactor);
513
        	break;
514 515 516 517 518 519
    }

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

520
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) {
521 522 523 524 525 526 527 528 529

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

530
    if(!isInside(idx-1,idy,idz))
531
        W = 0.0;
kraus's avatar
kraus committed
532
    if(!isInside(idx+1,idy,idz))
533
        E = 0.0;
534

535
    if(!isInside(idx,idy+1,idz))
536
        N = 0.0;
kraus's avatar
kraus committed
537
    if(!isInside(idx,idy-1,idz))
538
        S = 0.0;
kraus's avatar
kraus committed
539 540 541 542 543

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

546
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)
547
{
548
    scaleFactor = 1;
549

550 551 552
    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];
553

554
    double dx_w=hr[0];
555 556
    double dx_e=hr[0];
    double dy_n=hr[1];
557 558 559
    double dy_s=hr[1];
    double dz_f=hr[2];
    double dz_b=hr[2];
560 561
    C = 0.0;

562
    std::tuple<int, int, int> coordxyz(idx, idy, idz);
563

564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633
    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;
634
}
gsell's avatar
gsell committed
635

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

638
    int idx = 0, idy = 0, idz = 0;
gsell's avatar
gsell committed
639

640 641
    getCoord(id, idx, idy, idz);
    getNeighbours(idx, idy, idz, W, E, S, N, F, B);
gsell's avatar
gsell committed
642 643
}

644
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
645

646 647 648 649 650 651
    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);
652

kraus's avatar
kraus committed
653
    if(!isInside(idx+1,idy,idz))
gsell's avatar
gsell committed
654 655
        E = -1;

656 657 658 659
    if(!isInside(idx-1,idy,idz))
        W = -1;

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

kraus's avatar
kraus committed
662
    if(!isInside(idx,idy-1,idz))
gsell's avatar
gsell committed
663 664
        S = -1;

kraus's avatar
kraus committed
665 666 667 668 669
    if(!isInside(idx,idy,idz-1))
	F = -1;

    if(!isInside(idx,idy,idz+1))
	B = -1;
gsell's avatar
gsell committed
670 671 672 673 674 675 676 677 678 679

}


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

680
inline void ArbitraryDomain::rotateWithQuaternion(Vector_t & v, Quaternion_t const quaternion) {
681 682 683 684 685
    // 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
686 687 688 689

    v = 2.0 * dot(quaternionVectorComponent, v) * quaternionVectorComponent
        + (quaternionScalarComponent * quaternionScalarComponent
        -  dot(quaternionVectorComponent, quaternionVectorComponent)) * v
690 691 692
        + 2.0 * quaternionScalarComponent * cross(quaternionVectorComponent, v);
}

693
inline void ArbitraryDomain::rotateXAxisWithQuaternion(Vector_t & v, Quaternion_t const quaternion) {
694
    // rotates the positive xaxis using a quaternion.
kraus's avatar
kraus committed
695 696 697 698

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

701 702 703 704
    v(1) = 2.0 * (quaternion(1) * quaternion(2) + quaternion(0) * quaternion(3));
    v(2) = 2.0 * (quaternion(1) * quaternion(3) - quaternion(0) * quaternion(2));
}

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

708
    v(0) = 2.0 * (quaternion(1) * quaternion(2) - quaternion(0) * quaternion(3));
kraus's avatar
kraus committed
709 710

    v(1) = quaternion(0) * quaternion(0)
711 712 713 714 715 716 717
         - quaternion(1) * quaternion(1)
         + quaternion(2) * quaternion(2)
         - quaternion(3) * quaternion(3);

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

718
inline void ArbitraryDomain::rotateZAxisWithQuaternion(Vector_t & v, Quaternion_t const quaternion) {
719 720
    // rotates the positive zaxis using a quaternion.
    v(0) = 2.0 * (quaternion(1) * quaternion(3) + quaternion(0) * quaternion(2));
kraus's avatar
kraus committed
721
    v(1) = 2.0 * (quaternion(2) * quaternion(3) - quaternion(0) * quaternion(1));
722

kraus's avatar
kraus committed
723
    v(2) = quaternion(0) * quaternion(0)
724 725 726 727 728
         - quaternion(1) * quaternion(1)
         - quaternion(2) * quaternion(2)
         + quaternion(3) * quaternion(3);

}
729
#endif //#ifdef HAVE_SAAMG_SOLVER