WAKEINFLUENCE.cpp

// %%%%%%%%%%%%% mexWAKEINFLUENCE function %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#include <iostream>
#include <cstdio>
#include <cmath>
//#include <strings.h>

#include <cassert>
#include <atomic>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <vector>
#include <chrono>

#ifdef FSI_OPENMP
#include <omp.h>
#endif
#ifdef FSI_MPI
#include <mpi.h>
#endif

#include <fsi_thread.h>

using namespace std;
using namespace chrono;

#define X 0
#define Y 1
#define Z 2

extern condition_variable cv_parent;
extern vector<double **> tandem_output;
extern int world_rank;

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 * This function calculate the influence of wake doublets on the
 * wing panel's collocation points for 3D constant doublet and source
 * Cpanel method. This was ported from a MEX-file for MATLAB.
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

static mutex false_sharing_mtx;
#ifdef FSI_MEX_DEBUG
static FILE *fp;
#endif

static inline void INFLUENCE(double AB[] ,double colx,double coly,double colz, double x1,double y1,double z1,double x2,double y2,
        double z2,double x3,double y3,double z3,double x4,double y4,double z4,double miu)
{
    //fprintf(fp,"in INFLUENCE: %1.15g %1.15g %1.15g %1.15g %1.15g %1.15g %1.15g %1.15g %1.15g %1.15g %1.15g %1.15g %1.15g %1.15g %1.15g %1.15g \n",
//colx,coly,colz, x1,y1,z1,x2,y2, z2,x3,y3,z3,x4,y4,z4,miu);

    // Definition of the gobla constant
    double A1,A2,A3,AA,tx[3],ty[3],n[3],b1,b2,b3,bt[3],bb,v1,v2,v3,vv,tt;
    double e11,e22,e33,f1,f2,f3,s11,s12,s13,s21,s22,s23,s;
    double FF,FF_sqr,eror,fourpi,pi;
    double xc,yc,zc;
    double xl1,xl2,xl3,xl4;
    double yl1,yl2,yl3,yl4;
    double cpx,cpy,cpz;
    double d1,d2,d3,d4;
    //double a1,a2,b11,b22;
    double rad;
    double cpx1,cpx2,cpx3,cpx4;
    double cpy1,cpy2,cpy3,cpy4;
    double e1,e2,e3,e4;
    double r1,r2,r3,r4;
    double x21,x32,x43,x14;
    double y21,y32,y43,y14;
    double h1,h2,h3,h4;
    //double D1,D2;
// calculating ch,Aordwise tangent
    A1 =((x4+x3)-(x1+x2))/2;
    A2 =((y4+y3)-(y1+y2))/2;
    A3 =((z4+z3)-(z1+z2))/2;
    AA =sqrt(A1*A1+A2*A2+A3*A3);
    tx[0] =A1/AA;
    tx[1] =A2/AA;
    tx[2] =A3/AA;
// next another vector in this plan
    b1 = x2 - x1;
    b2 = y2 - y1;
    b3 = z2 - z1;
    bb=sqrt(b1*b1+b2*b2+b3*b3);
//d4 = sqrt((x1-x4)^2 + (y1-y4)^2+(z1-z4)^2);
    bt[0]=b1/bb;
    bt[1]=b2/bb;
    bt[2]=b3/bb;
// normal vector
    v1 = tx[1] *bt [2]    - tx[2]*bt[1];
    v2 = tx[2] *bt [0]    - tx[0]*bt[2];
    v3 = tx[0] *bt [1]    - tx[1]*bt[0];
    vv=sqrt(v1*v1+v2*v2+v3*v3);
    n[0]=v1/vv;
    n[1]=v2/vv;
    n[2]=v3/vv;
// tangential vector in spanwise direction
    ty[0] =  n[1]*tx[2]  - n[2]*tx[1];
    ty[1] =  n[2]*tx[0]  - n[0]*tx[2];
    ty[2] =  n[0]*tx[1]  - n[1]*tx[0];

    tt=sqrt(ty[0]*ty[0]+ty[1]*ty[1]+ty[2]*ty[2]);
    ty[0] = ty[0]/tt;
    ty[1] = ty[1]/tt;
    ty[2] = ty[2]/tt;

// calculation of area

    e11=x3-x1;
    e22=y3-y1;
    e33=z3-z1;
    f1=x2-x1;
    f2=y2-y1;
    f3=z2-z1;
//normal area
    s11=f2*b3-f3*b2;
    s12=b1*f3-f1*b3;
    s13=f1*b2-f2*b1;
    s21=b2*e33-b3*e22;
    s22=e11*b3-b1*e33;
    s23=b1*e22-b2*e11;
    s=0.5*(sqrt(s11*s11+s12*s12+s13*s13)+sqrt(s21*s21+s22*s22+s23*s23));

    pi=3.14159265358979;
    FF=5;
    FF_sqr = FF*FF;
    eror=1.0e-11;
    fourpi=miu/4/pi;



// coetroied of the qurd

    xc=0.25*(x1+x2+x3+x4);
    yc=0.25*(y1+y2+y3+y4);
    zc=0.25*(z1+z2+z3+z4);
// panel node coordinates (into local CS)

    xl1 = (x1-xc)*tx[0] + (y1-yc)*tx[1] + (z1-zc)*tx[2];
    xl2 = (x2-xc)*tx[0] + (y2-yc)*tx[1] + (z2-zc)*tx[2];
    xl3 = (x3-xc)*tx[0] + (y3-yc)*tx[1] + (z3-zc)*tx[2];
    xl4 = (x4-xc)*tx[0] + (y4-yc)*tx[1] + (z4-zc)*tx[2];

    yl1 = (x1-xc)*ty[0] + (y1-yc)*ty[1] + (z1-zc)*ty[2];
    yl2 = (x2-xc)*ty[0] + (y2-yc)*ty[1] + (z2-zc)*ty[2];
    yl3 = (x3-xc)*ty[0] + (y3-yc)*ty[1] + (z3-zc)*ty[2];
    yl4 = (x4-xc)*ty[0] + (y4-yc)*ty[1] + (z4-zc)*ty[2];


//transformation of influence points into LCS

    cpx = (colx-xc)*tx[0] + (coly-yc)*tx[1] +(colz-zc)*tx[2];
    cpy = (colx-xc)*ty[0] + (coly-yc)*ty[1] +(colz-zc)*ty[2];
    cpz = (colx-xc) *n[0] + (coly-yc) *n[1] +(colz-zc) *n[2];

// panel side lengths in local co-ordinate system

    d1 = sqrt((xl2-xl1)*(xl2-xl1) + (yl2-yl1)*(yl2-yl1));
    d2 = sqrt((xl3-xl2)*(xl3-xl2) + (yl3-yl2)*(yl3-yl2));
    d3 = sqrt((xl4-xl3)*(xl4-xl3) + (yl4-yl3)*(yl4-yl3));
    d4 = sqrt((xl1-xl4)*(xl1-xl4) + (yl1-yl4)*(yl1-yl4));
// calculation of the diagonals in local co-ordinate system

    //a1= (xl3-xl1);
    //a2= (yl3-yl1);
    //b11=-(xl2-xl4);
    //b22=-(yl2-yl4);
    //D1=sqrt(a1*a1+a2*a2);
    //D2=sqrt(b11*b11+b22*b22);
    // radiou vector
    rad = ((cpx-xc)*(cpx-xc) + (cpy-yc)*(cpy-yc) + (cpz)*(cpz));

    cpx1 = cpx - xl1;
    cpx2 = cpx - xl2;
    cpx3 = cpx - xl3;
    cpx4 = cpx - xl4;

    cpy1 = cpy - yl1;
    cpy2 = cpy - yl2;
    cpy3 = cpy - yl3;
    cpy4 = cpy - yl4;

    e1 = cpx1*cpx1+cpz*cpz;
    e2 = cpx2*cpx2+cpz*cpz;
    e3 = cpx3*cpx3+cpz*cpz;
    e4 = cpx4*cpx4+cpz*cpz;

    r1 = sqrt(e1 + cpy1*cpy1);
    r2 = sqrt(e2 + cpy2*cpy2);
    r3 = sqrt(e3 + cpy3*cpy3);
    r4 = sqrt(e4 + cpy4*cpy4);

    x21 = xl2-xl1;
    x32 = xl3-xl2;
    x43 = xl4-xl3;
    x14 = xl1-xl4;

    y21 = yl2-yl1;
    y32 = yl3-yl2;
    y43 = yl4-yl3;
    y14 = yl1-yl4;

    h1 = cpx1*cpy1;
    h2 = cpx2*cpy2;
    h3 = cpx3*cpy3;
    h4 = cpx4*cpy4;

    /* if distance of panel from influenced point is greater
     * then product of longer diagonal and "far field" coefficient
     * is assumed.
     */

// Starting of the loop
    if (sqrt(rad) >  5*FF_sqr)
    {
        AB[0] = -fourpi*s*cpz*pow(rad,-1.5);
        // AB[1] = -fourpi*s/sqrt(rad);
        return;
    }
#if 0  // B not used
    else if (sqrt(cpz*cpz)< eror)
    {
        if (d1 < eror) {
            b[0] = 0;
        } else {
            b[0] = (cpx1*y21-cpy1*x21)/d1*log((r1+r2+d1)/(r1+r2-d1));
        }
        if (d2 < eror) {
            b[1] = 0;
        } else {
            b[1] = (cpx2*y32-cpy2*x32)/d2*log((r2+r3+d2)/(r2+r3-d2));
        }
        if (d3 < eror) {
            b[2] = 0;
        } else {
            b[2] = (cpx3*y43-cpy3*x43)/d3*log((r3+r4+d3)/(r3+r4-d3));
        }
#if 0
        if (d4 < eror) {
            b[3] = 0;
        } else {
            b[3] = (cpx4*y14-cpy4*x14)/d4*log((r4+r1+d4)/(r4+r1-d4));
        }
#endif
        AB[0] = 0.0;
        AB[1] = -(b[0]+b[1]+b[2]+b[3])*fourpi;

    }
#endif
    else {
        double a[4];
        if (d1 < eror) {
            a[0] = 0;
            //b[0] = 0;
        } else {
            const double F1 = y21*e1 - x21*h1,
                         G1 = y21*e2 - x21*h2;
            a[0] = atan2(cpz*x21*(F1*r2-G1*r1),(cpz*cpz*x21*x21*r1*r2+F1*G1));
            //b[0] = (cpx1*y21-cpy1*x21)/d1*log((r1+r2+d1)/(r1+r2-d1));
        }
        if (d2 < eror) {
            a[1] = 0;
            //b[1] = 0;
        } else {
            const double F2 = y32*e2 - x32*h2,
                         G2 = y32*e3 - x32*h3;
            a[1] = atan2(cpz*x32*(F2*r3-G2*r2),(cpz*cpz*x32*x32*r2*r3+F2*G2));

            //b[1] = (cpx2*y32-cpy2*x32)/d2*log( (r2+r3+d2)/(r2+r3-d2));
        }
        if (d3 < eror) {
            a[2] = 0;
            //b[2] = 0;
        } else {
            const double F3 = y43*e3 - x43*h3,
                         G3 = y43*e4 - x43*h4;
            a[2] = atan2(cpz*x43*(F3*r4-G3*r3),(cpz*cpz*x43*x43*r3*r4+F3*G3));
            //b[2] = (cpx3*y43-cpy3*x43)/d3*log( (r3+r4+d3)/(r3+r4-d3));
        }
        if (d4 < eror) {
            a[3] = 0;
            //b[3] = 0;
        } else {
            const double F4 = y14*e4 - x14*h4,
                         G4 = y14*e1 - x14*h1;
            a[3] = atan2(cpz*x14*(F4*r1-G4*r4),(cpz*cpz*x14*x14*r4*r1+F4*G4));
            //b[3] = (cpx4*y14-cpy4*x14)/d4*log( (r4+r1+d4)/(r4+r1-d4));
        }
        AB[0] = (a[0]+a[1]+a[2]+a[3])*fourpi;
        // AB[1] = -(b[0]+b[1]+b[2]+b[3])*fourpi+cpz*AB[0] ;
    }
}

    //&Q[i * 3 + nts3 * j],
#define CINIT(Q, pWstart, nts3) const double \
    *p##Q##ij = pWstart, \
    *p##Q##i1j = p##Q##ij + 3, \
    *p##Q##ij1 = p##Q##ij + nts3, \
    *p##Q##i1j1 = p##Q##i1j + nts3

#define ITER(Q) \
    p##Q##ij = p##Q##i1j, \
    p##Q##i1j += 3, \
    p##Q##ij1 = p##Q##i1j1, \
    p##Q##i1j1 += 3

#define XYZOFFSET(Q, offset) \
    p##Q##offset[X], \
    p##Q##offset[Y], \
    p##Q##offset[Z]

inline double *
wakeinfluence_thread::doublet(double * const A,
                              const double xicjc, const double yicjc, const double zicjc,
                              const double * const W,
                              const double * const MUEW,
                              const int tm1, const int n)
{
    double *pA;
    const double *pWstart = &W[1 * 3],  // skip first element for some reason
                 *pMUEWstart = &MUEW[1];
    pA = A;
    for (int j = 0; j < n; j++) {
        CINIT(W, pWstart, nts3);
        pWstart += nts3;  // next span-wise panel
        //cerr << "pW - W=" << pWij - W << endl;
        //const double *pMUEW = &MUEW[1 + nts * j];
        const double *pMUEW = pMUEWstart;
        pMUEWstart += nts;
        //cerr << "pMUEW - MUEW=" << pMUEW - MUEW << endl;
        // *(A+(ic*ny+jc)+(i*ny+j)*nxny)=AB[0];
        for (int i = 1; i < tm1; i++, ITER(W), pMUEW++, pA++) {
            //cerr << "pA - A=" << pA - A << endl;
            INFLUENCE(pA,
                      xicjc, yicjc, zicjc,
                      XYZOFFSET(W, ij),
                      XYZOFFSET(W, ij1),
                      XYZOFFSET(W, i1j1),
                      XYZOFFSET(W, i1j),
                      *pMUEW);
            //cerr << "it=" << i << " in=" << j << " pA=" << pA - A << " val=" << *pA << endl;
            //if (world_rank == -1) cerr << "it=" << i << " in=" << j << " pA=" << pA - A << " val=" << *pA << endl;
#ifdef FSI_MEX_DEBUG
            if (fp != nullptr) fprintf(fp,"AB[0]=%f\n%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f \n",
                      *pA,
                      xicjc, yicjc, zicjc,
                      XYZOFFSET(W, ij),
                      XYZOFFSET(W, ij1),
                      XYZOFFSET(W, i1j1),
                      XYZOFFSET(W, i1j),
                      *pMUEW);
#endif
        }
    }
    return pA;  // note: only meaningful for packed buffer
}

#ifdef FSI_MEX_DEBUG
static void outbug(const int t, const char *label) {
    if (world_rank != 0) {
        char buf[256];
        snprintf(buf, sizeof buf - 1,
#ifdef _WIN32
        "f:/data/WAKEINFLUENCE_T%d.dat",
#else
        "/home/wm/ml/WLM_PM_CODE/data/WAKEINFLUENCE_T%d.dat",
#endif
    t + 1);
        fp = fopen(buf, world_rank == -1 ? "w" : "a");  // for MPI, this is called repeatedly, so append
        if (fp == nullptr) cerr << "Couldn't open %b " << buf << endl;
        else {
            fprintf(fp, "%s\n", label);
            cerr << "Writing " << label << " WAKEINFLUENCE debugging file: " << buf << endl;
        }
    }
}
#endif

void wakeinfluence_thread::mex_thread_init(const int t, const double *sig, double *A) {

    this->t = t;
    this->sig = sig;
    this->A = A;

    order = 0;
    threads_finished = 0;
#ifdef ALTERNATE_WORKINC
    workinc = mn / nthreads;
    if (mn % nthreads != 0) workinc++;
    if (workinc <= 0) workinc = 1;
    //cerr << "wakeinfluence:workinc=" << workinc << endl;
#endif

#ifdef FSI_MEX_DEBUG
    outbug(t, "threaded");
#endif

#ifdef FSI_OPENMP
    TSTART(mex_time, steady_clock::now());  // start mex timing
    omp_set_num_threads(nthreads);
    #pragma omp parallel
    {
        const int thrdid = omp_get_thread_num();
#ifdef FSI_STATS
        int ndoublets = 0;
        const steady_clock::time_point start = steady_clock::now();
#ifdef FSI_OPENMP
        int openmp_denied = 0;
        if (thrdid == 0) {  // only for one thread
            int nthrds = omp_get_num_threads();
            if (nthreads != nthrds) openmp_denied += nthreads - nthrds;
        }
#endif
#endif
        double * const Arecv = tandem_output[thrdid][wakeinfluence_index];
        // compute, writing to packed private buffer to avoid false sharing caching invalidation
        int ord;
        while ((ord = order.fetch_add(workinc)) < mn) {  // while work remains
            double *pA = Arecv;
            // decode for row, col
            for (int i = 0, iord = ord; i < workinc && iord < mn; i++, iord++) {
                const int ic = iord / ny,
                          jc = iord % ny,
                          icjc = 3 * (nx * jc + ic);  // decode colocation index
                assert(pA - Arecv <= outsize);
                assert(icjc <= 3 * ((nx * ny) - 1));
                pA = doublet(pA,
                             C[icjc], C[icjc + Y], C[icjc + Z],
                             B, sig,
                             t - 1, ny);
#ifdef FSI_STATS
                ndoublets++;
#endif
            }
            // write computations from packed buffer
            const double *pArecv = Arecv;
            {  // start false sharing critical section
                #pragma omp critical
                for (int i = 0, iord = ord; i < workinc && iord < mn; i++, iord++) {
                    double *pAstart = &A[iord + nmn];
                    const int tm1 = t - 1;
                    for (int iy = 0; iy < ny; iy++) {
                        pA = pAstart;
                        pAstart += mn;  // set for next span-wise panel
                        // *(A+(ic*ny+jc)+(i*ny+j)*nxny)=AB[0];
                        for (int it = 1; it < tm1; it++, pA += nmn) {
                            //cerr << "it=" << it << " in=" << iy << " pA=" << pA - A << " val=" << *pArecv << endl;
                            assert(pA - A <= outsize);
                            *pA = *pArecv++;
                        }
                    }
                }
            }  // end false sharing critical section
        }  // while work remains
#ifdef FSI_STATS
        // per-thread statistics
        mex_stat *pmex_stat = (*thread_stat_map[thrdid])[this];
        pmex_stat->call_count++;
        pmex_stat->elapsed += duration_cast<mex_thread_time_units_t>(steady_clock::now() - start);
        pmex_stat->doublets += ndoublets;
#ifdef FSI_OPENMP
        if (openmp_denied != 0) pmex_stat->openmp_denied += openmp_denied;
#endif
#endif
    }  // end parallel
    TSTOP(mex_time, steady_clock::now());  // stop mex timing and add elapsed
#else  // low-level threading model
    mex_parent();
#endif
}

void wakeinfluence_thread::mex_thread_run (const int partner_rank) {

    double * const Arecv = tandem_output[partner_rank][wakeinfluence_index];
    //double * const Arecv = new double [outsize];
#ifdef FSI_MPI
    bool send_sig = true;
#endif
#ifdef FSI_STATS
    int ndoublets = 0;
    int nsyncs = 0;
#endif
    for (;;) {  // until no more data
        const int ord = order.fetch_add(workinc);
#ifdef FSI_STATS
        nsyncs++;
#endif
        if (ord >= mn) {
#ifdef FSI_STATS
#ifndef FSI_OPENMP
            // statistics
            mex_stat *pmex_stat = (*thread_stat_map[this_thread::get_id()])[this];
            pmex_stat->doublets += ndoublets;
            pmex_stat->nsyncs += nsyncs;
#endif
#endif
            //if (icjc == mn + nthreads - 1) {
            // handle synchronization with parent in class
            if (is_last_thread()) {
#ifdef FSI_MEX_DEBUG
                if (fp != nullptr) fclose(fp);
#endif
            }
            return;  // to wait state
        }
        double *pA;
#ifdef FSI_MPI
        // supply order, indicate WAKEINFLUENCE
        int status = MPI_Send(sig, send_sig ? sigsize : 0, MPI_DOUBLE, partner_rank, wakeinfluence_mask | ord, MPI_COMM_WORLD);
        assert(status == MPI_SUCCESS);
        send_sig = false;
        try {
            status = MPI_Recv(Arecv, outsize, MPI_DOUBLE, partner_rank, MPI_ANY_TAG, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
        }
        catch (exception& e) {
            cerr << "exception caught on MPI_Recv in WAKEINFLUENCE "
                    "Exception: " << e.what() << '\n';
        }
        assert(status == MPI_SUCCESS);

#else
        // compute, writing to packed private buffer to avoid false sharing caching invalidation
        pA = Arecv;
        // decode for row, col
        for (int i = 0, iord = ord; i < workinc && iord < mn; i++, iord++) {
            const int ic = iord / ny,
                      jc = iord % ny,
                      icjc = 3 * (nx * jc + ic);  // decode colocation index
            assert(pA - Arecv <= outsize);
            assert(icjc <= 3 * ((nx * ny) - 1));
            // #define CSIZE (3 * 2 * m * n)  // number of elements CXYZ
            pA = doublet(pA,
                         C[icjc], C[icjc + Y], C[icjc + Z],
                         B, sig,
                         t - 1, ny);
                         // *(A+(ic*ny+jc)+(i*ny+j)*nxny)=AB[0];
        }
#endif
        // write computations from packed buffer
        const double *pArecv = Arecv;
        {  // start false sharing critical section
            if (nthreads > 1) unique_lock<mutex> lck(false_sharing_mtx);
            for (int i = 0, iord = ord; i < workinc && iord < mn; i++, iord++) {
                double *pAstart = &A[iord + nmn];
                const int tm1 = t - 1;
                for (int iy = 0; iy < ny; iy++) {
                    pA = pAstart;
                    pAstart += mn;  // set for next span-wise panel
                    //cerr << "pA - A=" << pA - A << endl;
                    // *(A+(ic*ny+jc)+(i*ny+j)*nxny)=AB[0];
                    // mat C1(2 * m * n, nts * n); ZERO(C1);
                    for (int it = 1; it < tm1; it++, pA += nmn) {
                        //cerr << "it=" << it << " in=" << iy << " pA=" << pA - A << " val=" << *pArecv << endl;
                        assert(pA - A <= outsize);
                        *pA = *pArecv++;
                    }
                }
#ifdef FSI_STATS
                ndoublets++;
#endif
            }
        }  // end false sharing critical section
    }
}

#ifdef FSI_MPI
void wakeinfluence_thread::compute(int ord, const double * const psig, const int pt) {
    //oWAKEINFLUENCE.mex_thread_init(t, &MUEW1(1, 0), C1.memptr());
    // WAKEINFLUENCE - mat MUEW1(nts, n);
    // mat C1(2 * m * n, nts * n);

    //cerr << "wakeinfluence::init: world_rank=" << world_rank << endl;
#ifdef FSI_MEX_DEBUG
    outbug(pt, "compute");
#endif

    t = pt;

    // reserve space for both A and B
    if (A == nullptr) {
        //TODO - what about column consideration?
        A = new double [outsize]; //% Influence co-efficient matrix of surface doublet distribution.
    }
    int nwork;
    // decode for row, col
    double *pA = A;
    for (nwork = 0; nwork < workinc && ord < mn; nwork++, ord++) {
        const int ic = ord / ny,
                  jc = ord % ny,
                  icjc = 3 * (nx * jc + ic);  // decode colocation index TODO check this!!!
        pA = doublet(pA,
                     C[icjc], C[icjc + Y], C[icjc + Z],
                     B, psig,
                     t - 1, ny);
        // *(A+(ic*ny+jc)+(i*ny+j)*nxny)=AB[0];
    }
    assert(nwork * ny * (t - 1) <= outsize);
    assert(pA - A <= outsize);
    //mat C1(2 * m * n, nts * n);
    int status = MPI_Send(A, pA - A, MPI_DOUBLE, 0, 0, MPI_COMM_WORLD);
    assert(status == MPI_SUCCESS);
#ifdef FSI_MEX_DEBUG
    if (fp != nullptr) fclose(fp);
#endif
}
#endif