Jiaming Guo / flow3

  #ifndef FLOW3_H
  #define FLOW3_H
  
  #include <algorithm>
  
  //STIM include
  #include <stim/parser/arguments.h>
  #include <stim/visualization/gl_network.h>
  #include <stim/visualization/colormap.h>
  #include <stim/math/matrix.h>
  #include <stim/visualization/gl_aaboundingbox.h>
  #include <stim/ui/progressbar.h>
  #include <stim/grids/image_stack.h>
  
  #ifdef __CUDACC__
  #include <cublas_v2.h>
  #include <stim/cuda/cudatools/error.h>
  #endif
  
  namespace stim {
  	template <typename A, typename B, typename C>
  	struct triple {
  		A first;
  		B second;
  		C third;
  	};
  
  	template <typename T>
  	struct bridge {
  		std::vector<unsigned> v;				// vertices' indices
  		std::vector<typename stim::vec3<T> > V;	// vertices' coordinates
  		T l;		// length

  		T r;		// radius

  		T deltaP;	// pressure drop
  		T Q;		// volume flow rate
  	};
  
  	template <typename T>
  	struct sphere {
  		stim::vec3<T> c;		// center of sphere

  		T r;					// radius

  	};
  
  	template <typename T>

  	struct cone {				// radius changes gradually

  		stim::vec3<T> c1;		// center of geometry start hat
  		stim::vec3<T> c2;		// center of geometry end hat

  		T r1;					// radius at start hat
  		T r2;					// radius at end hat

  	};
  
  	template <typename T>
  	struct cuboid {
  		stim::vec3<T> c;
  		T l;					// length
  		T w;					// width
  		T h;					// height
  	};
  

  	template <typename T>
  	struct circuit {
  		std::vector<typename std::pair<unsigned, unsigned> > v;		// end vertex index
  		std::vector<T> r;											// branch resistence
  	};
  

  	/// indicator function
  #ifdef __CUDACC__
  	// for sphere
  	template <typename T>

  	__global__ void inside_sphere(const stim::sphere<T> *V, size_t num, T Z,size_t *R, T *S, unsigned char *ptr, int x, int y, int z, T std = 1.0f) {

  
  		unsigned ix = blockDim.x * blockIdx.x + threadIdx.x;
  		unsigned iy = blockDim.y * blockIdx.y + threadIdx.y;
  
  		if (ix >= R[1] || iy >= R[2]) return;		// avoid seg-fault
  
  		// find world_pixel coordinates
  		stim::vec3<T> world_pixel;
  		world_pixel[0] = (T)ix * S[1] - x;			// translate origin to center of the network
  		world_pixel[1] = (T)iy * S[2] - y;

  		world_pixel[2] = ((T)z - Z / 2.0f) * S[3];	// ???center of box minus half width

  
  		float distance = FLT_MAX;
  		float tmp_distance;
  		unsigned idx;
  
  		for (unsigned i = 0; i < num; i++) {
  			tmp_distance = (V[i].c - world_pixel).len();
  			if (tmp_distance <= distance) {
  				distance = tmp_distance;
  				idx = i;
  			}
  		}
  		if (distance <= V[idx].r)
  			ptr[(R[2] - 1 - iy) * R[0] * R[1] + ix * R[0]] = 255;

  		else {
  			T g = gaussianFunction(std::pow(distance - V[idx].r, 2), std);
  			if(g > 0.1f)
  				ptr[(R[2] - 1 - iy) * R[0] * R[1] + ix * R[0]] = 255;
  		}

  	}
  
  	// for cone
  	template <typename T>

  	__global__ void inside_cone(const stim::cone<T> *E, size_t num, T Z, size_t *R, T *S, unsigned char *ptr, int x, int y, int z, T std = 1.0f) {

  
  		unsigned ix = blockDim.x * blockIdx.x + threadIdx.x;
  		unsigned iy = blockDim.y * blockIdx.y + threadIdx.y;
  
  		if (ix >= R[1] || iy >= R[2]) return;			// avoid segfault
  
  		stim::vec3<T> world_pixel;
  		world_pixel[0] = (T)ix * S[1] - x;
  		world_pixel[1] = (T)iy * S[2] - y;

  		world_pixel[2] = ((T)z - Z / 2.0f) * S[3];

  
  		float distance = FLT_MAX;
  		float tmp_distance;

  		float rr;										// radius at the surface where projection meets

  
  		for (unsigned i = 0; i < num; i++) {			// find the nearest cylinder
  			tmp_distance = ((world_pixel - E[i].c1).cross(world_pixel - E[i].c2)).len() / (E[i].c2 - E[i].c1).len();
  			if (tmp_distance <= distance) {
  				// we only focus on point to line segment
  				// check to see whether projection is lying outside the line segment
  				float a = (world_pixel - E[i].c1).dot((E[i].c2 - E[i].c1).norm());
  				float b = (world_pixel - E[i].c2).dot((E[i].c1 - E[i].c2).norm());
  				float length = (E[i].c1 - E[i].c2).len();
  				if (a <= length && b <= length) {		// projection lying inside the line segment
  					distance = tmp_distance;
  					rr = E[i].r1 + (E[i].r2 - E[i].r1) * a / (length);		// linear change
  				}
  			}
  		}
  		if (distance <= rr)
  			ptr[(R[2] - 1 - iy) * R[0] * R[1] + ix * R[0]] = 255;

  		else {
  			T g = gaussianFunction(std::pow(distance - rr, 2), std);
  			if (g > 0.1f)
  				ptr[(R[2] - 1 - iy) * R[0] * R[1] + ix * R[0]] = 255;
  		}

  	}
  
  	// for source bus
  	template <typename T>

  	__global__ void inside_cuboid(const stim::cuboid<T> *B, size_t num, T Z, size_t *R, T *S, unsigned char *ptr, int x, int y, int z) {

  
  		unsigned ix = blockDim.x * blockIdx.x + threadIdx.x;
  		unsigned iy = blockDim.y * blockIdx.y + threadIdx.y;
  
  		if (ix >= R[1] || iy >= R[2]) return;			// avoid segfault
  
  		stim::vec3<T> world_pixel;
  		world_pixel[0] = (T)ix * S[1] - x;
  		world_pixel[1] = (T)iy * S[2] - y;

  		world_pixel[2] = ((T)z - Z / 2.0f) * S[3];

  
  		for (unsigned i = 0; i < num; i++) {
  			bool left_outside = false;					// flag indicates point is outside the left bound
  			bool right_outside = false;
  
  			stim::vec3<T> tmp = B[i].c;
  			stim::vec3<T> L = stim::vec3<T>(tmp[0] - B[i].l / 2.0f, tmp[1] - B[i].h / 2.0f, tmp[2] - B[i].w / 2.0f);
  			stim::vec3<T> U = stim::vec3<T>(tmp[0] + B[i].l / 2.0f, tmp[1] + B[i].h / 2.0f, tmp[2] + B[i].w / 2.0f);
  
  			for (unsigned d = 0; d < 3; d++) {
  				if (world_pixel[d] < L[d])				// if the point is less than the minimum bound
  					left_outside = true;
  				if (world_pixel[d] > U[d])				// if the point is greater than the maximum bound
  					right_outside = true;
  			}
  			if (!left_outside && !right_outside)
  				ptr[(R[2] - 1 - iy) * R[0] * R[1] + ix * R[0]] = 255;
  		}
  	}
  #endif
  
  	template <typename T>

  	class flow : public stim::gl_network<T> {

  
  	private:

  		unsigned num_edge;
  		unsigned num_vertex;

  		GLuint dlist;					// display list for inlets/outlets connections
  
  		enum direction { UP, LEFT, DOWN, RIGHT };

  
  		// calculate the cofactor of elemen[row][col]
  		void get_minor(T** src, T** dest, int row, int col, int order) {
  
  			// index of element to be copied
  			int rowCount = 0;
  			int colCount = 0;
  
  			for (int i = 0; i < order; i++) {
  				if (i != row) {
  					colCount = 0;
  					for (int j = 0; j < order; j++) {
  						// when j is not the element
  						if (j != col) {
  							dest[rowCount][colCount] = src[i][j];
  							colCount++;
  						}
  					}
  					rowCount++;
  				}
  			}
  		}
  
  		// calculate the det()
  		T determinant(T** mat, int order) {
  
  			// degenate case when n = 1
  			if (order == 1)
  				return mat[0][0];
  
  			T det = 0.0;		// determinant value
  
  								// allocate the cofactor matrix
  			T** minor = (T**)malloc((order - 1) * sizeof(T*));
  			for (int i = 0; i < order - 1; i++)
  				minor[i] = (T*)malloc((order - 1) * sizeof(T));
  
  
  			for (int i = 0; i < order; i++) {
  
  				// get minor of element(0, i)
  				get_minor(mat, minor, 0, i, order);
  
  				// recursion
  				det += (i % 2 == 1 ? -1.0 : 1.0) * mat[0][i] * determinant(minor, order - 1);
  			}
  
  			// release memory
  			for (int i = 0; i < order - 1; i++)
  				free(minor[i]);
  			free(minor);
  
  			return det;
  		}
  
  	protected:
  
  		using stim::network<T>::E;
  		using stim::network<T>::V;
  		using stim::network<T>::get_start_vertex;
  		using stim::network<T>::get_end_vertex;
  		using stim::network<T>::get_r;
  		using stim::network<T>::get_average_r;
  		using stim::network<T>::get_l;

  		T** C;																	// Conductance
  		std::vector<typename stim::triple<unsigned, unsigned, float> > Q;		// volume flow rate
  		std::vector<T> QQ;														// Q' vector
  		std::vector<T> pressure;												// final pressure

  		std::vector<typename std::vector<typename stim::vec3<T> > > in_backup;	// inlet connection back up
  		std::vector<typename std::vector<typename stim::vec3<T> > > out_backup;

  		std::string units;														// length units

  
  	public:
  

  		bool set = false;														// flag indicates the pressure has been set

  		std::vector<T> P;														// initial pressure

  		std::vector<T> v;														// average velocity along each edge

  		std::vector<typename stim::vec3<T> > main_feeder;						// inlet/outlet main feeder
  		std::vector<unsigned> pendant_vertex;
  		std::vector<typename stim::triple<unsigned, unsigned, T> > input;		// first one store which vertex, second one stores which edge, third one stores in/out volume flow rate of that vertex
  		std::vector<typename stim::triple<unsigned, unsigned, T> > output;
  		std::vector<typename stim::bridge<T> > inlet;							// input bridge
  		std::vector<typename stim::bridge<T> > outlet;							// output bridge
  		std::vector<typename stim::sphere<T> > A;			// sphere model for making image stack
  		std::vector<typename stim::cone<T> > B;				// cone(cylinder) model for making image stack
  		std::vector<typename stim::cuboid<T> > CU;			// cuboid model for making image stack
  		stim::gl_aaboundingbox<T> bb;						// bounding box

  		std::vector<bool> inlet_feasibility;				// list of flags indicate whether one inlet connection is feasible
  		std::vector<bool> outlet_feasibility;
  		std::vector<typename std::pair<stim::vec3<T>, stim::vec3<T> > > inbb;	// inlet connection bounding box
  		std::vector<typename std::pair<stim::vec3<T>, stim::vec3<T> > > outbb;	// outlet connection bounding box

  		T Ps;												// source and end pressure
  		T Pe;

  
  		flow() {}				// default constructor
  		~flow() {
  			for (unsigned i = 0; i < num_vertex; i++)
  				delete[] C[i];
  			delete[] C;

  		}		// default destructor

  
  		void init(unsigned n_e, unsigned n_v) {
  
  			num_edge = n_e;
  			num_vertex = n_v;
  
  			C = new T*[n_v]();
  			for (unsigned i = 0; i < n_v; i++) {
  				C[i] = new T[n_v]();
  			}
  
  			QQ.resize(n_v);
  			P.resize(n_v);
  			pressure.resize(n_v);
  
  			Q.resize(n_e);
  			v.resize(n_e);
  		}
  
  		void clear() {
  
  			for (unsigned i = 0; i < num_vertex; i++) {
  				QQ[i] = 0;
  				pressure[i] = 0;
  				for (unsigned j = 0; j < num_vertex; j++) {
  					C[i][j] = 0;
  				}
  			}
  			main_feeder.clear();
  			input.clear();
  			output.clear();
  			inlet.clear();
  			outlet.clear();

  
  			if (glIsList(dlist)) {
  				glDeleteLists(dlist, 1);					// delete display list for modify
  				glDeleteLists(dlist + 1, 1);
  			}

  		}
  

  		void set_units(std::string u) {
  			units = u;
  		}
  

  		// copy radius from cylinder to flow
  		void set_radius(unsigned i, T radius) {

  			for (unsigned j = 0; j < num_edge; j++) {
  				if (E[j].v[0] == i)
  					E[j].cylinder<T>::set_r(0, radius);
  				else if (E[j].v[1] == i)
  					E[j].cylinder<T>::set_r(E[j].size() - 1, radius);
  			}
  		}
  

  		// get the radius of vertex i

  		T get_radius(unsigned i) {

  			unsigned tmp_e;				// edge index
  			unsigned tmp_v;				// vertex index in that edge
  			for (unsigned j = 0; j < num_edge; j++) {
  				if (E[j].v[0] == i) {
  					tmp_e = j;
  					tmp_v = 0;
  				}
  				else if (E[j].v[1] == i) {
  					tmp_e = j;

  					tmp_v = (unsigned)(E[j].size() - 1);

  				}
  			}
  
  			return E[tmp_e].r(tmp_v);
  		}
  

  		// get the radius of point j on edge i

  		T get_radius(unsigned i, unsigned j) {
  			return E[i].r(j);
  		}
  

  		// back up vertices
  		std::vector<stim::vec3<T> > back_vertex() {
  			std::vector<stim::vec3<T> > result;
  
  			for (unsigned i = 0; i < num_vertex; i++)
  				result.push_back(stim::vec3<T>(V[i][0], V[i][1], V[i][2]));
  
  			return result;
  		}
  
  		// get pendant vertices

  		std::vector<unsigned> get_pendant_vertex() {
  			std::vector<unsigned> result;
  			int count = 0;
  
  			for (unsigned i = 0; i < V.size(); i++) {			// for every vertex
  				for (unsigned j = 0; j < E.size(); j++) {		// for every edge
  					if (i == E[j].v[0] || i == E[j].v[1])		// check whether current vertex terminates one edge
  						count++;
  				}
  				if (count == 1) 								// is pendant vertex
  					result.push_back(i);
  				count = 0;										// reset count
  			}
  
  			return result;
  		}
  

  		// get the velocity of pendant vertex i
  		T get_velocity(unsigned i) {

  			unsigned tmp_e;				// edge index
  			for (unsigned j = 0; j < num_edge; j++) {
  				if (E[j].v[0] == i) {
  					tmp_e = j;
  					break;
  				}
  				else if (E[j].v[1] == i) {
  					tmp_e = j;
  					break;
  				}
  			}
  
  			return v[tmp_e];
  		}
  
  		// set pressure at specifi vertex
  		void set_pressure(unsigned i, T value) {
  			P[i] = value;
  		}
  

  		// extrct the largest connected component
  		void extract_lcc() {
  			
  
  		}
  

  		// solve the linear system to get stable flow state
  		void solve_flow(T viscosity) {
  
  			// clear up last time simulation
  			clear();
  
  			// get the pendant vertex indices

  			pendant_vertex = get_pendant_vertex();

  
  			// get bounding box
  			bb = (*this).boundingbox();
  
  			// set the conductance matrix of flow object
  			unsigned start_vertex = 0;
  			unsigned end_vertex = 0;
  			for (unsigned i = 0; i < num_edge; i++) {

  				start_vertex = (unsigned)get_start_vertex(i);		// get the start vertex index of current edge
  				end_vertex = (unsigned)get_end_vertex(i);			// get the end vertex index of current edge

  				C[start_vertex][end_vertex] = -((T)stim::PI * std::pow(get_average_r(i), 4)) / (8 * u * get_l(i));

  
  				C[end_vertex][start_vertex] = C[start_vertex][end_vertex];
  			}
  			// set the diagonal to the negative sum of row element
  			float sum = 0.0;
  			for (unsigned i = 0; i < num_vertex; i++) {
  				for (unsigned j = 0; j < num_vertex; j++) {
  					sum += C[i][j];
  				}
  				C[i][i] = -sum;
  				sum = 0.0;
  			}
  
  			// get the Q' vector QQ
  			// matrix manipulation to zero out the conductance matrix as defined by the boundary values that were enterd
  			for (unsigned i = 0; i < num_vertex; i++) {
  				if (P[i] != 0) {			// for every dangle vertex
  					for (unsigned j = 0; j < num_vertex; j++) {
  						if (j == i) {
  							QQ[i] = C[i][i] * P[i];
  						}
  						else {
  							C[i][j] = 0;
  							QQ[j] = QQ[j] - C[j][i] * P[i];
  							C[j][i] = 0;
  						}
  					}
  				}
  			}
  
  			// get the inverse of conductance matrix
  			stim::matrix<float> _C(num_vertex, num_vertex);
  			inversion(C, num_vertex, _C.data());

  			// get the pressure in the network
  			for (unsigned i = 0; i < num_vertex; i++) {
  				for (unsigned j = 0; j < num_vertex; j++) {
  					pressure[i] += _C(i, j) * QQ[j];
  				}
  			}
  
  			// get the flow state from known pressure

  			T start_pressure = 0.0;
  			T end_pressure = 0.0;
  			T deltaP = 0.0;

  			for (unsigned i = 0; i < num_edge; i++) {

  				start_vertex = (unsigned)get_start_vertex(i);
  				end_vertex = (unsigned)get_end_vertex(i);

  				start_pressure = pressure[start_vertex];		// get the start vertex pressure of current edge
  				end_pressure = pressure[end_vertex];			// get the end vertex pressure of current edge
  				deltaP = start_pressure - end_pressure;				// deltaP = Pa - Pb
  
  				Q[i].first = start_vertex;
  				Q[i].second = end_vertex;

  				Q[i].third = ((T)stim::PI * std::pow(get_average_r(i), 4) * deltaP) / (8 * u * get_l(i));
  				v[i] = Q[i].third / ((T)stim::PI * std::pow(get_average_r(i), 2));

  			}
  		}
  
  		// get the brewer color map based on velocity
  		void get_color_map(T& max_v, T& min_v, std::vector<unsigned char>& color, std::vector<unsigned> pendant_vertex) {

  			size_t num_edge = Q.size();
  			size_t num_vertex = QQ.size();

  
  			// find the absolute maximum velocity and minimum velocity
  			std::vector<float> abs_V(num_edge);
  			for (unsigned i = 0; i < num_edge; i++) {
  				abs_V[i] = std::fabsf(v[i]);
  			}
  
  			max_v = *std::max_element(abs_V.begin(), abs_V.end());
  			min_v = *std::min_element(abs_V.begin(), abs_V.end());

  			// get the color map based on velocity range along the network
  			color.clear();
  			if (pendant_vertex.size() == 2 && num_edge - num_vertex + 1 <= 0) 		// only one inlet and one outlet

  				color.resize(num_edge * 3, (unsigned char)128);

  			else {
  				color.resize(num_edge * 3);
  				stim::cpu2cpu<float>(&abs_V[0], &color[0], num_edge, min_v, max_v, stim::cmBrewer);
  			}
  		}
  
  		// print flow
  		void print_flow() {
  
  			// show the pressure information in console box

  			std::cout << "PRESSURE(g/" << units << "/s^2):" << std::endl;

  			for (unsigned i = 0; i < num_vertex; i++) {
  				std::cout << "[" << i << "] " << pressure[i] << std::endl;
  			}
  			// show the flow rate information in console box

  			std::cout << "VOLUME FLOW RATE(" << units << "^3/s):" << std::endl;

  			for (unsigned i = 0; i < num_edge; i++) {
  				std::cout << "(" << Q[i].first << "," << Q[i].second << ")" << Q[i].third << std::endl;
  			}
  		}
  
  		/// helper function
  		// find hilbert curve order
  		// @param: current direct length between two vertices
  		// @param: desire length
  		void find_hilbert_order(T l, T d, int &order) {

  			bool flag = false;
  			int o = 1;
  			T tmp;					// temp of length
  			while (!flag) {
  				// convert from cartesian length to hilbert length
  				// l -> l * (4 ^ order - 1)/(2 ^ order - 1)

  				tmp = (T)(l * (std::pow(4, o) - 1) / (std::pow(2, o) - 1));

  				if (tmp >= d)
  					flag = true;
  				else
  					o++;
  			}
  			order = o;
  		}
  

  		// move hilbert curves

  		void move(unsigned i, T *c, direction dir, T dl, int feeder, bool invert) {

  			int cof = (invert) ? -1 : 1;
  
  			switch (dir) {
  			case UP:
  				c[1] += dl;
  				break;
  			case LEFT:
  				c[0] -= cof * dl;
  				break;
  			case DOWN:
  				c[1] -= dl;
  				break;
  			case RIGHT:
  				c[0] += cof * dl;
  				break;
  			}
  
  			stim::vec3<T> tmp;
  			for (unsigned i = 0; i < 3; i++)
  				tmp[i] = c[i];

  			if (feeder == 1)					// inlet main feeder
  				inlet[i].V.push_back(tmp);
  			else if (feeder == 0)				// outlet main feeder
  				outlet[i].V.push_back(tmp);
  		}
  

  		// form hilbert curves

  		void hilbert_curve(unsigned i, T *c, int order, T dl, int feeder, bool invert, direction dir = DOWN) {

  
  			if (order == 1) {

  				switch (dir) {
  				case UP:
  					move(i, c, DOWN, dl, feeder, invert);
  					move(i, c, RIGHT, dl, feeder, invert);
  					move(i, c, UP, dl, feeder, invert);
  					break;
  				case LEFT:
  					move(i, c, RIGHT, dl, feeder, invert);
  					move(i, c, DOWN, dl, feeder, invert);
  					move(i, c, LEFT, dl, feeder, invert);
  					break;
  				case DOWN:
  					move(i, c, UP, dl, feeder, invert);
  					move(i, c, LEFT, dl, feeder, invert);
  					move(i, c, DOWN, dl, feeder, invert);
  					break;
  				case RIGHT:
  					move(i, c, LEFT, dl, feeder, invert);
  					move(i, c, UP, dl, feeder, invert);
  					move(i, c, RIGHT, dl, feeder, invert);
  					break;
  				}

  			}
  			else if (order > 1) {
  				switch (dir) {
  				case UP:
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, LEFT);
  					move(i, c, DOWN, dl, feeder, invert);
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, UP);
  					move(i, c, RIGHT, dl, feeder, invert);
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, UP);
  					move(i, c, UP, dl, feeder, invert);
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, RIGHT);
  					break;
  				case LEFT:
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, UP);
  					move(i, c, RIGHT, dl, feeder, invert);
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, LEFT);
  					move(i, c, DOWN, dl, feeder, invert);
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, LEFT);
  					move(i, c, LEFT, dl, feeder, invert);
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, DOWN);
  					break;
  				case DOWN:
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, RIGHT);
  					move(i, c, UP, dl, feeder, invert);
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, DOWN);
  					move(i, c, LEFT, dl, feeder, invert);
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, DOWN);
  					move(i, c, DOWN, dl, feeder, invert);
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, LEFT);
  					break;
  				case RIGHT:
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, DOWN);
  					move(i, c, LEFT, dl, feeder, invert);
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, RIGHT);
  					move(i, c, UP, dl, feeder, invert);
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, RIGHT);
  					move(i, c, RIGHT, dl, feeder, invert);
  					hilbert_curve(i, c, order - 1, dl, feeder, invert, UP);
  					break;
  				}
  			}
  		}
  

  		/// render function
  		// find two envelope caps for two spheres
  		// @param cp1, cp2: list of points on the cap
  		// @param center1, center2: center point of cap

  		// @param r1, r2: radius of cap

  		void find_envelope(std::vector<typename stim::vec3<float> > &cp1, std::vector<typename stim::vec3<float> > &cp2, stim::vec3<float> center1, stim::vec3<float> center2, float r1, float r2, GLint subdivision) {
  
  			stim::vec3<float> tmp_d;
  			if (r1 == r2) {						// two vertices have the same radius
  				tmp_d = center2 - center1;		// calculate the direction vector
  				tmp_d = tmp_d.norm();
  				stim::circle<float> tmp_c;		// in order to get zero direction vector
  				tmp_c.rotate(tmp_d);
  
  				stim::circle<float> c1(center1, r1, tmp_d, tmp_c.U);
  				stim::circle<float> c2(center2, r2, tmp_d, tmp_c.U);
  				cp1 = c1.glpoints(subdivision);
  				cp2 = c2.glpoints(subdivision);
  			}
  			else {
  				if (r1 < r2) {					// switch index, we always want r1 to be larger than r2
  					stim::vec3<float> tmp_c = center2;
  					center2 = center1;
  					center1 = tmp_c;
  					float tmp_r = r2;
  					r2 = r1;
  					r1 = tmp_r;
  				}
  				tmp_d = center2 - center1;		// bigger one points to smaller one
  				tmp_d = tmp_d.norm();
  
  				float D = (center1 - center2).len();
  				stim::vec3<float> exp;
  				exp[0] = (center2[0] * r1 - center1[0] * r2) / (r1 - r2);
  				exp[1] = (center2[1] * r1 - center1[1] * r2) / (r1 - r2);
  
  				stim::vec3<float> t1, t2, t3, t4;
  				t1[2] = t2[2] = center1[2];		// decide the specific plane to work on
  				t3[2] = t4[2] = center2[2];
  
  				// first two
  				t1[0] = pow(r1, 2)*(exp[0] - center1[0]);
  				t1[0] += r1*(exp[1] - center1[1])*sqrt(pow((exp[0] - center1[0]), 2) + pow((exp[1] - center1[1]), 2) - pow(r1, 2));
  				t1[0] /= (pow((exp[0] - center1[0]), 2) + pow((exp[1] - center1[1]), 2));
  				t1[0] += center1[0];
  
  				t2[0] = pow(r1, 2)*(exp[0] - center1[0]);
  				t2[0] -= r1*(exp[1] - center1[1])*sqrt(pow((exp[0] - center1[0]), 2) + pow((exp[1] - center1[1]), 2) - pow(r1, 2));
  				t2[0] /= (pow((exp[0] - center1[0]), 2) + pow((exp[1] - center1[1]), 2));
  				t2[0] += center1[0];
  
  				t1[1] = pow(r1, 2)*(exp[1] - center1[1]);
  				t1[1] -= r1*(exp[0] - center1[0])*sqrt(pow((exp[0] - center1[0]), 2) + pow((exp[1] - center1[1]), 2) - pow(r1, 2));
  				t1[1] /= (pow((exp[0] - center1[0]), 2) + pow((exp[1] - center1[1]), 2));
  				t1[1] += center1[1];
  
  				t2[1] = pow(r1, 2)*(exp[1] - center1[1]);
  				t2[1] += r1*(exp[0] - center1[0])*sqrt(pow((exp[0] - center1[0]), 2) + pow((exp[1] - center1[1]), 2) - pow(r1, 2));
  				t2[1] /= (pow((exp[0] - center1[0]), 2) + pow((exp[1] - center1[1]), 2));
  				t2[1] += center1[1];
  
  				// check the correctness of the points
  				//float s = (center1[1] - t1[1])*(exp[1] - t1[1]) / ((t1[0] - center1[0])*(t1[0] - exp[0]));
  				//if (s != 1) {			// swap t1[1] and t2[1]
  				//	float tmp_t = t2[1];
  				//	t2[1] = t1[1];
  				//	t1[1] = tmp_t;
  				//}
  
  				// second two
  				t3[0] = pow(r2, 2)*(exp[0] - center2[0]);
  				t3[0] += r2*(exp[1] - center2[1])*sqrt(pow((exp[0] - center2[0]), 2) + pow((exp[1] - center2[1]), 2) - pow(r2, 2));
  				t3[0] /= (pow((exp[0] - center2[0]), 2) + pow((exp[1] - center2[1]), 2));
  				t3[0] += center2[0];
  
  				t4[0] = pow(r2, 2)*(exp[0] - center2[0]);
  				t4[0] -= r2*(exp[1] - center2[1])*sqrt(pow((exp[0] - center2[0]), 2) + pow((exp[1] - center2[1]), 2) - pow(r2, 2));
  				t4[0] /= (pow((exp[0] - center2[0]), 2) + pow((exp[1] - center2[1]), 2));
  				t4[0] += center2[0];
  
  				t3[1] = pow(r2, 2)*(exp[1] - center2[1]);
  				t3[1] -= r2*(exp[0] - center2[0])*sqrt(pow((exp[0] - center2[0]), 2) + pow((exp[1] - center2[1]), 2) - pow(r2, 2));
  				t3[1] /= (pow((exp[0] - center2[0]), 2) + pow((exp[1] - center2[1]), 2));
  				t3[1] += center2[1];
  
  				t4[1] = pow(r2, 2)*(exp[1] - center2[1]);
  				t4[1] += r2*(exp[0] - center2[0])*sqrt(pow((exp[0] - center2[0]), 2) + pow((exp[1] - center2[1]), 2) - pow(r2, 2));
  				t4[1] /= (pow((exp[0] - center2[0]), 2) + pow((exp[1] - center2[1]), 2));
  				t4[1] += center2[1];
  
  				// check the correctness of the points
  				//s = (center2[1] - t3[1])*(exp[1] - t3[1]) / ((t3[0] - center2[0])*(t3[0] - exp[0]));
  				//if (s != 1) {			// swap t1[1] and t2[1]
  				//	float tmp_t = t4[1];
  				//	t4[1] = t3[1];
  				//	t3[1] = tmp_t;
  				//}
  
  				stim::vec3<float> d1;
  				float dot;
  				float a;
  				float new_r;
  				stim::vec3<float> new_u;
  				stim::vec3<float> new_c;
  
  				// calculate the bigger circle
  				d1 = t1 - center1;
  				dot = d1.dot(tmp_d);