Bugfix in search_for_outside_edge routine.

[voro++.git] / trunk / zeo / v_network.cc

#include "v_network.hh"

/** Initializes the Voronoi network object. The geometry is set up to match a
 * corresponding container class, and memory is allocated for the network.
 * \param[in] c a reference to a container or container_poly class. */
template<class c_class>
voronoi_network::voronoi_network(c_class &c,double net_tol_) :
        bx(c.bx), bxy(c.bxy), by(c.by), bxz(c.bxz), byz(c.byz), bz(c.bz),
        nx(c.nx), ny(c.ny), nz(c.nz), nxyz(nx*ny*nz),
        xsp(nx/bx), ysp(ny/by), zsp(nz/bz), net_tol(net_tol_) {
        int l;

        // Allocate memory for vertex structure
        pts=new double*[nxyz];
        idmem=new int*[nxyz];
        ptsc=new int[nxyz];
        ptsmem=new int[nxyz];
        for(l=0;l<nxyz;l++) {
                pts[l]=new double[4*init_network_vertex_memory];
                idmem[l]=new int[init_network_vertex_memory];
                ptsc[l]=0;ptsmem[l]=init_network_vertex_memory;
        }

        // Allocate memory for network edges and related statistics
        edc=0;edmem=init_network_vertex_memory*nxyz;
        ed=new int*[edmem];
        ne=new int*[edmem];
        pered=new unsigned int*[edmem];
        raded=new block*[edmem];
        nu=new int[edmem];
        nec=new int[edmem];
        numem=new int[edmem];

        // Allocate memory for back pointers
        reg=new int[edmem];
        regp=new int[edmem];

        // Allocate edge memory
        for(l=0;l<edmem;l++) {
                ed[l]=new int[2*init_network_edge_memory];
                ne[l]=ed[l]+init_network_edge_memory;
        }
        for(l=0;l<edmem;l++) raded[l]=new block[init_network_edge_memory];
        for(l=0;l<edmem;l++) pered[l]=new unsigned int[init_network_edge_memory];
        for(l=0;l<edmem;l++) {nu[l]=nec[l]=0;numem[l]=init_network_edge_memory;}

        // vertices
        vmap=new int[4*init_vertices];
        map_mem=init_vertices;
}

/** The voronoi_network destructor removes the dynamically allocated memory. */
voronoi_network::~voronoi_network() {
        int l;

        // Remove Voronoi mapping array
        delete [] vmap;

        // Remove individual edge arrays
        for(l=0;l<edmem;l++) delete [] pered[l];
        for(l=0;l<edmem;l++) delete [] raded[l];
        for(l=0;l<edmem;l++) delete [] ed[l];

        // Remove back pointers
        delete [] regp;delete [] reg;

        // Remove memory for edges and related statistics
        delete [] numem;delete [] nec;delete [] nu;
        delete [] raded;delete [] pered;
        delete [] ne;delete [] ed;

        // Remove vertex structure arrays
        for(l=0;l<nxyz;l++) {
                delete [] idmem[l];
                delete [] pts[l];
        }
        delete [] ptsmem;delete [] ptsc;
        delete [] idmem;delete [] pts;
}

/** Increase network memory for a particular region. */
void voronoi_network::add_network_memory(int l) {
        ptsmem[l]<<=1;

        // Check to see that an absolute maximum in memory allocation
        // has not been reached, to prevent runaway allocation
        if(ptsmem[l]>max_network_vertex_memory)
                voro_fatal_error("Container vertex maximum memory allocation exceeded",VOROPP_MEMORY_ERROR);

        // Allocate new arrays
        double *npts(new double[4*ptsmem[l]]);
        int *nidmem(new int[ptsmem[l]]);

        // Copy the contents of the old arrays into the new ones
        for(int i=0;i<4*ptsc[l];i++) npts[i]=pts[l][i];
        for(int i=0;i<ptsc[l];i++) nidmem[i]=idmem[l][i];

        // Delete old arrays and update pointers to the new ones
        delete [] pts[l];delete [] idmem[l];
        pts[l]=npts;idmem[l]=nidmem;
}

/** Increase edge network memory. */
void voronoi_network::add_edge_network_memory() {
        int i;
        edmem<<=1;

        // Allocate new arrays
        int **ned(new int*[edmem]);
        int **nne(new int*[edmem]);
        block **nraded(new block*[edmem]);
        unsigned int **npered(new unsigned int*[edmem]);
        int *nnu(new int[edmem]);
        int *nnec(new int[edmem]);
        int *nnumem(new int[edmem]);
        int *nreg(new int[edmem]);
        int *nregp(new int[edmem]);

        // Copy the contents of the old arrays into the new ones
        for(i=0;i<edc;i++) {
                ned[i]=ed[i];
                nne[i]=ne[i];
                nraded[i]=raded[i];
                npered[i]=pered[i];
                nnu[i]=nu[i];
                nnec[i]=nec[i];
                nnumem[i]=numem[i];
                nreg[i]=reg[i];
                nregp[i]=regp[i];
        }

        // Carry out new allocation
        while(i<edmem) {
                ned[i]=new int[2*init_network_edge_memory];
                nne[i]=ned[i]+init_network_edge_memory;
                nnu[i]=nnec[i]=0;nnumem[i]=init_network_edge_memory;
                nraded[i]=new block[init_network_edge_memory];
                npered[i++]=new unsigned int[init_network_edge_memory];
        }

        // Delete old arrays and update pointers to the new ones
        delete [] ed;ed=ned;
        delete [] ne;ne=nne;
        delete [] raded;raded=nraded;
        delete [] pered;pered=npered;
        delete [] nu;nu=nnu;
        delete [] nec;nec=nnec;
        delete [] numem;numem=nnumem;
        delete [] reg;reg=nreg;
        delete [] regp;regp=nregp;
}

/** Increase a particular vertex memory. */
void voronoi_network::add_particular_vertex_memory(int l) {
        numem[l]<<=1;

        // Check that the vertex allocation does not exceed a maximum safe
        // limit
        if(numem[l]>max_vertex_order)
                voro_fatal_error("Particular vertex maximum memory allocation exceeded",VOROPP_MEMORY_ERROR);

        // Allocate new arrays
        int *ned(new int[2*numem[l]]);
        int *nne(ned+numem[l]);
        block *nraded(new block[numem[l]]);
        unsigned int *npered(new unsigned int[numem[l]]);

        // Copy the contents of the old arrays into the new ones
        for(int i=0;i<nu[l];i++) {
                ned[i]=ed[l][i];
                nraded[i]=raded[l][i];
                npered[i]=pered[l][i];
        }
        for(int i=0;i<nec[l];i++) nne[i]=ne[l][i];

        // Delete old arrays and update pointers to the new ones
        delete [] ed[l];ed[l]=ned;ne[l]=nne;
        delete [] raded[l];raded[l]=nraded;
        delete [] pered[l];pered[l]=npered;
}


/** Increases the memory for the vertex mapping.
 * \param[in] pmem the amount of memory needed. */
void voronoi_network::add_mapping_memory(int pmem) {
        do {map_mem<<=1;} while(map_mem<pmem);
        delete [] vmap;
        vmap=new int[4*map_mem];
}

/** Clears the class of all vertices and edges. */
void voronoi_network::clear_network() {
        int l;
        edc=0;
        for(l=0;l<nxyz;l++) ptsc[l]=0;
        for(l=0;l<edmem;l++) nu[l]=0;
}

/** Outputs the network in a format that can be read by gnuplot.
 * \param[in] fp a file handle to write to. */
void voronoi_network::draw_network(FILE *fp) {
        int l,q,ai,aj,ak;
        double x,y,z,*ptsp;
        for(l=0;l<edc;l++) {
                ptsp=pts[reg[l]]+4*regp[l];
                x=*(ptsp++);y=*(ptsp++);z=*ptsp;
                for(q=0;q<nu[l];q++) {
                        unpack_periodicity(pered[l][q],ai,aj,ak);
                        if(ed[l][q]<l&&ai==0&&aj==0&&ak==0) continue;
                        ptsp=pts[reg[ed[l][q]]]+4*regp[ed[l][q]];
                        fprintf(fp,"%g %g %g\n%g %g %g\n\n\n",x,y,z,
                                *ptsp+bx*ai+bxy*aj+bxz*ak,
                                ptsp[1]+by*aj+byz*ak,ptsp[2]+bz*ak);
                }
        }
}

/** Prints out the network.
 * \param[in] fp a file handle to write to.
 * \param[in] reverse_remove a boolean value, setting whether or not to remove
 *                           reverse edges. */
void voronoi_network::print_network(FILE *fp,bool reverse_remove) {
        int ai,aj,ak,j,l,ll,q;
        double x,y,z,x2,y2,z2,*ptsp;

        // Print the vertex table
        fprintf(fp,"Vertex table:\n%d\n",edc);
        //os << edc << "\n";
        for(l=0;l<edc;l++) {
                ptsp=pts[reg[l]];j=4*regp[l];
                fprintf(fp,"%d %g %g %g %g",l,ptsp[j],ptsp[j+1],ptsp[j+2],ptsp[j+3]);
                for(ll=0;ll<nec[l];ll++) fprintf(fp," %d",ne[l][ll]);
                fputs("\n",fp);
        }

        // Print out the edge table, loop over vertices
        fputs("\nEdge table:\n",fp);
        for(l=0;l<edc;l++) {

                // Store the position of this vertex
                ptsp=pts[reg[l]];j=4*regp[l];
                x=ptsp[j];y=ptsp[j+1];z=ptsp[j+2];

                // Loop over edges of this vertex
                for(q=0;q<nu[l];q++) {

                        unpack_periodicity(pered[l][q],ai,aj,ak);

                        // If this option is enabled, then the code will not
                        // print edges from i to j for j<i.
                        if(reverse_remove) if(ed[l][q]<l&&ai==0&&aj==0&&ak==0) continue;

                        fprintf(fp,"%d -> %d",l,ed[l][q]);
                        raded[l][q].print(fp);

                        // Compute and print the length of the edge
                        ptsp=pts[reg[ed[l][q]]];j=4*regp[ed[l][q]];
                        x2=ptsp[j]+ai*bx+aj*bxy+ak*bxz-x;
                        y2=ptsp[j+1]+aj*by+ak*byz-y;
                        z2=ptsp[j+2]+ak*bz-z;
                        fprintf(fp," %d %d %d %g\n",ai,aj,ak,sqrt(x2*x2+y2*y2+z2*z2));
                }
        }
}

// Converts three periodic image displacements into a single unsigned integer.
// \param[in] i the periodic image in the x direction.
// \param[in] j the periodic image in the y direction.
// \param[in] k the periodic image in the z direction.
// \return The packed integer. */
inline unsigned int voronoi_network::pack_periodicity(int i,int j,int k) {
        unsigned int pa=((unsigned int) (127+i));
        pa<<=8;pa+=((unsigned int) (127+j));
        pa<<=8;pa+=((unsigned int) (127+k));
        return pa;
}

/** Unpacks an unsigned integer into three periodic image displacements.
 * \param[in] pa the packed integer.
// \param[out] i the periodic image in the x direction.
// \param[out] j the periodic image in the y direction.
// \param[out] k the periodic image in the z direction. */
inline void voronoi_network::unpack_periodicity(unsigned int pa,int &i,int &j,int &k) {
        i=((signed int) (pa>>16))-127;
        j=((signed int) ((pa>>8)&255))-127;
        k=((signed int) (pa&255))-127;
}

/** Adds a Voronoi cell to the network structure. The routine first checks all
 * of the Voronoi cell vertices and merges them with existing ones where
 * possible. Edges are then added to the structure.
 * \param[in] c a reference to a Voronoi cell.
 * \param[in] (x,y,z) the position of the Voronoi cell.
 * \param[in] idn the ID number of the particle associated with the cell. */
template<class v_cell>
void voronoi_network::add_to_network_internal(v_cell &c,int idn,double x,double y,double z,double rad,int *cmap) {
        int i,j,k,ijk,l,q,ai,aj,ak,*vmp(cmap);
        double gx,gy,vx,vy,vz,crad,*cp(c.pts);

        // Loop over the vertices of the Voronoi cell
        for(l=0;l<c.p;l++,vmp+=4) {

                // Compute the real position of this vertex, and evaluate its
                // position along the non-rectangular axes
                vx=x+cp[4*l]*0.5;vy=y+cp[4*l+1]*0.5;vz=z+cp[4*l+2]*0.5;
                gx=vx-vy*(bxy/by)+vz*(bxy*byz-by*bxz)/(by*bz);
                gy=vy-vz*(byz/bz);

                // Compute the adjusted radius, which will be needed either way
                crad=0.5*sqrt(cp[4*l]*cp[4*l]+cp[4*l+1]*cp[4*l+1]+cp[4*l+2]*cp[4*l+2])-rad;

                // Check to see if a vertex very close to this one already
                // exists in the network
                if(search_previous(gx,gy,vx,vy,vz,ijk,q,vmp[1],vmp[2],vmp[3])) {

                        // If it does, then just map the Voronoi cell
                        // vertex to it
                        *vmp=idmem[ijk][q];

                        // Store this radius if it smaller than the current
                        // value
                        if(pts[ijk][4*q+3]>crad) pts[ijk][4*q+3]=crad;
                } else {
                        k=step_int(vz*zsp);if(k<0||k>=nz) {ak=step_div(k,nz);vx-=bxz*ak;vy-=byz*ak;vz-=bz*ak;k-=ak*nz;} else ak=0;
                        j=step_int(gy*ysp);if(j<0||j>=ny) {aj=step_div(j,ny);vx-=bxy*aj;vy-=by*aj;j-=aj*ny;} else aj=0;
                        i=step_int(gx*xsp);if(i<0||i>=nx) {ai=step_div(i,nx);vx-=bx*ai;i-=ai*nx;} else ai=0;

                        vmp[1]=ai;vmp[2]=aj;vmp[3]=ak;
                        ijk=i+nx*(j+ny*k);

                        if(edc==edmem) add_edge_network_memory();
                        if(ptsc[ijk]==ptsmem[ijk]) add_network_memory(ijk);

                        reg[edc]=ijk;regp[edc]=ptsc[ijk];
                        pts[ijk][4*ptsc[ijk]]=vx;
                        pts[ijk][4*ptsc[ijk]+1]=vy;
                        pts[ijk][4*ptsc[ijk]+2]=vz;
                        pts[ijk][4*ptsc[ijk]+3]=crad;
                        idmem[ijk][ptsc[ijk]++]=edc;
                        *vmp=edc++;
                }

                // Add the neighbor information to this vertex
                add_neighbor(*vmp,idn);
        }

        add_edges_to_network(c,x,y,z,rad,cmap);
}

/** Adds a neighboring particle ID to a vertex in the Voronoi network, first
 * checking that the ID is not already recorded.
 * \param[in] k the Voronoi vertex.
 * \param[in] idn the particle ID number. */
inline void voronoi_network::add_neighbor(int k,int idn) {
        for(int i=0;i<nec[k];i++) if(ne[k][i]==idn) return;
        if(nec[k]==numem[k]) add_particular_vertex_memory(k);
        ne[k][nec[k]++]=idn;
}

/** Adds edges to the network structure, after the vertices have been
 * considered. This routine assumes that the cmap array has a mapping between
 * Voronoi cell vertices and Voronoi network vertices. */
template<class v_cell>
void voronoi_network::add_edges_to_network(v_cell &c,double x,double y,double z,double rad,int *cmap) {
        int i,j,ai,aj,ak,bi,bj,bk,k,l,q,*vmp;unsigned int cper;
        double vx,vy,vz,wx,wy,wz,dx,dy,dz,dis;double *pp;
        for(l=0;l<c.p;l++) {
                vmp=cmap+4*l;k=*(vmp++);ai=*(vmp++);aj=*(vmp++);ak=*vmp;
                pp=pts[reg[k]]+4*regp[k];
                vx=pp[0]+ai*bx+aj*bxy+ak*bxz;
                vy=pp[1]+aj*by+ak*byz;
                vz=pp[2]+ak*bz;
                for(q=0;q<c.nu[l];q++) {
                        i=c.ed[l][q];
                        vmp=cmap+4*i;
                        j=*(vmp++);bi=*(vmp++);bj=*(vmp++);bk=*vmp;

                        // Skip if this is a self-connecting edge
                        if(j==k&&bi==ai&&bj==aj&&bk==ak) continue;
                        cper=pack_periodicity(bi-ai,bj-aj,bk-ak);
                        pp=pts[reg[j]]+(4*regp[j]);
                        wx=pp[0]+bi*bx+bj*bxy+bk*bxz;
                        wy=pp[1]+bj*by+bk*byz;
                        wz=pp[2]+bk*bz;
                        dx=wx-vx;dy=wy-vy;dz=wz-vz;
                        dis=(x-vx)*dx+(y-vy)*dy+(z-vz)*dz;
                        dis/=dx*dx+dy*dy+dz*dz;
                        if(dis<0) dis=0;if(dis>1) dis=1;
                        wx=vx-x+dis*dx;wy=vy-y+dis*dy;wz=vz-z+dis*dz;
                        int nat=not_already_there(k,j,cper);
                        if(nat==nu[k]) {
                                if(nu[k]==numem[k]) add_particular_vertex_memory(k);
                                ed[k][nu[k]]=j;
                                raded[k][nu[k]].first(sqrt(wx*wx+wy*wy+wz*wz)-rad,dis);
                                pered[k][nu[k]++]=cper;
                        } else {
                                raded[k][nat].add(sqrt(wx*wx+wy*wy+wz*wz)-rad,dis);
                        }
                }
        }
}

template<class v_cell>
void voronoi_network::add_to_network_rectangular_internal(v_cell &c,int idn,double x,double y,double z,double rad,int *cmap) {
        int i,j,k,ijk,l,q,ai,aj,ak,*vmp(cmap);
        double vx,vy,vz,crad,*cp(c.pts);

        for(l=0;l<c.p;l++,vmp+=4) {
                vx=x+cp[4*l]*0.5;vy=y+cp[4*l+1]*0.5;vz=z+cp[4*l+2]*0.5;
                crad=0.5*sqrt(cp[4*l]*cp[4*l]+cp[4*l+1]*cp[4*l+1]+cp[4*l+2]*cp[4*l+2])-rad;
                if(safe_search_previous_rect(vx,vy,vz,ijk,q,vmp[1],vmp[2],vmp[3])) {
                        *vmp=idmem[ijk][q];

                        // Store this radius if it smaller than the current
                        // value
                        if(pts[ijk][4*q+3]>crad) pts[ijk][4*q+3]=crad;
                } else {
                        k=step_int(vz*zsp);
                        if(k<0||k>=nz) {
                                ak=step_div(k,nz);
                                vz-=ak*bz;vy-=ak*byz;vx-=ak*bxz;k-=ak*nz;
                        } else ak=0;
                        j=step_int(vy*ysp);
                        if(j<0||j>=ny) {
                                aj=step_div(j,ny);
                                vy-=aj*by;vx-=aj*bxy;j-=aj*ny;
                        } else aj=0;
                        i=step_int(vx*xsp);
                        if(i<0||i>=nx) {
                                ai=step_div(i,nx);
                                vx-=ai*bx;i-=ai*nx;
                        } else ai=0;
                        vmp[1]=ai;
                        vmp[2]=aj;
                        vmp[3]=ak;
                        ijk=i+nx*(j+ny*k);
                        if(edc==edmem) add_edge_network_memory();
                        if(ptsc[ijk]==ptsmem[ijk]) add_network_memory(ijk);
                        reg[edc]=ijk;regp[edc]=ptsc[ijk];
                        pts[ijk][4*ptsc[ijk]]=vx;
                        pts[ijk][4*ptsc[ijk]+1]=vy;
                        pts[ijk][4*ptsc[ijk]+2]=vz;
                        pts[ijk][4*ptsc[ijk]+3]=crad;
                        idmem[ijk][ptsc[ijk]++]=edc;
                        *vmp=edc++;
                }

                add_neighbor(*vmp,idn);
        }

        add_edges_to_network(c,x,y,z,rad,cmap);
}

int voronoi_network::not_already_there(int k,int j,unsigned int cper) {
        for(int i=0;i<nu[k];i++) if(ed[k][i]==j&&pered[k][i]==cper) return i;
        return nu[k];
}

bool voronoi_network::search_previous(double gx,double gy,double x,double y,double z,int &ijk,int &q,int &pi,int &pj,int &pk) {
        int ai=step_int((gx-net_tol)*xsp),bi=step_int((gx+net_tol)*xsp);
        int aj=step_int((gy-net_tol)*ysp),bj=step_int((gy+net_tol)*ysp);
        int ak=step_int((z-net_tol)*zsp),bk=step_int((z+net_tol)*zsp);
        int i,j,k,mi,mj,mk;
        double px,py,pz,px2,py2,px3,*pp;

        for(k=ak;k<=bk;k++) {
                pk=step_div(k,nz);px=pk*bxz;py=pk*byz;pz=pk*bz;mk=k-nz*pk;
                for(j=aj;j<=bj;j++) {
                        pj=step_div(j,ny);px2=px+pj*bxy;py2=py+pj*by;mj=j-ny*pj;
                        for(i=ai;i<=bi;i++) {
                                pi=step_div(i,nx);px3=px2+pi*bx;mi=i-nx*pi;
                                ijk=mi+nx*(mj+ny*mk);
                                pp=pts[ijk];
                                for(q=0;q<ptsc[ijk];q++,pp+=4) if(fabs(*pp+px3-x)<net_tol&&fabs(pp[1]+py2-y)<net_tol&&fabs(pp[2]+pz-z)<net_tol) return true;
                        }
                }
        }
        return false;
}

bool voronoi_network::safe_search_previous_rect(double x,double y,double z,int &ijk,int &q,int &ci,int &cj,int &ck) {
        const double tol(0.5*net_tol);
        if(search_previous_rect(x+tol,y+tol,z+tol,ijk,q,ci,cj,ck)) return true;
        if(search_previous_rect(x-tol,y+tol,z+tol,ijk,q,ci,cj,ck)) return true;
        if(search_previous_rect(x+tol,y-tol,z+tol,ijk,q,ci,cj,ck)) return true;
        if(search_previous_rect(x-tol,y-tol,z+tol,ijk,q,ci,cj,ck)) return true;
        if(search_previous_rect(x+tol,y+tol,z-tol,ijk,q,ci,cj,ck)) return true;
        if(search_previous_rect(x-tol,y+tol,z-tol,ijk,q,ci,cj,ck)) return true;
        if(search_previous_rect(x+tol,y-tol,z-tol,ijk,q,ci,cj,ck)) return true;
        return search_previous_rect(x-tol,y-tol,z-tol,ijk,q,ci,cj,ck);
}

bool voronoi_network::search_previous_rect(double x,double y,double z,int &ijk,int &q,int &ci,int &cj,int &ck) {
        int k=step_int(z*zsp);
        if(k<0||k>=nz) {
                ck=step_div(k,nz);
                z-=ck*bz;y-=ck*byz;x-=ck*bxz;k-=ck*nz;
        } else ck=0;

        int j=step_int(y*ysp);
        if(j<0||j>=ny) {
                cj=step_div(j,ny);
                y-=cj*by;x-=cj*bxy;j-=cj*ny;
        } else cj=0;

        ijk=step_int(x*xsp);
        if(ijk<0||ijk>=nx) {
                ci=step_div(ijk,nx);
                x-=ci*bx;ijk-=ci*nx;
        } else ci=0;

        ijk+=nx*(j+ny*k);double *pp(pts[ijk]);
        for(q=0;q<ptsc[ijk];q++,pp+=4)
                if(fabs(*pp-x)<net_tol&&fabs(pp[1]-y)<net_tol&&fabs(pp[2]-z)<net_tol) return true;
        return false;
}

/** Custom int function, that gives consistent stepping for negative numbers.
 * With normal int, we have (-1.5,-0.5,0.5,1.5) -> (-1,0,0,1).
 * With this routine, we have (-1.5,-0.5,0.5,1.5) -> (-2,-1,0,1). */
inline int voronoi_network::step_int(double a) {
        return a<0?int(a)-1:int(a);
}

/** Custom integer division function, that gives consistent stepping for
 * negative numbers. */
inline int voronoi_network::step_div(int a,int b) {
        return a>=0?a/b:-1+(a+1)/b;
}

// Explicit instantiation
template voronoi_network::voronoi_network(container_periodic&, double);
template voronoi_network::voronoi_network(container_periodic_poly&, double);
template void voronoi_network::add_to_network<voronoicell>(voronoicell&, int, double, double, double, double);
template void voronoi_network::add_to_network<voronoicell_neighbor>(voronoicell_neighbor&, int, double, double, double, double);
template void voronoi_network::add_to_network_rectangular<voronoicell>(voronoicell&, int, double, double, double, double);
template void voronoi_network::add_to_network_rectangular<voronoicell_neighbor>(voronoicell_neighbor&, int, double, double, double, double);