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

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

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

    startId = 0;

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

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

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

62 63
    setHr(hr);

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

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

71 72
    hasGeometryChanged_m = true;

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
    Vector_t P, saveP, dir, I;
    //Reference Point
    Vector_t P0 = geomCentroid_m;
84

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

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

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

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

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

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

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

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

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

174 175 176 177 178 179 180 181 182 183 184
    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++;
		}
            }
185 186
         }
     }
gsell's avatar
gsell committed
187 188
}

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

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

193
    setHr(hr);
194

195 196 197 198
    globalMeanR_m = getGlobalMeanR();

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

    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
208

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

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

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

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

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

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

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

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

238 239 240 241 242 243 244 245
	          //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);
246
		  //P += geomCentroid_m; //sorry, this doesn't make sense. -DW
247
                  //P += globalMeanR_m;
248 249

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

251 252 253
		      // 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;
254

255 256 257 258 259 260 261 262
                      // 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);
263

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

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

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

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

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

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

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

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

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

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

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

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

363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381
    //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;
382

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

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

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

    INFOMSG(level2 << "* Done." << endl);
403
}
404

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

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

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

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

429
inline bool ArbitraryDomain::isInside(int idx, int idy, int idz) {
430

431 432 433 434 435 436 437 438
    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];
439
    P[1] = minCoords_m[1] + (idy + 0.5) * hr[1];
440
    P[2] = minCoords_m[2] + (idz + 0.5) * hr[2];
441

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

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

450 451 452
    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];
453

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

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

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

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

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

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

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

    return ret;
gsell's avatar
gsell committed
487
}
488
*/
gsell's avatar
gsell committed
489 490

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

492 493
	return numXY[z];
}
gsell's avatar
gsell committed
494

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

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

502
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
503

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

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

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

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

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

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

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

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

552 553 554
    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];
555

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

564
    std::tuple<int, int, int> coordxyz(idx, idy, idz);
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 634 635
    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;
636
}
gsell's avatar
gsell committed
637

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

640
    int idx = 0, idy = 0, idz = 0;
gsell's avatar
gsell committed
641

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

646
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
647

648 649 650 651 652 653
    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);
654

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

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

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

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

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

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

}


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

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

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

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

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

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

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

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

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

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

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

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

}
731
#endif //#ifdef HAVE_SAAMG_SOLVER